/// <summary> /// /// </summary> /// <param name="euler"></param> /// <returns></returns> public static Matrix4 MakeRotationFromEuler(Euler euler) { var m = new Matrix4(); var x = euler.X; var y = euler.Y; var z = euler.Z; var a = (float)Math.Cos(x); var b = (float)Math.Sin(x); var c = (float)Math.Cos(y); var d = (float)Math.Sin(y); var e = (float)Math.Cos(z); var f = (float)Math.Sin(z); if (euler.Order == Euler.RotationOrders.XYZ) { var ae = a * e; var af = a * f; var be = b * e; var bf = b * f; m.M11 = c * e; m.M21 = -c * f; m.M31 = d; m.M12 = af + be * d; m.M22 = ae - bf * d; m.M32 = -b * c; m.M13 = bf - ae * d; m.M23 = be + af * d; m.M33 = a * c; } else if (euler.Order == Euler.RotationOrders.YXZ) { throw new NotImplementedException(); } else if (euler.Order == Euler.RotationOrders.ZXY) { throw new NotImplementedException(); } else if (euler.Order == Euler.RotationOrders.ZYX) { throw new NotImplementedException(); } else if (euler.Order == Euler.RotationOrders.YZX) { throw new NotImplementedException(); } else if (euler.Order == Euler.RotationOrders.XZY) { throw new NotImplementedException(); } // last column m.M14 = 0; m.M24 = 0; m.M34 = 0; // bottom row m.M41 = 0; m.M42 = 0; m.M43 = 0; m.M44 = 1; return(m); }
/// <summary> /// /// </summary> /// <param name="euler"></param> /// <returns></returns> public Quaternion SetFromEuler(Euler euler) { var q = this; var c1 = (float)System.Math.Cos(euler.X / 2); var c2 = (float)System.Math.Cos(euler.Y / 2); var c3 = (float)System.Math.Cos(euler.Z / 2); var s1 = (float)System.Math.Sin(euler.X / 2); var s2 = (float)System.Math.Sin(euler.Y / 2); var s3 = (float)System.Math.Sin(euler.Z / 2); if (euler.Order == Euler.RotationOrder.XYZ) { q.x = s1 * c2 * c3 + c1 * s2 * s3; q.y = c1 * s2 * c3 - s1 * c2 * s3; q.z = c1 * c2 * s3 + s1 * s2 * c3; q.w = c1 * c2 * c3 - s1 * s2 * s3; } else if (euler.Order == Euler.RotationOrder.YXZ) { q.x = s1 * c2 * c3 + c1 * s2 * s3; q.y = c1 * s2 * c3 - s1 * c2 * s3; q.z = c1 * c2 * s3 - s1 * s2 * c3; q.w = c1 * c2 * c3 + s1 * s2 * s3; } else if (euler.Order == Euler.RotationOrder.ZXY) { q.x = s1 * c2 * c3 - c1 * s2 * s3; q.y = c1 * s2 * c3 + s1 * c2 * s3; q.z = c1 * c2 * s3 + s1 * s2 * c3; q.w = c1 * c2 * c3 - s1 * s2 * s3; } else if (euler.Order == Euler.RotationOrder.ZYX) { q.x = s1 * c2 * c3 - c1 * s2 * s3; q.y = c1 * s2 * c3 + s1 * c2 * s3; q.z = c1 * c2 * s3 - s1 * s2 * c3; q.w = c1 * c2 * c3 + s1 * s2 * s3; } else if (euler.Order == Euler.RotationOrder.YZX) { q.x = s1 * c2 * c3 + c1 * s2 * s3; q.y = c1 * s2 * c3 + s1 * c2 * s3; q.z = c1 * c2 * s3 - s1 * s2 * c3; q.w = c1 * c2 * c3 - s1 * s2 * s3; } else if (euler.Order == Euler.RotationOrder.XZY) { q.x = s1 * c2 * c3 - c1 * s2 * s3; q.y = c1 * s2 * c3 - s1 * c2 * s3; q.z = c1 * c2 * s3 + s1 * s2 * c3; q.w = c1 * c2 * c3 + s1 * s2 * s3; } this.OnPropertyChanged(); return(q); }
/// <summary> /// /// </summary> /// <param name="q"></param> /// <param name="order"></param> /// <param name="update"></param> /// <returns></returns> public static Euler SetFromQuaternion(Quaternion q, Euler.RotationOrders order = Euler.RotationOrders.XYZ, bool update = false) { var euler = new Euler(); // q is assumed to be normalized // http://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/content/SpinCalc.m var sqx = q.X * q.X; var sqy = q.Y * q.Y; var sqz = q.Z * q.Z; var sqw = q.W * q.W; if (order == Euler.RotationOrders.XYZ) { euler.X = (float)Math.Atan2(2 * (q.X * q.W - q.Y * q.Z), (sqw - sqx - sqy + sqz)); euler.Y = (float)Math.Asin((2 * (q.X * q.Z + q.Y * q.W).Clamp(-1, 1))); euler.Z = (float)Math.Atan2(2 * (q.Z * q.W - q.X * q.Y), (sqw + sqx - sqy - sqz)); } else if (order == Euler.RotationOrders.YXZ) { euler.X = (float)Math.Asin((2 * (q.X * q.W - q.Y * q.Z).Clamp(-1, 1))); euler.Y = (float)Math.Atan2(2 * (q.X * q.Z + q.Y * q.W), (sqw - sqx - sqy + sqz)); euler.Z = (float)Math.Atan2(2 * (q.X * q.Y + q.Z * q.W), (sqw - sqx + sqy - sqz)); } else if (order == Euler.RotationOrders.ZXY) { euler.X = (float)Math.Asin((2 * (q.X * q.W + q.Y * q.Z).Clamp(-1, 1))); euler.Y = (float)Math.Atan2(2 * (q.Y * q.W - q.Z * q.X), (sqw - sqx - sqy + sqz)); euler.Z = (float)Math.Atan2(2 * (q.Z * q.W - q.X * q.Y), (sqw - sqx + sqy - sqz)); } else if (order == Euler.RotationOrders.ZYX) { euler.X = (float)Math.Atan2(2 * (q.X * q.W + q.Z * q.Y), (sqw - sqx - sqy + sqz)); euler.Y = (float)Math.Asin((2 * (q.Y * q.W - q.X * q.Z).Clamp(-1, 1))); euler.Z = (float)Math.Atan2(2 * (q.X * q.Y + q.Z * q.W), (sqw + sqx - sqy - sqz)); } else if (order == Euler.RotationOrders.YZX) { euler.X = (float)Math.Atan2(2 * (q.X * q.W - q.Z * q.Y), (sqw - sqx + sqy - sqz)); euler.Y = (float)Math.Atan2(2 * (q.Y * q.W - q.X * q.Z), (sqw + sqx - sqy - sqz)); euler.Z = (float)Math.Asin((2 * (q.X * q.Y + q.Z * q.W).Clamp(-1, 1))); } else if (order == Euler.RotationOrders.XZY) { euler.X = (float)Math.Atan2(2 * (q.X * q.W + q.Y * q.Z), (sqw - sqx + sqy - sqz)); euler.Y = (float)Math.Atan2(2 * (q.X * q.Z + q.Y * q.W), (sqw + sqx - sqy - sqz)); euler.Z = (float)Math.Asin((2 * (q.Z * q.W - q.X * q.Y).Clamp(-1, 1))); } euler.Order = order; // if ( update !== false ) this.onChangeCallback(); return(euler); }
public Vector4 ApplyEuler(Euler Euler) { this.ApplyQuaternion(new Quaternion().SetFromEuler(Euler)); return(this); }
/// <summary> /// /// </summary> /// <returns></returns> public Matrix4 MakeRotationFromEuler(Euler euler) { var x = euler.X; var y = euler.Y; var z = euler.Z; var a = (float)Math.Cos(x); var b = (float)Math.Sin(x); var c = (float)Math.Cos(y); var d = (float)Math.Sin(y); var e = (float)Math.Cos(z); var f = (float)Math.Sin(z); if (euler.Order == Euler.RotationOrder.XYZ) { var ae = a * e; var af = a * f; var be = b * e; var bf = b * f; this.Elements[0] = c * e; this.Elements[4] = -c * f; this.Elements[8] = d; this.Elements[1] = af + be * d; this.Elements[5] = ae - bf * d; this.Elements[9] = -b * c; this.Elements[2] = bf - ae * d; this.Elements[6] = be + af * d; this.Elements[10] = a * c; } else if (euler.Order == Euler.RotationOrder.YXZ) { var ce = c * e; var cf = c * f; var de = d * e; var df = d * f; this.Elements[0] = ce + df * b; this.Elements[4] = de * b - cf; this.Elements[8] = a * d; this.Elements[1] = a * f; this.Elements[5] = a * e; this.Elements[9] = -b; this.Elements[2] = cf * b - de; this.Elements[6] = df + ce * b; this.Elements[10] = a * c; } else if (euler.Order == Euler.RotationOrder.ZXY) { var ce = c * e; var cf = c * f; var de = d * e; var df = d * f; this.Elements[0] = ce - df * b; this.Elements[4] = -a * f; this.Elements[8] = de + cf * b; this.Elements[1] = cf + de * b; this.Elements[5] = a * e; this.Elements[9] = df - ce * b; this.Elements[2] = -a * d; this.Elements[6] = b; this.Elements[10] = a * c; } else if (euler.Order == Euler.RotationOrder.ZYX) { var ae = a * e; var af = a * f; var be = b * e; var bf = b * f; this.Elements[0] = c * e; this.Elements[4] = be * d - af; this.Elements[8] = ae * d + bf; this.Elements[1] = c * f; this.Elements[5] = bf * d + ae; this.Elements[9] = af * d - be; this.Elements[2] = -d; this.Elements[6] = b * c; this.Elements[10] = a * c; } else if (euler.Order == Euler.RotationOrder.YZX) { var ac = a * c; var ad = a * d; var bc = b * c; var bd = b * d; this.Elements[0] = c * e; this.Elements[4] = bd - ac * f; this.Elements[8] = bc * f + ad; this.Elements[1] = f; this.Elements[5] = a * e; this.Elements[9] = -b * e; this.Elements[2] = -d * e; this.Elements[6] = ad * f + bc; this.Elements[10] = ac - bd * f; } else if (euler.Order == Euler.RotationOrder.XZY) { var ac = a * c; var ad = a * d; var bc = b * c; var bd = b * d; this.Elements[0] = c * e; this.Elements[4] = -f; this.Elements[8] = d * e; this.Elements[1] = ac * f + bd; this.Elements[5] = a * e; this.Elements[9] = ad * f - bc; this.Elements[2] = bc * f - ad; this.Elements[6] = b * e; this.Elements[10] = bd * f + ac; } // last column this.Elements[3] = 0; this.Elements[7] = 0; this.Elements[11] = 0; // bottom row this.Elements[12] = 0; this.Elements[13] = 0; this.Elements[14] = 0; this.Elements[15] = 1; return(this); }