计算与异常二维空间中点的角度的函数

Question

我正在寻找一个强大的函数来计算一个对象和一个点之间的差异（delta）。

例如，在 A 点有一个方向为 1.2 Rad 的物体，该物体需要转动多少角度才能面向 B 点。

此外，我正在一个奇怪的坐标系中工作，其中北（0 Rad）朝向 +X，下图显示了这一点。

我了解基本原理，但我正在努力制作强大的东西。

我的c++函数模板是这样的，

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

    //TODO:

    return xxxxxxx;
}

任何帮助将不胜感激。

提前干杯。

Answer 1

这就是答案。

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;
}