New test implementation of waypoint calculation
Merge remote-tracking branch 'origin/master'