I have this function to convert Euler Angles to a rotation matrix but I would like to reverse it and get the Euler Angles when I only have the rotation matrix. The reason for it is I want to be able to set an objects transform using a transform matrix then I would like to update that objects Euler rotation variable that is in degrees. (I have a function that calculate the rotation matrix from the transform matrix)
public static Mat4 RotMat(Vec3 _rot)
{
double rx = Math2.degToRad(_rot.x);
double[,] trmx = new double[4, 4] { { 1, 0, 0, 0 }, { 0, Math.Cos(rx), Math.Sin(rx), 0 }, { 0, -Math.Sin(rx), Math.Cos(rx), 0 }, { 0, 0, 0, 1 } };
Mat4 rmx = new Mat4(trmx);
double ry = Math2.degToRad(_rot.y);
double[,] trmy = new double[4, 4] { { Math.Cos(ry), 0, -Math.Sin(ry), 0 }, { 0, 1, 0, 0 }, { Math.Sin(ry), 0, Math.Cos(ry), 0 }, { 0, 0, 0, 1 } };
Mat4 rmy = new Mat4(trmy);
double rz = Math2.degToRad(_rot.z);
double[,] trmz = new double[4, 4] { { Math.Cos(rz), Math.Sin(rz), 0, 0 }, { -Math.Sin(rz), Math.Cos(rz), 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } };
Mat4 rmz = new Mat4(trmz);
return (rmx * rmy * rmz);
}
This is an attempt based on some code I found but does not always give me the expected result, it works sometimes (I know there is the gimbal lock issue with euler angles so maybe this is a futile attempt ).
public static Vec3 GetRot(Mat4 _tm)
{
Mat4 rm = GetRotMat(_tm);
double sy = Math.Sqrt(rm[0, 0] * rm[0, 0] + rm[1, 0] * rm[1, 0]);
bool singular = sy < 1e-6; // If
double x, y, z;
if (!singular)
{
x = Math.Atan2(rm[2, 1], rm[2, 2]);
y = Math.Atan2(-rm[2, 0], sy);
z = Math.Atan2(rm[1, 0], rm[0, 0]);
}
else
{
x = Math.Atan2(-rm[1, 2], rm[1, 1]);
y = Math.Atan2(-rm[2, 0], sy);
z = 0;
}
x = Math2.radToDeg(x);
y = Math2.radToDeg(y);
z = Math2.radToDeg(z);
return new Vec3(x, y, z) * -1;
}