3
\$\begingroup\$

I would like to calculate angle between two 2D vectors. Here's a picture of what I'm trying to achieve:

Angle between two vectors

I have an obstacle line segment AB and incoming moving PC. If I'm coming from one side C¹D¹ and angle ⍺ < π/2 (i.e. right side) then PC will turn left. On the other hand if ⍺ > π/2 then PC will turn right. But if PC will come from the other side C²D² it should work as well for angle β as per picture.

So what I need is to determine whether the angle between AB and CD is larger to the left or right basically.

I've tried these things, while having vectors defined as:

Vector2 ab = Vector2.Normalize(new Vector2(b.x - a.x, b.y - a.y));
Vector2 cd = Vector2.Normalize(direction);

1) To calculate angle using atan2, but it works only randomly (1/4 of cases).

Single angle = Math.Atan2(cd.Y, cd.X) - Math.Atan2(ab.Y, ab.X);

if (angle > MathUtil.PiOverTwo)
{
    RotateRight();
}
else if (angle < MathUtil.PiOverTwo)
{
    RotateLeft();
}

2) To use normals somehow. This works a bit better but only from one side I think. (1/2 of cases, better but not there)

Vector2 normal1 = Vector2.Normalize(new Vector2(-ab.Y, ab.X));
Vector2 normal2 = Vector2.Normalize(new Vector2(ab.Y, -ab.X));
Double angle1 = Math.Atan2(normal1.Y, normal1.X) - Math.Atan2(cd.Y, cd.X);
Double angle2 = Math.Atan2(normal2.Y, normal2.X) - Math.Atan2(cd.Y, cd.X);

if (angle1 > angle2)
{
    RotateRight();
}
else if (angle1 < angle2)
{
    RotateLeft();
}
\$\endgroup\$
1

2 Answers 2

5
\$\begingroup\$

The sign of the dot-product of C with AB will be positive when the vector component of CD parallel to vector AB is in the direction AB, and negative when it is in the direction BA.

The sign of the (z-component of the) cross-product of vector CD with vector AB will indicate which side of AB the agent is approaching from.

Depending on your sign conventions, multiplying these two sign results will either give you -1 => turn left, +1 => turn right; or the opposite.

Update:

Formula for z-component of the cross product of two vectors U and V lying in the XY plane:

z(U * V) = Ux . Vy - Uy . Vx

Formula for dot-product of two vectors U and V lying in the XY plane:

U . V = Ux . Vx + Uy . Vy
\$\endgroup\$
5
  • 3
    \$\begingroup\$ Look Ma! No ATan2 calls. \$\endgroup\$ Commented Oct 14, 2014 at 3:57
  • \$\begingroup\$ So it's only a cross product? Wow. I'll try that later and if true will accept. I was so focused it must be angle I didn't consider easier solutions. \$\endgroup\$
    – SmartK8
    Commented Oct 14, 2014 at 6:15
  • \$\begingroup\$ I did a Vector3.Cross(AB, CD) and rotated left or right depending on Z < 0 or Z > 0 but it doesn't work. If I approach it from one side it rotates always left and it rotates always right from the other. I'm puzzled. \$\endgroup\$
    – SmartK8
    Commented Oct 14, 2014 at 13:39
  • \$\begingroup\$ @SmartK8: Multiply the sign of the cross-product z-projection with the sign of the dot-product, which indicates the trend (towards A or towards B), and use that. \$\endgroup\$ Commented Oct 14, 2014 at 21:32
  • \$\begingroup\$ OK, I'll try that. Tomorrow.. once the weed will subside. \$\endgroup\$
    – SmartK8
    Commented Oct 14, 2014 at 21:37
2
\$\begingroup\$

If I understand what you're asking, the vector CD is just a vector, not a ray, so only the direction matters, not location. However, AB is a line segment, not just a vector, so its location matters.

Your tests have one 'if' test to make two cases, but I think you actually have four cases. Let's look at the diagram in AB's reference frame:

Vector and line

If you can calculate the angle between normal1 and vector CD, then from 0-π/2 it's “turn right”, π/2-π it's “turn left”, π-3π/2 it's “turn right”, and 3π/2-2π it's “turn left”.

\$\endgroup\$
2
  • 1
    \$\begingroup\$ Isn't the product: sign(AB . CD) * sign(AB x CD) sufficient to determine the desired turn? \$\endgroup\$ Commented Oct 14, 2014 at 0:14
  • \$\begingroup\$ The problem is that there are two normals (see my code 2). I don't know which side I'm approaching from. But maybe my code is just wrong. But I'll try cross product solution (duh!) of Pieter first. \$\endgroup\$
    – SmartK8
    Commented Oct 14, 2014 at 6:18

Not the answer you're looking for? Browse other questions tagged .