给定的3D旋转矩阵和3D方向矢量发现角差

Given 3D rotation matrix and 3D direction vector finding angular difference

本文关键字：3D 发现 方向 旋转      更新时间：2023-10-16

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

假定要做的旧代码是错误的，并且不起作用。即使是越野车，我也想分享它，因为它可能有助于理解我的问题。

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

上面的方法中缺少的内容是简单的内容，还是我应该完全改变方法？给定方向向量，从旋转矩阵中找到X和Y方向的角度距离的最佳方法是什么？

在局部坐标系统中制作向量（就像我认为in3DDirection已经在）中，并应用逆球坐标转换。请参阅链接中的等式（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
相关文章：