Lazy theta*: any-angle path planning and path length analysis in 3D
A Formal BasisfortheHeuristic Determination of Minimum CostPaths