c++ - 给定 3D 旋转矩阵和 3D 方向 vector 求角度差

Question

在我当前的项目中，我需要从 3D 旋转矩阵中找到方向 vector 的角距离。

假设执行此操作的遗留代码存在错误并且无法正常工作。即使它是 buggy ，我也想分享它，因为它可能有助于理解我的问题。

double angularDiff( bool isSideAngle, const Matrix& in3DRotationMatrix, const Vector& in3DDirection )
{
    // Unit vector convertion
    in3DDirection.Normalize();

    // Simple matrix vector multiplication 
    Vector newDir = in3DRotationMatrix*in3DDirection;

    // Code suppose to find side angle difference 
    // or forward angle difference according to boolean parameter
    if ( isSideAngle )
        result = atan2(newDir[1],  newDir[0]);
    else 
        result = atan2(newDir[2],  newDir[0]);

    return result;    
}

上述方法中缺少的是简单的东西还是我应该完全改变我的方法？给定一个方向 vector ，从旋转矩阵找到 X 和 Y 方向的角距离的最佳方法是什么？

Answer 1

在局部坐标系中创建 vector (就像我认为 in3DDirection 已经在)并应用逆向 spherical coordinate转型。请参阅链接中的等式 (1) 到 (3)。

double x = in3DDirection[0], y = in3DDirection[1], z=in3DDirection[2];
double r = sqrt(x*x+y*y+z*z);
theta = atan2(y,x);  // range is -pi..pi
phi = acos(z/r);     // range is -pi/2..pi/2