I have a three lat-lon coordinates that make up two line segment A to B to C. I also found a function that can return north-bearing of a line segment A-B or B-C in -180 to 180 manner. However, I'm having trouble to determine when a car reaches from A to B, should it turn right or left to continue to C.
+3
A:
Edited to fix the over 180 issue, also now supports U-Turns.
const int THRESHOLD = 0;
Direction TurnLeftOrRight(Point A, Point B, Point C)
{
int angle = ToAngle(B,C) - ToAngle(A,B);
if((angle > THRESHOLD && angle < 180 - THREASHOLD) || angle < -180 - THREASHOLD)
return Direction.Right;
else if ((angle < 0 - THREASHOLD && angle > -180 + THREASHOLD) || angle > 180 + THREASHOLD)
return Direction.Left;
else if (angle >= 0 - THREASHOLD && angle <= THREASHOLD)
return Direction.Straight
else
return Direction.UTurn;
}
You could also do tolerances between left right and strait just change the first angle > 0
to angle > 45
and the second one to angle < -45
Scott Chamberlain
2010-08-05 21:41:38
A = 16.83432,96.17936B = 16.83378,96.1781C = 16.83329,96.17831A-B = -114B-C = 157(B-C)-(A-B) = 271so, according to you, it'll be turn Right. But I can visually confirm that it is turning left. Which is exactly why I have to ask the question. Thanks.
VOX
2010-08-05 22:15:44
271 is more than 180, so it's turning so far to the right that it has come around the rear and points leftish now.
Karl
2010-08-05 22:54:20
+8
A:
EDIT: Previous answer was wrong. now this is the correct
public Direction GetDirection(Point a, Point b, Point c)
{
double theta1 = GetAngle(a, b);
double theta2 = GetAngle(b, c);
double delta = NormalizeAngle(theta2 - theta1);
if ( delta == 0 )
return Direction.Straight;
else if ( delta == Math.PI )
return Direction.Backwards;
else if ( delta < Math.PI )
return Direction.Left;
else return Direction.Right;
}
private Double GetAngle(Point p1, Point p2)
{
Double angleFromXAxis = Math.Atan ((p2.Y - p1.Y ) / (p2.X - p1.X ) ); // where y = m * x + K
return p2.X - p1.X < 0 ? m + Math.PI : m ); // The will go to the correct Quadrant
}
private Double NormalizeAngle(Double angle)
{
return angle < 0 ? angle + 2 * Math.PI : angle; //This will make sure angle is [0..2PI]
}
Carlos Muñoz
2010-08-05 21:44:05
A:
If AB is the bearing of B from A, and BC that of C from B, the turn angle is remainder( BC-AB, 360.0); (assuming degrees). If this is positive the turn is to the right. In your example remainder( BC-AB, 360.0) is remainder(271,360) = -89.
dmuir
2010-08-06 08:58:34
The remainder function is standard in POSIX math libraries. From 'man remainder' : These functions shall return the floating-point remainder r= x- ny when y is non-zero. The value n is the integral value nearest the exact value x/ y. When |n-x/y|=0.5, the value n is chosen to be even.For example remainder(10,360) will be 10, while remainder(350,360) will be -10. As a general rule I've found it best always to use remainder when subtracting angles.
dmuir
2010-08-06 16:22:47