Lazy theta*: any-angle path planning and path length analysis in 3D
Introduction to AI Robotics