计算在不寻常的二维空间中与点的角度的函数。

Question

我正在寻找一个强大的函数来计算一个物体和一个点之间的差值(delta)。

例如，在A点有一个物体，方向是1.2拉德，那么物体要转过去面对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;
}