3D A* Search appears to function correctly