#region License /* FNA - XNA4 Reimplementation for Desktop Platforms * Copyright 2009-2016 Ethan Lee and the MonoGame Team * * Released under the Microsoft Public License. * See LICENSE for details. */ /* Derived from code by the Mono.Xna Team (Copyright 2006). * Released under the MIT License. See monoxna.LICENSE for details. */ #endregion #region Using Statements using System; using System.ComponentModel; using System.Diagnostics; using Microsoft.Xna.Framework.Design; #endregion namespace Microsoft.Xna.Framework { /// /// An efficient mathematical representation for three dimensional rotations. /// [Serializable] [TypeConverter(typeof(QuaternionConverter))] [DebuggerDisplay("{DebugDisplayString,nq}")] public struct Quaternion : IEquatable { #region Public Static Properties /// /// Returns a quaternion representing no rotation. /// public static Quaternion Identity { get { return identity; } } #endregion #region Internal Properties internal string DebugDisplayString { get { if (this == Quaternion.Identity) { return "Identity"; } return string.Concat( X.ToString(), " ", Y.ToString(), " ", Z.ToString(), " ", W.ToString() ); } } #endregion #region Public Fields /// /// The x coordinate of this . /// public float X; /// /// The y coordinate of this . /// public float Y; /// /// The z coordinate of this . /// public float Z; /// /// The rotation component of this . /// public float W; #endregion #region Private Static Variables private static readonly Quaternion identity = new Quaternion(0, 0, 0, 1); #endregion #region Public Constructors /// /// Constructs a quaternion with X, Y, Z and W from four values. /// /// The x coordinate in 3d-space. /// The y coordinate in 3d-space. /// The z coordinate in 3d-space. /// The rotation component. public Quaternion(float x, float y, float z, float w) { this.X = x; this.Y = y; this.Z = z; this.W = w; } /// /// Constructs a quaternion with X, Y, Z from and rotation component from a scalar. /// /// The x, y, z coordinates in 3d-space. /// The rotation component. public Quaternion(Vector3 vectorPart, float scalarPart) { this.X = vectorPart.X; this.Y = vectorPart.Y; this.Z = vectorPart.Z; this.W = scalarPart; } #endregion #region Public Methods /// /// Transforms this quaternion into its conjugated version. /// public void Conjugate() { X = -X; Y = -Y; Z = -Z; } /// /// Compares whether current instance is equal to specified . /// /// The to compare. /// true if the instances are equal; false otherwise. public override bool Equals(object obj) { return (obj is Quaternion) && Equals((Quaternion) obj); } /// /// Compares whether current instance is equal to specified . /// /// The to compare. /// true if the instances are equal; false otherwise. public bool Equals(Quaternion other) { return ( (MathHelper.WithinEpsilon(this.X, other.X)) && (MathHelper.WithinEpsilon(this.Y, other.Y)) && (MathHelper.WithinEpsilon(this.Z, other.Z)) && (MathHelper.WithinEpsilon(this.W, other.W)) ); } /// /// Gets the hash code of this . /// /// Hash code of this . public override int GetHashCode() { return ( this.X.GetHashCode() + this.Y.GetHashCode() + this.Z.GetHashCode() + this.W.GetHashCode() ); } /// /// Returns the magnitude of the quaternion components. /// /// The magnitude of the quaternion components. public float Length() { float num = ( (this.X * this.X) + (this.Y * this.Y) + (this.Z * this.Z) + (this.W * this.W) ); return (float) Math.Sqrt((double) num); } /// /// Returns the squared magnitude of the quaternion components. /// /// The squared magnitude of the quaternion components. public float LengthSquared() { return ( (this.X * this.X) + (this.Y * this.Y) + (this.Z * this.Z) + (this.W * this.W) ); } /// /// Scales the quaternion magnitude to unit length. /// public void Normalize() { float num = 1.0f / ((float) Math.Sqrt( (X * X) + (Y * Y) + (Z * Z) + (W * W) )); this.X *= num; this.Y *= num; this.Z *= num; this.W *= num; } /// /// Returns a representation of this in the format: /// {X:[] Y:[] Z:[] W:[]} /// /// A representation of this . public override string ToString() { return ( "{X:" + X.ToString() + " Y:" + Y.ToString() + " Z:" + Z.ToString() + " W:" + W.ToString() + "}" ); } #endregion #region Public Static Methods /// /// Creates a new that contains the sum of two quaternions. /// /// Source . /// Source . /// The result of the quaternion addition. public static Quaternion Add(Quaternion quaternion1, Quaternion quaternion2) { Quaternion quaternion; Add(ref quaternion1, ref quaternion2, out quaternion); return quaternion; } /// /// Creates a new that contains the sum of two quaternions. /// /// Source . /// Source . /// The result of the quaternion addition as an output parameter. public static void Add( ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result ) { result.X = quaternion1.X + quaternion2.X; result.Y = quaternion1.Y + quaternion2.Y; result.Z = quaternion1.Z + quaternion2.Z; result.W = quaternion1.W + quaternion2.W; } /// /// Creates a new that contains concatenation between two quaternion. /// /// The first to concatenate. /// The second to concatenate. /// The result of rotation of followed by rotation. public static Quaternion Concatenate(Quaternion value1, Quaternion value2) { Quaternion quaternion; Concatenate(ref value1, ref value2, out quaternion); return quaternion; } /// /// Creates a new that contains concatenation between two quaternion. /// /// The first to concatenate. /// The second to concatenate. /// The result of rotation of followed by rotation as an output parameter. public static void Concatenate( ref Quaternion value1, ref Quaternion value2, out Quaternion result ) { float x1 = value1.X; float y1 = value1.Y; float z1 = value1.Z; float w1 = value1.W; float x2 = value2.X; float y2 = value2.Y; float z2 = value2.Z; float w2 = value2.W; result.X = ((x2 * w1) + (x1 * w2)) + ((y2 * z1) - (z2 * y1)); result.Y = ((y2 * w1) + (y1 * w2)) + ((z2 * x1) - (x2 * z1)); result.Z = ((z2 * w1) + (z1 * w2)) + ((x2 * y1) - (y2 * x1)); result.W = (w2 * w1) - (((x2 * x1) + (y2 * y1)) + (z2 * z1)); } /// /// Creates a new that contains conjugated version of the specified quaternion. /// /// The quaternion which values will be used to create the conjugated version. /// The conjugate version of the specified quaternion. public static Quaternion Conjugate(Quaternion value) { return new Quaternion(-value.X, -value.Y, -value.Z, value.W); } /// /// Creates a new that contains conjugated version of the specified quaternion. /// /// The quaternion which values will be used to create the conjugated version. /// The conjugated version of the specified quaternion as an output parameter. public static void Conjugate(ref Quaternion value, out Quaternion result) { result.X = -value.X; result.Y = -value.Y; result.Z = -value.Z; result.W = value.W; } /// /// Creates a new from the specified axis and angle. /// /// The axis of rotation. /// The angle in radians. /// The new quaternion builded from axis and angle. public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle) { Quaternion quaternion; CreateFromAxisAngle(ref axis, angle, out quaternion); return quaternion; } /// /// Creates a new from the specified axis and angle. /// /// The axis of rotation. /// The angle in radians. /// The new quaternion builded from axis and angle as an output parameter. public static void CreateFromAxisAngle( ref Vector3 axis, float angle, out Quaternion result ) { float half = angle * 0.5f; float sin = (float) Math.Sin((double) half); float cos = (float) Math.Cos((double) half); result.X = axis.X * sin; result.Y = axis.Y * sin; result.Z = axis.Z * sin; result.W = cos; } /// /// Creates a new from the specified . /// /// The rotation matrix. /// A quaternion composed from the rotation part of the matrix. public static Quaternion CreateFromRotationMatrix(Matrix matrix) { Quaternion quaternion; CreateFromRotationMatrix(ref matrix, out quaternion); return quaternion; } /// /// Creates a new from the specified . /// /// The rotation matrix. /// A quaternion composed from the rotation part of the matrix as an output parameter. public static void CreateFromRotationMatrix(ref Matrix matrix, out Quaternion result) { float sqrt; float half; float scale = matrix.M11 + matrix.M22 + matrix.M33; if (scale > 0.0f) { sqrt = (float) Math.Sqrt(scale + 1.0f); result.W = sqrt * 0.5f; sqrt = 0.5f / sqrt; result.X = (matrix.M23 - matrix.M32) * sqrt; result.Y = (matrix.M31 - matrix.M13) * sqrt; result.Z = (matrix.M12 - matrix.M21) * sqrt; } else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33)) { sqrt = (float) Math.Sqrt(1.0f + matrix.M11 - matrix.M22 - matrix.M33); half = 0.5f / sqrt; result.X = 0.5f * sqrt; result.Y = (matrix.M12 + matrix.M21) * half; result.Z = (matrix.M13 + matrix.M31) * half; result.W = (matrix.M23 - matrix.M32) * half; } else if (matrix.M22 > matrix.M33) { sqrt = (float) Math.Sqrt(1.0f + matrix.M22 - matrix.M11 - matrix.M33); half = 0.5f/sqrt; result.X = (matrix.M21 + matrix.M12)*half; result.Y = 0.5f*sqrt; result.Z = (matrix.M32 + matrix.M23)*half; result.W = (matrix.M31 - matrix.M13)*half; } else { sqrt = (float) Math.Sqrt(1.0f + matrix.M33 - matrix.M11 - matrix.M22); half = 0.5f / sqrt; result.X = (matrix.M31 + matrix.M13) * half; result.Y = (matrix.M32 + matrix.M23) * half; result.Z = 0.5f * sqrt; result.W = (matrix.M12 - matrix.M21) * half; } } /// /// Creates a new from the specified yaw, pitch and roll angles. /// /// Yaw around the y axis in radians. /// Pitch around the x axis in radians. /// Roll around the z axis in radians. /// A new quaternion from the concatenated yaw, pitch, and roll angles. public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll) { Quaternion quaternion; CreateFromYawPitchRoll(yaw, pitch, roll, out quaternion); return quaternion; } /// /// Creates a new from the specified yaw, pitch and roll angles. /// /// Yaw around the y axis in radians. /// Pitch around the x axis in radians. /// Roll around the z axis in radians. /// A new quaternion from the concatenated yaw, pitch, and roll angles as an output parameter. public static void CreateFromYawPitchRoll( float yaw, float pitch, float roll, out Quaternion result) { float halfRoll = roll * 0.5f; float sinRoll = (float) Math.Sin(halfRoll); float cosRoll = (float) Math.Cos(halfRoll); float halfPitch = pitch * 0.5f; float sinPitch = (float) Math.Sin(halfPitch); float cosPitch = (float) Math.Cos(halfPitch); float halfYaw = yaw * 0.5f; float sinYaw = (float) Math.Sin(halfYaw); float cosYaw = (float) Math.Cos(halfYaw); result.X = ((cosYaw * sinPitch) * cosRoll) + ((sinYaw * cosPitch) * sinRoll); result.Y = ((sinYaw * cosPitch) * cosRoll) - ((cosYaw * sinPitch) * sinRoll); result.Z = ((cosYaw * cosPitch) * sinRoll) - ((sinYaw * sinPitch) * cosRoll); result.W = ((cosYaw * cosPitch) * cosRoll) + ((sinYaw * sinPitch) * sinRoll); } /// /// Divides a by the other . /// /// Source . /// Divisor . /// The result of dividing the quaternions. public static Quaternion Divide(Quaternion quaternion1, Quaternion quaternion2) { Quaternion quaternion; Divide(ref quaternion1, ref quaternion2, out quaternion); return quaternion; } /// /// Divides a by the other . /// /// Source . /// Divisor . /// The result of dividing the quaternions as an output parameter. public static void Divide( ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result ) { float x = quaternion1.X; float y = quaternion1.Y; float z = quaternion1.Z; float w = quaternion1.W; float num14 = ( (quaternion2.X * quaternion2.X) + (quaternion2.Y * quaternion2.Y) + (quaternion2.Z * quaternion2.Z) + (quaternion2.W * quaternion2.W) ); float num5 = 1f / num14; float num4 = -quaternion2.X * num5; float num3 = -quaternion2.Y * num5; float num2 = -quaternion2.Z * num5; float num = quaternion2.W * num5; float num13 = (y * num2) - (z * num3); float num12 = (z * num4) - (x * num2); float num11 = (x * num3) - (y * num4); float num10 = ((x * num4) + (y * num3)) + (z * num2); result.X = ((x * num) + (num4 * w)) + num13; result.Y = ((y * num) + (num3 * w)) + num12; result.Z = ((z * num) + (num2 * w)) + num11; result.W = (w * num) - num10; } /// /// Returns a dot product of two quaternions. /// /// The first quaternion. /// The second quaternion. /// The dot product of two quaternions. public static float Dot(Quaternion quaternion1, Quaternion quaternion2) { return ( (quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y) + (quaternion1.Z * quaternion2.Z) + (quaternion1.W * quaternion2.W) ); } /// /// Returns a dot product of two quaternions. /// /// The first quaternion. /// The second quaternion. /// The dot product of two quaternions as an output parameter. public static void Dot( ref Quaternion quaternion1, ref Quaternion quaternion2, out float result ) { result = ( (quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y) + (quaternion1.Z * quaternion2.Z) + (quaternion1.W * quaternion2.W) ); } /// /// Returns the inverse quaternion which represents the opposite rotation. /// /// Source . /// The inverse quaternion. public static Quaternion Inverse(Quaternion quaternion) { Quaternion inverse; Inverse(ref quaternion, out inverse); return inverse; } /// /// Returns the inverse quaternion which represents the opposite rotation. /// /// Source . /// The inverse quaternion as an output parameter. public static void Inverse(ref Quaternion quaternion, out Quaternion result) { float num2 = ( (quaternion.X * quaternion.X) + (quaternion.Y * quaternion.Y) + (quaternion.Z * quaternion.Z) + (quaternion.W * quaternion.W) ); float num = 1f / num2; result.X = -quaternion.X * num; result.Y = -quaternion.Y * num; result.Z = -quaternion.Z * num; result.W = quaternion.W * num; } /// /// Performs a linear blend between two quaternions. /// /// Source . /// Source . /// The blend amount where 0 returns and 1 . /// The result of linear blending between two quaternions. public static Quaternion Lerp( Quaternion quaternion1, Quaternion quaternion2, float amount ) { Quaternion quaternion; Lerp(ref quaternion1, ref quaternion2, amount, out quaternion); return quaternion; } /// /// Performs a linear blend between two quaternions. /// /// Source . /// Source . /// The blend amount where 0 returns and 1 . /// The result of linear blending between two quaternions as an output parameter. public static void Lerp( ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result ) { float num = amount; float num2 = 1f - num; float num5 = ( (quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y) + (quaternion1.Z * quaternion2.Z) + (quaternion1.W * quaternion2.W) ); if (num5 >= 0f) { result.X = (num2 * quaternion1.X) + (num * quaternion2.X); result.Y = (num2 * quaternion1.Y) + (num * quaternion2.Y); result.Z = (num2 * quaternion1.Z) + (num * quaternion2.Z); result.W = (num2 * quaternion1.W) + (num * quaternion2.W); } else { result.X = (num2 * quaternion1.X) - (num * quaternion2.X); result.Y = (num2 * quaternion1.Y) - (num * quaternion2.Y); result.Z = (num2 * quaternion1.Z) - (num * quaternion2.Z); result.W = (num2 * quaternion1.W) - (num * quaternion2.W); } float num4 = ( (result.X * result.X) + (result.Y * result.Y) + (result.Z * result.Z) + (result.W * result.W) ); float num3 = 1f / ((float) Math.Sqrt((double) num4)); result.X *= num3; result.Y *= num3; result.Z *= num3; result.W *= num3; } /// /// Performs a spherical linear blend between two quaternions. /// /// Source . /// Source . /// The blend amount where 0 returns and 1 . /// The result of spherical linear blending between two quaternions. public static Quaternion Slerp( Quaternion quaternion1, Quaternion quaternion2, float amount ) { Quaternion quaternion; Slerp(ref quaternion1, ref quaternion2, amount, out quaternion); return quaternion; } /// /// Performs a spherical linear blend between two quaternions. /// /// Source . /// Source . /// The blend amount where 0 returns and 1 . /// The result of spherical linear blending between two quaternions as an output parameter. public static void Slerp( ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result ) { float num2; float num3; float num = amount; float num4 = ( (quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y) + (quaternion1.Z * quaternion2.Z) + (quaternion1.W * quaternion2.W) ); float flag = 1.0f; if (num4 < 0f) { flag = -1.0f; num4 = -num4; } if (num4 > 0.999999f) { num3 = 1f - num; num2 = num * flag; } else { float num5 = (float) Math.Acos((double) num4); float num6 = (float) (1.0 / Math.Sin((double) num5)); num3 = ((float) Math.Sin((double) ((1f - num) * num5))) * num6; num2 = flag * (((float) Math.Sin((double) (num * num5))) * num6); } result.X = (num3 * quaternion1.X) + (num2 * quaternion2.X); result.Y = (num3 * quaternion1.Y) + (num2 * quaternion2.Y); result.Z = (num3 * quaternion1.Z) + (num2 * quaternion2.Z); result.W = (num3 * quaternion1.W) + (num2 * quaternion2.W); } /// /// Creates a new that contains subtraction of one from another. /// /// Source . /// Source . /// The result of the quaternion subtraction. public static Quaternion Subtract(Quaternion quaternion1, Quaternion quaternion2) { Quaternion quaternion; Subtract(ref quaternion1, ref quaternion2, out quaternion); return quaternion; } /// /// Creates a new that contains subtraction of one from another. /// /// Source . /// Source . /// The result of the quaternion subtraction as an output parameter. public static void Subtract( ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result ) { result.X = quaternion1.X - quaternion2.X; result.Y = quaternion1.Y - quaternion2.Y; result.Z = quaternion1.Z - quaternion2.Z; result.W = quaternion1.W - quaternion2.W; } /// /// Creates a new that contains a multiplication of two quaternions. /// /// Source . /// Source . /// The result of the quaternion multiplication. public static Quaternion Multiply(Quaternion quaternion1, Quaternion quaternion2) { Quaternion quaternion; Multiply(ref quaternion1, ref quaternion2, out quaternion); return quaternion; } /// /// Creates a new that contains a multiplication of and a scalar. /// /// Source . /// Scalar value. /// The result of the quaternion multiplication with a scalar. public static Quaternion Multiply(Quaternion quaternion1, float scaleFactor) { Quaternion quaternion; Multiply(ref quaternion1, scaleFactor, out quaternion); return quaternion; } /// /// Creates a new that contains a multiplication of two quaternions. /// /// Source . /// Source . /// The result of the quaternion multiplication as an output parameter. public static void Multiply( ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result ) { float x = quaternion1.X; float y = quaternion1.Y; float z = quaternion1.Z; float w = quaternion1.W; float num4 = quaternion2.X; float num3 = quaternion2.Y; float num2 = quaternion2.Z; float num = quaternion2.W; float num12 = (y * num2) - (z * num3); float num11 = (z * num4) - (x * num2); float num10 = (x * num3) - (y * num4); float num9 = ((x * num4) + (y * num3)) + (z * num2); result.X = ((x * num) + (num4 * w)) + num12; result.Y = ((y * num) + (num3 * w)) + num11; result.Z = ((z * num) + (num2 * w)) + num10; result.W = (w * num) - num9; } /// /// Creates a new that contains a multiplication of and a scalar. /// /// Source . /// Scalar value. /// The result of the quaternion multiplication with a scalar as an output parameter. public static void Multiply( ref Quaternion quaternion1, float scaleFactor, out Quaternion result ) { result.X = quaternion1.X * scaleFactor; result.Y = quaternion1.Y * scaleFactor; result.Z = quaternion1.Z * scaleFactor; result.W = quaternion1.W * scaleFactor; } /// /// Flips the sign of the all the quaternion components. /// /// Source . /// The result of the quaternion negation. public static Quaternion Negate(Quaternion quaternion) { return new Quaternion( -quaternion.X, -quaternion.Y, -quaternion.Z, -quaternion.W ); } /// /// Flips the sign of the all the quaternion components. /// /// Source . /// The result of the quaternion negation as an output parameter. public static void Negate(ref Quaternion quaternion, out Quaternion result) { result.X = -quaternion.X; result.Y = -quaternion.Y; result.Z = -quaternion.Z; result.W = -quaternion.W; } /// /// Scales the quaternion magnitude to unit length. /// /// Source . /// The unit length quaternion. public static Quaternion Normalize(Quaternion quaternion) { Quaternion quaternion2; Normalize(ref quaternion, out quaternion2); return quaternion2; } /// /// Scales the quaternion magnitude to unit length. /// /// Source . /// The unit length quaternion an output parameter. public static void Normalize(ref Quaternion quaternion, out Quaternion result) { float num = 1.0f / ((float) Math.Sqrt( (quaternion.X * quaternion.X) + (quaternion.Y * quaternion.Y) + (quaternion.Z * quaternion.Z) + (quaternion.W * quaternion.W) )); result.X = quaternion.X * num; result.Y = quaternion.Y * num; result.Z = quaternion.Z * num; result.W = quaternion.W * num; } #endregion #region Public Static Operator Overloads /// /// Adds two quaternions. /// /// Source on the left of the add sign. /// Source on the right of the add sign. /// Sum of the vectors. public static Quaternion operator +(Quaternion quaternion1, Quaternion quaternion2) { Quaternion quaternion; Add(ref quaternion1, ref quaternion2, out quaternion); return quaternion; } /// /// Divides a by the other . /// /// Source on the left of the div sign. /// Divisor on the right of the div sign. /// The result of dividing the quaternions. public static Quaternion operator /(Quaternion quaternion1, Quaternion quaternion2) { Quaternion quaternion; Divide(ref quaternion1, ref quaternion2, out quaternion); return quaternion; } /// /// Compares whether two instances are equal. /// /// instance on the left of the equal sign. /// instance on the right of the equal sign. /// true if the instances are equal; false otherwise. public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2) { return quaternion1.Equals(quaternion2); } /// /// Compares whether two instances are not equal. /// /// instance on the left of the not equal sign. /// instance on the right of the not equal sign. /// true if the instances are not equal; false otherwise. public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2) { return !quaternion1.Equals(quaternion2); } /// /// Multiplies two quaternions. /// /// Source on the left of the mul sign. /// Source on the right of the mul sign. /// Result of the quaternions multiplication. public static Quaternion operator *(Quaternion quaternion1, Quaternion quaternion2) { Quaternion quaternion; Multiply(ref quaternion1, ref quaternion2, out quaternion); return quaternion; } /// /// Multiplies the components of quaternion by a scalar. /// /// Source on the left of the mul sign. /// Scalar value on the right of the mul sign. /// Result of the quaternion multiplication with a scalar. public static Quaternion operator *(Quaternion quaternion1, float scaleFactor) { Quaternion quaternion; Multiply(ref quaternion1, scaleFactor, out quaternion); return quaternion; } /// /// Subtracts a from a . /// /// Source on the left of the sub sign. /// Source on the right of the sub sign. /// Result of the quaternion subtraction. public static Quaternion operator -(Quaternion quaternion1, Quaternion quaternion2) { Quaternion quaternion; Subtract(ref quaternion1, ref quaternion2, out quaternion); return quaternion; } /// /// Flips the sign of the all the quaternion components. /// /// Source on the right of the sub sign. /// The result of the quaternion negation. public static Quaternion operator -(Quaternion quaternion) { Quaternion quaternion2; Negate(ref quaternion, out quaternion2); return quaternion2; } #endregion } }