Added more waypoint code from Minishark branch.