[Pierre-Marie Baty]

Well, it's exactly like what Shrike says, except that:

1- you don't check the destination waypoint when the player has moved but when he pressed a key in the menu

2- you don't check the destination waypoint by cycling through them and selecting the nearest one, but by cycling through them and selecting the one for which the angles to the player are the closest to the player's view angles.