Найти два вектора и угол между ними - PullRequest
Новая
       11

Найти два вектора и угол между ними

1 голос
/ 30 ноября 2019

У меня есть транспортное средство и точка, представляющая пешехода, мне удалось найти оба вектора a и b (см. Рисунок ниже), а затем я попытался вычислить угол между ними

Я использую известную формулу, так как(Угол) = величина (вектор_а) * величина (вектор_а)) / dot_product (вектор_а, вектор_b), но когда я вычисляю косинус, он намного больше единицы, поэтому невозможно вычислить угол ... Что я делаю не так? 

    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)

enter image description here

Большое спасибо

1 Ответ

0 голосов
/ 05 декабря 2019

Вам понадобится курс автомобиля и положение пешехода относительно автомобиля. Если у вас есть положение пешехода и автомобиля в мировых координатах (т.е. абсолютное положение) и скорость автомобиля, код прост: 

import numpy as np

def detect_pedestrian(car_vector, pedestrian_vector, car_velocity):
    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)
    return(np.arccos(cosine))

car = np.array([5, 2])
pedestrian = np.array([2, 3])
v = np.array([0, 2])

angle_rad = detect_pedestrian(car, pedestrian, v)
print(f"Pedestrian angle: {np.degrees(angle_rad)}°")
# Pedestrian angle: 71.56505117707799°
...