Determining yaw, pitch, and roll from a rotation matrix

1.3k views Asked by At

I am having trouble finding the right yaw, pitch, and roll. I have a 3 boxes place in a random plane.

Image with the boxes

I want to find the yaw, pitch, and roll for each box. The formula that I am using is:

string _outOrientation = string.Empty;
Point3d _worldPointA = new Point3d(0.0, 0.0, 0.0);
Point3d _worldPointB = new Point3d(0.0, 0.0, 1.0);
Vector3d _worldZvector = _worldPointB - _worldPointA;

Plane _scenePlane = new Plane(_worldPointA, _worldZvector);
Plane _orientationPlane = _planeBase;

//here is a 4x4 tranformation matrix
Orientation _orientation = new Orientation(_scenePlane, _orientationPlane);

Transform _transform = _orientation.ToMatrix();
//ROLL 
double Aradian = Math.Atan2(_transform.M21, _transform.M22);
//PITCH 
double Bradian = -Math.Atan2(_transform.M20, Math.Sqrt(_transform.M21 * _transform.M21 + _transform.M22 * _transform.M22));
//YAW 
double Cradian = Math.Atan2(_transform.M10, _transform.M00);

//ROLL to degree
 double Adegree = RadianToDegree(Aradian);                                                                         
//PITCH to degree
 double Bdegree = RadianToDegree(Bradian);
//YAW to degree
double Cdegree = RadianToDegree(Cradian);

_outOrientation = string.Format("{0} {1} {2}", Adegree.ToString(), Bdegree.ToString(), Cdegree.ToString());

My result is this this.

0

There are 0 answers