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

Question

In my current project I need to find the angular distance of direction vector from a 3D rotation matrix.

The legacy code which suppose to do that is buggy and is not working. Even it is buggy I want to share it because it may help understanding my problem.

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

Is what is missing in the method above something simple or should I change my approach totally? Given a direction vector what is the best method to find angular distance in X and Y directions from a rotation matrix?

There are two angles that describe the direction of the vector local to the rotation matrix (coordinate system). Which angle do you want? Alternatively, the rotation matrix defines three orthogonal direction vectors and you can find the angle of your vector to each one of them by calculating the angle between two vectors. — John Alexiou
@ja72 I want both of them. Like the code above the new function a create will have a parameter bool isSideAngle. According to this boolean I will calculate side angle or forward angle. — Kadir Erdem Demir
Is in3DDirection in world coordinates or in local coordinates? — John Alexiou

John Alexiou John Alexiou · Accepted Answer · 2016-12-18T16:51:21

Make the vector in the local coordinate system (like I think in3DDirection is in already) and apply the inverse spherical coordinate transformation. See equations (1) through (3) in the link.

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

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

1 Answers