I would suggesst to use A* algorithm.
In the net you can find examples for a 2D grid, but it really doesn't matter
what kind of grid or way point system you use.
http://www.policyalmanac.org/games/aStarTutorial.htm
Try this article, I think it quite clearly explains the basics of
path finding on 2D grid. Once you understand how it works
you can start to implement path finding for waypoint system.