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