Udacity Flying Car Event Driven Programming Waypoints following by a quadrotor, src. Motion Planning 2D and 3D paths construction with A* and PRM. Have a look at the Unity simulation example.