Implemented 3D A* algorithm. Works on a cube. More testing must be done to make sure though.