Search code examples
geometrymappingdirectionscross-product

Logic for "slight right turn" given three points


Given three co-planar (2D) points (X1, Y1), (X2, Y2), and (X3, Y3), which represent (respectively ...) "1=where I was, 2=where I am, and 3=where I am going," I need a simple algorithm that will tell me, e.g.

  • Veer to the right
  • Make a slight left turn
  • Turn to the left

In other words, (a) is the turn to the left or to the right; and (b) how sharp is the turn (letting me be arbitrary about this).

For the first part, I've already learned about how to use (see wikipedia: Graham Scan, and question #26315401 here) cross-product to determine whether the turn is to the left or to the right, based on whether the path is counterclockwise.

And, I'm sure that ATAN2() will be at the core of determining how sharp the turn is.

But I can't .. quite .. wrap my head around the proper math which will work in all orientations. (Especially when the angle crosses the zero-line. (A bearing of 350 degrees to 10 degrees is a gap of 20 degrees, not 340, etc.)

Okay, I'm tired. [... of bangin' my head against the wall this mornin'.] "Every time I think I've got it, I'm not sure." So, okay, it's time to ask ... :-)


Solution

  • When you calculate direction change angles with Atan2, don't bother about absolute angles. You have not to calculate two bearings and subtract them - Atan2 can give you relative angle between the first and the second vectors in range -Pi..Pi (-180..180) (range might depend on programming language).

    x12 = x2-x1
    y12 = y2-y1
    x23 = x3-x2
    y23 = y3-y2
    DirChange = Atan2(x12*y23-x23*y12, x12*x23+y12*y23)
    

    Some explanations: we can calculate sine of vector-vector angle through cross product and vector norms (|A| = Sqrt(A.x*A.x + A.y*A.y)):

    Sin(A_B) = (A x B) / (|A|*|B|)
    

    and cosine of vector-vector angle through dot (scalar) product and vector norms:

    Cos(A_B) = (A * B) / (|A|*|B|)
    

    Imagine that Atan2 calculates angle with sine and cosine of this angle, excluding common denominator (product of norms)

    A_B = Atan2(Sin(A_B), Cos(A_B))
    

    Example in Delphi:

    var
      P1, P2, P3: TPoint;
      x12, y12, x23, y23: Integer;
      DirChange: Double;
    begin
      P1 := Point(0, 0);
      P2 := Point(1, 0);
      P3 := Point(2, 1);
      x12 := P2.X - P1.X;
      y12 := P2.Y - P1.Y;
      x23 := P3.X - P2.X;
      y23 := P3.Y - P2.Y;
      DirChange := Math.ArcTan2(x12 * y23 - x23 * y12, x12 * x23 + y12* y23);
      Memo1.Lines.Add(Format('%f radians   %f degrees',
        [DirChange, RadToDeg(DirChange)]));
    

    Output: 0.79 radians 45.00 degrees (left turn)

    for your example data set (1,1), (3,2), and (6,3)

    -0.14 radians -8.13 degrees (right turn)