# Finding an angle between two bodies

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)

$$$$

• You are not normalizing the vectors (dividing them by their length.) – Bram Dec 1 '19 at 20:17
• I tried to normalize it, but it still does not work, do you know where the problem could be? (Sorry for not replying sooner, too manyother projects... :/ – Mechatrnk Dec 5 '19 at 13:58
• @Mechatrnk what does "np.linalg.norm" in your code do? looks like normalizing from the wording, but you use it like it is the vectorlength ..? Does that function actually return the length of the vector? also why magnitude²/dot ? – reiti.net Dec 7 '19 at 15:49
• That function returns vector pointing in the same direction as the original one, but it's norm (length) is one... – Mechatrnk Dec 7 '19 at 15:51

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.

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)

• I tried to normalize it, but it still does not work, do you know where the problem could be? (Sorry for not replying sooner, too manyother projects... :/ – Mechatrnk Dec 5 '19 at 13:58

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))}°")
`
• I accepted my own answer to show that the problem is solved, sorry if there is any rule which forbids accepting own answers. – Mechatrnk Dec 5 '19 at 14:39
• If there were that would forbid you from accepting your own answer, they would be integrated in the system and you would not be able to do it; so it's perfectly fine to accept your own answer. Just consider the other two answers that have been posted; if they have been useful, you should consider up-voting them (but it's in no way mandatory). – Vaillancourt Dec 5 '19 at 15:30