fixed bug in InsertWaypointWithRetiming