Функция для вычисления угла к точке в необычном 2D-пространстве - PullRequest
Новая
       114

Функция для вычисления угла к точке в необычном 2D-пространстве

0 голосов
/ 06 мая 2020

Я ищу надежную функцию для вычисления разницы (дельты) между объектом и точкой.

Например, в точке A был объект с ориентацией 1,2 рад, что будет необходимым углом для поворота объекта, чтобы повернуться лицом к точке B.

Кроме того, я работаю в нечетной системе координат, где север (0 Rad) обращен к + X, изображение ниже показывает это .

enter image description here

Я понимаю основы, но изо всех сил пытаюсь создать что-то надежное.

Мой шаблон функции C ++ выглядит так , 

    float Robot::getDeltaHeading(float _x1, float _y1, float _x2, float _y2, float _currentHeading) {

    //TODO:

    return xxxxxxx;
}

Любая помощь будет принята с благодарностью.

Заранее приветствую.

1 Ответ

0 голосов
/ 07 мая 2020

Вот и ответ. 

float Robot::getDeltaHeading(float _x1, float _y1, float _x2, float _y2, float _currentHeading) {

    _currentHeading -= 90;

    double Ux = 0.0, Uy = 0.0, Vx = 0.0, Vy = 0.0, d = 0.0;

    d = sqrtf(powf(abs(_x1 - _x2), 2) + powf(abs(_y1 - _x2), 2));

    Ux = (_x2 - _x1) / d;
    Uy = (_y2 - _y1) / d;

    Vx = cos(_currentHeading * (3.14159f / 180.0));
    Vy = sin(_currentHeading * (3.14159f / 180.0));

    auto ans = 90 + (atan2(((Ux * Vy) - (Uy * Vx)), ((Ux * Vx) + (Uy * Vy))) * (180.0 / 3.14159f));

    while (ans > 180) ans -= 360;
    while (ans < -180) ans += 360;

    return ans;
}
...