Actually implement waypoint clustering based on distance.