Revision history [back]

Instead of trying to solve this problem in terms of X and Y (Lat and Long) You want to use some more relevant geometry. Referring to your diagram above, if you define an infinite line through the next two green waypoints. Then you can calculate the position of the closest point on that line to your current position. If this position is before the first green waypoint then it hasn't been passed, however if that closest point is beyond the first green waypoint then the point has been passed and you can move to the next one. The maths for this is surprisingly simple using vectors:

Define three vectors:

• P the current location
• W1 waypoint 1
• W2 waypoint 2

Calculate the unit direction (D) between W2 and W1

• D = (W2 - W1) / |W2 - W1|

then distance in front or behind W1 (Dist)

• Dist = (P - W1).dot(D)

This will avoid the orientation problems you described. However this may mean that you could potentially drift a long way from the waypoints if there are a lot of obstacles. Hope this helps though.