Finding an angle between two bodies

Question

I am trying to find angle between vectors a and b. But the cosine for some reason is not within +/- 1, that means that it is not possible to find an angle between a car and a pedestrian...

Any help appreciated

EDIT I have rewritten my function in accordance with the answer below (I had to do some changes due to writing in Python, for example pedestrian.body.position is equivalent to body.GetPosition()

I also tried to normalize both vectors, but the cosine is still not within +/- 1 And the car can move in both direction, so I do not know how to get its other component...

    def detect_pedestrian(car,pedestrian):
        #Find a vector which connects pedestrian with a car
        pedestrian_position = pedestrian.body.position
        car_position = car.body.position

        #Find vectors a and b
        car_vector = Box2D.b2Body.__GetTransform(car.body).R.GetYAxis()
        pedestrian_vector = pedestrian_position - car_position

        #Calculate magnitudes of both vector
        pedestrian_vector_magnitude = np.linalg.norm(pedestrian_vector)
        car_vector_magnitude = np.linalg.norm(car_vector)

        #Normalize both vectors
        pedestrian_vector_normalized = pedestrian_vector/pedestrian_vector_magnitude
        car_vector_normalized = car_vector/car_vector_magnitude

        #Calculate angle
        magnitudes = pedestrian_vector_magnitude * car_vector_magnitude
        dot_product = b2Dot(pedestrian_vector_normalized, car_vector_normalized)
        cosine = magnitudes/dot_product
        arccos = np.arccos(cosine)

        print(cosine)

```

Answer 1

To calculate vector a, you need to use GetPostion() on the bodies, and subtract them, then normalize it.

// Get vector a
b2Vec2 carp = carbody.GetPosition();
b2Vec2 pedp = pedbody.GetPosition();
b2Vec2 a = b2SubV2V2( pedp, carp );
a.Normalize();

// Get vector b
carbody.GetTransform().R().col1(); // assume car travels in +x, use col2() for +y.

// Determine angle between them.
float32 dotp = b2DotV2V2( a, b );
// NOTE: Should clamp dotp to -1..1 because of FP inaccuracy.
dotp = dotp < -1 ? -1 : dotp;
dotp = dotp >  1 ?  1 : dotp;
float32 ang = acosf( dotp );

You probably also want the sign of the angle (is the pedestrian to the right, or to the left of the car?)

You can obtain that using the dot product of a with the sideways-axis of the car's frame, and see if that is positive or negative. If it is to the right, flip the sign of the angle you calculated.

Answer 2

The dot product of two UNIT vectors equals the cosine of the angle between those vectors.

so in order to get what you expect you have to use unit vectors - so be sure to have your two vectors normalized before doing the dot product (looks like you don't do that)

Answer 3

Well, this is my final working solution of a function detect_pedestrian

    car_velocity = car.body.__GetLinearVelocity()
    if car_velocity == (0,0):
        car_velocity = (0,1)
    car_vector = Box2D.b2Body.__GetTransform(car.body).R.GetYAxis()
    pedestrian_vector = pedestrian.body.position - car.body.position

    pedestrian_relative = pedestrian_vector - car_vector
    pedestrian_length = np.linalg.norm(pedestrian_relative)
    velocity_length = np.linalg.norm(car_velocity)
    dot_product = np.dot(pedestrian_relative, car_velocity)
    cosine = dot_product / (pedestrian_length * velocity_length)
    print(f"Pedestrian angle: {np.degrees(np.arccos(cosine))}°")