Completion of 3d pathfinding algorithm