public static EulerRot ToEulerAngles(double x, double y, double z, double w)
{
double[,] matRot = Matrix.MakeRotationFromQuaternion(x, y, z, w);
EulerRot result = new EulerRot();
result.y = Math.Asin(Clamp(matRot[0, 2], -1, 1));
if (Math.Abs(matRot[0, 2]) < 0.99999999)
{
result.x = Math.Atan2(-matRot[1, 2], matRot[2, 2]);
result.z = Math.Atan2(-matRot[0, 1], matRot[0, 0]);
}
else
{
result.x = Math.Atan2(matRot[2, 1], matRot[2, 2]);
result.z = 0;
}
result.x = RadianToDegree(result.x);
result.y = RadianToDegree(result.y);
result.z = RadianToDegree(result.z);
return(result);
}
static public EulerRot GetEulerAnglesFromMatrix(double[,] matrix)
{
double nz1, nz2, nz3;
nz3 = Math.Sqrt(matrix[1, 0] * matrix[1, 0] + matrix[1, 1] * matrix[1, 1]);
nz1 = (-matrix[1, 0] * matrix[1, 2]) / nz3;
nz2 = (-matrix[1, 1] * matrix[1, 2]) / nz3;
double vx = nz1 * matrix[0, 0] + nz2 * matrix[0, 1] + nz3 * matrix[0, 2];
double vz = nz1 * matrix[2, 0] + nz2 * matrix[2, 1] + nz3 * matrix[2, 2];
EulerRot retval = new EulerRot();
retval.x = RadianToDegree(Math.Asin(Clamp(matrix[1, 2], -1, 1)));
retval.y = -RadianToDegree(Math.Atan2(vx, vz));
retval.z = -RadianToDegree(Math.Atan2(matrix[1, 0], matrix[1, 1]));
return(retval);
}
