fast_search of closest waypoints