Hello,
I have 4 points (rectangle) at one plane (control points after alignment). Before reconstruction, I want to generate and import *.rcbox file for reconstruction areas inside this rectangle.
For correctly determine the reconstruction region by these points, the <yawPitchRoll> parameter must be set in the file. These 3 angles of rotation are interdependent. In what sequence and in what direction is the rotation of these parameters? Yaw-Pitch-Roll or Roll-Pitch-Yaw or…, positive angle is right hand or left hand?
I have developed a program for determining these angles from the rotation matrix, but only one angle is correctly determined. Please help to change the algorithm.
Program code below (A, B and D - 3 points in rectangle).
Vector3 AD = Vector3.Normalize(A - D);
Vector3 AB = Vector3.Normalize(B - A);
Vector3 Cr = Vector3.Normalize(Vector3.Cross(AD, AB));
float[,] R = new float[3, 3] { { AD.X, AB.X, Cr.X }, { AD.Y, AB.Y, Cr.Y }, { AD.Z, AB.Z, Cr.Z } };
float[] angles = EulerAngles(R);
double angle_yaw = angles[0] * 180 / Math.PI;
double angle_pitch = angles[1] * 180 / Math.PI;
double angle_roll = angles[2] * 180 / Math.PI;
static bool CloseEnough(float a, float b, float epsilon = float.Epsilon)
{
return (epsilon > Math.Abs(a - b));
}
static float[] EulerAngles(float[,] R)
{
//check for gimbal lock
if (CloseEnough(R[0, 2], -1.0f))
{
float x = 0; //gimbal lock, value of x doesn’t matter
float y = (float)Math.PI / 2;
float z = (float)Math.Atan2(R[1, 0], R[2, 0]);
return new float[] { x, y, z };
}
else if (CloseEnough(R[0, 2], 1.0f))
{
float x = 0;
float y = -(float)Math.PI / 2;
float z = (float)Math.Atan2(-R[1, 0], -R[2, 0]);
return new float[] { x, y, z };
}
else
{
float x = -(float)Math.Asin(R[0, 2]);
float y = (float)Math.Atan2(R[1, 2] / Math.Cos(x), R[2, 2] / Math.Cos(x));
float z = (float)Math.Atan2(R[0, 1] / Math.Cos(x), R[0, 0] / Math.Cos(x));
return new float[] { x, y, z };
}
}