// Copyright (c) Xenko contributors (https://xenko.com) and Silicon Studio Corp. (https://www.siliconstudio.co.jp) // Distributed under the MIT license. See the LICENSE.md file in the project root for more information. // // ----------------------------------------------------------------------------- // Original code from SlimMath project. http://code.google.com/p/slimmath/ // Greetings to SlimDX Group. Original code published with the following license: // ----------------------------------------------------------------------------- /* * Copyright (c) 2007-2011 SlimDX Group * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ using System; using System.Globalization; using System.Runtime.InteropServices; using System.Runtime.Serialization; namespace math { /// /// Represents a four dimensional mathematical quaternion. /// [DataContract( Name = "quaternion")] [DataStyle(DataStyle.Compact)] [StructLayout(LayoutKind.Sequential, Pack = 4)] public struct Quaternion : IEquatable, IFormattable { /// /// The size of the type, in bytes. /// public static readonly int SizeInBytes = lib.Util.SizeOf(); /// /// A with all of its components set to zero. /// public static readonly Quaternion Zero = new Quaternion(); /// /// A with all of its components set to one. /// public static readonly Quaternion One = new Quaternion(1.0f, 1.0f, 1.0f, 1.0f); /// /// The identity (0, 0, 0, 1). /// public static readonly Quaternion Identity = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f); /// /// The X component of the quaternion. /// public float X; /// /// The Y component of the quaternion. /// public float Y; /// /// The Z component of the quaternion. /// public float Z; /// /// The W component of the quaternion. /// public float W; /// /// Initializes a new instance of the struct. /// /// The value that will be assigned to all components. public Quaternion(float value) { X = value; Y = value; Z = value; W = value; } /// /// Initializes a new instance of the struct. /// /// A vector containing the values with which to initialize the components. public Quaternion(Vec4 value) { X = value.X; Y = value.Y; Z = value.Z; W = value.W; } /// /// Initializes a new instance of the struct. /// /// A vector containing the values with which to initialize the X, Y, and Z components. /// Initial value for the W component of the quaternion. public Quaternion(Vec3 value, float w) { X = value.X; Y = value.Y; Z = value.Z; W = w; } /// /// Initializes a new instance of the struct. /// /// A vector containing the values with which to initialize the X and Y components. /// Initial value for the Z component of the quaternion. /// Initial value for the W component of the quaternion. public Quaternion(Vec2 value, float z, float w) { X = value.X; Y = value.Y; Z = z; W = w; } /// /// Initializes a new instance of the struct. /// /// Initial value for the X component of the quaternion. /// Initial value for the Y component of the quaternion. /// Initial value for the Z component of the quaternion. /// Initial value for the W component of the quaternion. public Quaternion(float x, float y, float z, float w) { X = x; Y = y; Z = z; W = w; } /// /// Initializes a new instance of the struct. /// /// The values to assign to the X, Y, Z, and W components of the quaternion. This must be an array with four elements. /// Thrown when is null. /// Thrown when contains more or less than four elements. public Quaternion(float[] values) { if (values == null) throw new ArgumentNullException("values"); if (values.Length != 4) throw new ArgumentOutOfRangeException("values", "There must be four and only four input values for Quaternion."); X = values[0]; Y = values[1]; Z = values[2]; W = values[3]; } /// /// Gets a value indicating whether this instance is equivalent to the identity quaternion. /// /// /// true if this instance is an identity quaternion; otherwise, false. /// public bool IsIdentity { get { return this.Equals(Identity); } } /// /// Gets a value indicting whether this instance is normalized. /// public bool IsNormalized { get { return Math.Abs((X * X) + (Y * Y) + (Z * Z) + (W * W) - 1f) < MathUtil.ZeroTolerance; } } /// /// Gets the angle of the quaternion. /// /// The quaternion's angle. public float Angle { get { float length = (X * X) + (Y * Y) + (Z * Z); if (length < MathUtil.ZeroTolerance) return 0.0f; return (float)(2.0 * Math.Acos(W)); } } /// /// Gets the axis components of the quaternion. /// /// The axis components of the quaternion. public Vec3 Axis { get { float length = (X * X) + (Y * Y) + (Z * Z); if (length < MathUtil.ZeroTolerance) return Vec3.UnitX; float inv = 1.0f / length; return new Vec3(X * inv, Y * inv, Z * inv); } } /// /// Gets yaw/pitch/roll equivalent of the quaternion /// public Vec3 YawPitchRoll { get { Vec3 yawPitchRoll; RotationYawPitchRoll(ref this, out yawPitchRoll.X, out yawPitchRoll.Y, out yawPitchRoll.Z); return yawPitchRoll; } } /// /// Gets or sets the component at the specified index. /// /// The value of the X, Y, Z, or W component, depending on the index. /// The index of the component to access. Use 0 for the X component, 1 for the Y component, 2 for the Z component, and 3 for the W component. /// The value of the component at the specified index. /// Thrown when the is out of the range [0, 3]. public float this[int index] { get { switch (index) { case 0: return X; case 1: return Y; case 2: return Z; case 3: return W; } throw new ArgumentOutOfRangeException("index", "Indices for Quaternion run from 0 to 3, inclusive."); } set { switch (index) { case 0: X = value; break; case 1: Y = value; break; case 2: Z = value; break; case 3: W = value; break; default: throw new ArgumentOutOfRangeException("index", "Indices for Quaternion run from 0 to 3, inclusive."); } } } /// /// Conjugates the quaternion. /// public void Conjugate() { X = -X; Y = -Y; Z = -Z; } /// /// Conjugates and renormalizes the quaternion. /// public void Invert() { float lengthSq = LengthSquared(); if (lengthSq > MathUtil.ZeroTolerance) { lengthSq = 1.0f / lengthSq; X = -X * lengthSq; Y = -Y * lengthSq; Z = -Z * lengthSq; W = W * lengthSq; } } /// /// Calculates the length of the quaternion. /// /// The length of the quaternion. /// /// may be preferred when only the relative length is needed /// and speed is of the essence. /// public float Length() { return (float)Math.Sqrt((X * X) + (Y * Y) + (Z * Z) + (W * W)); } /// /// Calculates the squared length of the quaternion. /// /// The squared length of the quaternion. /// /// This method may be preferred to when only a relative length is needed /// and speed is of the essence. /// public float LengthSquared() { return (X * X) + (Y * Y) + (Z * Z) + (W * W); } /// /// Converts the quaternion into a unit quaternion. /// public void Normalize() { float length = Length(); if (length > MathUtil.ZeroTolerance) { float inverse = 1.0f / length; X *= inverse; Y *= inverse; Z *= inverse; W *= inverse; } } /// /// Creates an array containing the elements of the quaternion. /// /// A four-element array containing the components of the quaternion. public float[] ToArray() { return new float[] { X, Y, Z, W }; } /// /// Adds two quaternions. /// /// The first quaternion to add. /// The second quaternion to add. /// When the method completes, contains the sum of the two quaternions. public static void Add(ref Quaternion left, ref Quaternion right, out Quaternion result) { result.X = left.X + right.X; result.Y = left.Y + right.Y; result.Z = left.Z + right.Z; result.W = left.W + right.W; } /// /// Adds two quaternions. /// /// The first quaternion to add. /// The second quaternion to add. /// The sum of the two quaternions. public static Quaternion Add(Quaternion left, Quaternion right) { Quaternion result; Add(ref left, ref right, out result); return result; } /// /// Subtracts two quaternions. /// /// The first quaternion to subtract. /// The second quaternion to subtract. /// When the method completes, contains the difference of the two quaternions. public static void Subtract(ref Quaternion left, ref Quaternion right, out Quaternion result) { result.X = left.X - right.X; result.Y = left.Y - right.Y; result.Z = left.Z - right.Z; result.W = left.W - right.W; } /// /// Subtracts two quaternions. /// /// The first quaternion to subtract. /// The second quaternion to subtract. /// The difference of the two quaternions. public static Quaternion Subtract(Quaternion left, Quaternion right) { Quaternion result; Subtract(ref left, ref right, out result); return result; } /// /// Scales a quaternion by the given value. /// /// The quaternion to scale. /// The amount by which to scale the quaternion. /// When the method completes, contains the scaled quaternion. public static void Multiply(ref Quaternion value, float scale, out Quaternion result) { result.X = value.X * scale; result.Y = value.Y * scale; result.Z = value.Z * scale; result.W = value.W * scale; } /// /// Scales a quaternion by the given value. /// /// The quaternion to scale. /// The amount by which to scale the quaternion. /// The scaled quaternion. public static Quaternion Multiply(Quaternion value, float scale) { Quaternion result; Multiply(ref value, scale, out result); return result; } /// /// Modulates a quaternion by another. /// /// The first quaternion to modulate. /// The second quaternion to modulate. /// When the moethod completes, contains the modulated quaternion. public static void Multiply(ref Quaternion left, ref Quaternion right, out Quaternion result) { float lx = left.X; float ly = left.Y; float lz = left.Z; float lw = left.W; float rx = right.X; float ry = right.Y; float rz = right.Z; float rw = right.W; result.X = (rx * lw + lx * rw + ry * lz) - (rz * ly); result.Y = (ry * lw + ly * rw + rz * lx) - (rx * lz); result.Z = (rz * lw + lz * rw + rx * ly) - (ry * lx); result.W = (rw * lw) - (rx * lx + ry * ly + rz * lz); } /// /// Modulates a quaternion by another. /// /// The first quaternion to modulate. /// The second quaternion to modulate. /// The modulated quaternion. public static Quaternion Multiply(Quaternion left, Quaternion right) { Quaternion result; Multiply(ref left, ref right, out result); return result; } /// /// Reverses the direction of a given quaternion. /// /// The quaternion to negate. /// When the method completes, contains a quaternion facing in the opposite direction. public static void Negate(ref Quaternion value, out Quaternion result) { result.X = -value.X; result.Y = -value.Y; result.Z = -value.Z; result.W = -value.W; } /// /// Reverses the direction of a given quaternion. /// /// The quaternion to negate. /// A quaternion facing in the opposite direction. public static Quaternion Negate(Quaternion value) { Quaternion result; Negate(ref value, out result); return result; } /// /// Returns a containing the 4D Cartesian coordinates of a point specified in Barycentric coordinates relative to a 2D triangle. /// /// A containing the 4D Cartesian coordinates of vertex 1 of the triangle. /// A containing the 4D Cartesian coordinates of vertex 2 of the triangle. /// A containing the 4D Cartesian coordinates of vertex 3 of the triangle. /// Barycentric coordinate b2, which expresses the weighting factor toward vertex 2 (specified in ). /// Barycentric coordinate b3, which expresses the weighting factor toward vertex 3 (specified in ). /// When the method completes, contains a new containing the 4D Cartesian coordinates of the specified point. public static void Barycentric(ref Quaternion value1, ref Quaternion value2, ref Quaternion value3, float amount1, float amount2, out Quaternion result) { Quaternion start, end; Slerp(ref value1, ref value2, amount1 + amount2, out start); Slerp(ref value1, ref value3, amount1 + amount2, out end); Slerp(ref start, ref end, amount2 / (amount1 + amount2), out result); } /// /// Returns a containing the 4D Cartesian coordinates of a point specified in Barycentric coordinates relative to a 2D triangle. /// /// A containing the 4D Cartesian coordinates of vertex 1 of the triangle. /// A containing the 4D Cartesian coordinates of vertex 2 of the triangle. /// A containing the 4D Cartesian coordinates of vertex 3 of the triangle. /// Barycentric coordinate b2, which expresses the weighting factor toward vertex 2 (specified in ). /// Barycentric coordinate b3, which expresses the weighting factor toward vertex 3 (specified in ). /// A new containing the 4D Cartesian coordinates of the specified point. public static Quaternion Barycentric(Quaternion value1, Quaternion value2, Quaternion value3, float amount1, float amount2) { Quaternion result; Barycentric(ref value1, ref value2, ref value3, amount1, amount2, out result); return result; } /// /// Conjugates a quaternion. /// /// The quaternion to conjugate. /// When the method completes, contains the conjugated quaternion. 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; } /// /// Conjugates a quaternion. /// /// The quaternion to conjugate. /// The conjugated quaternion. public static Quaternion Conjugate(Quaternion value) { Quaternion result; Conjugate(ref value, out result); return result; } /// /// Calculates the dot product of two quaternions. /// /// First source quaternion. /// Second source quaternion. /// When the method completes, contains the dot product of the two quaternions. public static void Dot(ref Quaternion left, ref Quaternion right, out float result) { result = (left.X * right.X) + (left.Y * right.Y) + (left.Z * right.Z) + (left.W * right.W); } /// /// Calculates the dot product of two quaternions. /// /// First source quaternion. /// Second source quaternion. /// The dot product of the two quaternions. public static float Dot(Quaternion left, Quaternion right) { return (left.X * right.X) + (left.Y * right.Y) + (left.Z * right.Z) + (left.W * right.W); } /// /// Exponentiates a quaternion. /// /// The quaternion to exponentiate. /// When the method completes, contains the exponentiated quaternion. public static void Exponential(ref Quaternion value, out Quaternion result) { float angle = (float)Math.Sqrt((value.X * value.X) + (value.Y * value.Y) + (value.Z * value.Z)); float sin = (float)Math.Sin(angle); if (Math.Abs(sin) >= MathUtil.ZeroTolerance) { float coeff = sin / angle; result.X = coeff * value.X; result.Y = coeff * value.Y; result.Z = coeff * value.Z; } else { result = value; } result.W = (float)Math.Cos(angle); } /// /// Exponentiates a quaternion. /// /// The quaternion to exponentiate. /// The exponentiated quaternion. public static Quaternion Exponential(Quaternion value) { Quaternion result; Exponential(ref value, out result); return result; } /// /// Conjugates and renormalizes the quaternion. /// /// The quaternion to conjugate and renormalize. /// When the method completes, contains the conjugated and renormalized quaternion. public static void Invert(ref Quaternion value, out Quaternion result) { result = value; result.Invert(); } /// /// Conjugates and renormalizes the quaternion. /// /// The quaternion to conjugate and renormalize. /// The conjugated and renormalized quaternion. public static Quaternion Invert(Quaternion value) { Quaternion result; Invert(ref value, out result); return result; } /// /// Performs a linear interpolation between two quaternions. /// /// Start quaternion. /// End quaternion. /// Value between 0 and 1 indicating the weight of . /// When the method completes, contains the linear interpolation of the two quaternions. /// /// This method performs the linear interpolation based on the following formula. /// start + (end - start) * amount /// Passing a value of 0 will cause to be returned; a value of 1 will cause to be returned. /// public static void Lerp(ref Quaternion start, ref Quaternion end, float amount, out Quaternion result) { float inverse = 1.0f - amount; if (Dot(start, end) >= 0.0f) { result.X = (inverse * start.X) + (amount * end.X); result.Y = (inverse * start.Y) + (amount * end.Y); result.Z = (inverse * start.Z) + (amount * end.Z); result.W = (inverse * start.W) + (amount * end.W); } else { result.X = (inverse * start.X) - (amount * end.X); result.Y = (inverse * start.Y) - (amount * end.Y); result.Z = (inverse * start.Z) - (amount * end.Z); result.W = (inverse * start.W) - (amount * end.W); } result.Normalize(); } /// /// Performs a linear interpolation between two quaternion. /// /// Start quaternion. /// End quaternion. /// Value between 0 and 1 indicating the weight of . /// The linear interpolation of the two quaternions. /// /// This method performs the linear interpolation based on the following formula. /// start + (end - start) * amount /// Passing a value of 0 will cause to be returned; a value of 1 will cause to be returned. /// public static Quaternion Lerp(Quaternion start, Quaternion end, float amount) { Quaternion result; Lerp(ref start, ref end, amount, out result); return result; } /// /// Calculates the natural logarithm of the specified quaternion. /// /// The quaternion whose logarithm will be calculated. /// When the method completes, contains the natural logarithm of the quaternion. public static void Logarithm(ref Quaternion value, out Quaternion result) { if (Math.Abs(value.W) < 1.0) { float angle = (float)Math.Acos(value.W); float sin = (float)Math.Sin(angle); if (Math.Abs(sin) >= MathUtil.ZeroTolerance) { float coeff = angle / sin; result.X = value.X * coeff; result.Y = value.Y * coeff; result.Z = value.Z * coeff; } else { result = value; } } else { result = value; } result.W = 0.0f; } /// /// Calculates the natural logarithm of the specified quaternion. /// /// The quaternion whose logarithm will be calculated. /// The natural logarithm of the quaternion. public static Quaternion Logarithm(Quaternion value) { Quaternion result; Logarithm(ref value, out result); return result; } /// /// Converts the quaternion into a unit quaternion. /// /// The quaternion to normalize. /// When the method completes, contains the normalized quaternion. public static void Normalize(ref Quaternion value, out Quaternion result) { Quaternion temp = value; result = temp; result.Normalize(); } /// /// Converts the quaternion into a unit quaternion. /// /// The quaternion to normalize. /// The normalized quaternion. public static Quaternion Normalize(Quaternion value) { value.Normalize(); return value; } /// /// Rotates a Vector3 by the specified quaternion rotation. /// /// The vector to rotate. public void Rotate(ref Vec3 vector) { var pureQuaternion = new Quaternion(vector, 0); pureQuaternion = Conjugate(this) * pureQuaternion * this; vector.X = pureQuaternion.X; vector.Y = pureQuaternion.Y; vector.Z = pureQuaternion.Z; } /// /// Creates a quaternion given a rotation and an axis. /// /// The axis of rotation. /// The angle of rotation. /// When the method completes, contains the newly created quaternion. public static void RotationAxis(ref Vec3 axis, float angle, out Quaternion result) { Vec3 normalized; Vec3.Normalize(ref axis, out normalized); float half = angle * 0.5f; float sin = (float)Math.Sin(half); float cos = (float)Math.Cos(half); result.X = normalized.X * sin; result.Y = normalized.Y * sin; result.Z = normalized.Z * sin; result.W = cos; } /// /// Creates a quaternion given a rotation and an axis. /// /// The axis of rotation. /// The angle of rotation. /// The newly created quaternion. public static Quaternion RotationAxis(Vec3 axis, float angle) { Quaternion result; RotationAxis(ref axis, angle, out result); return result; } /// /// Creates a quaternion given a rotation matrix. /// /// The rotation matrix. /// When the method completes, contains the newly created quaternion. public static void RotationMatrix(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 quaternion given a rotation matrix. /// /// The rotation matrix. /// The newly created quaternion. public static Quaternion RotationMatrix(Matrix matrix) { Quaternion result; RotationMatrix(ref matrix, out result); return result; } /// /// Creates a quaternion that rotates around the x-axis. /// /// Angle of rotation in radians. /// When the method completes, contains the newly created quaternion. public static void RotationX(float angle, out Quaternion result) { float halfAngle = angle * 0.5f; result = new Quaternion((float)Math.Sin(halfAngle), 0.0f, 0.0f, (float)Math.Cos(halfAngle)); } /// /// Creates a quaternion that rotates around the x-axis. /// /// Angle of rotation in radians. /// The created rotation quaternion. public static Quaternion RotationX(float angle) { Quaternion result; RotationX(angle, out result); return result; } /// /// Creates a quaternion that rotates around the y-axis. /// /// Angle of rotation in radians. /// When the method completes, contains the newly created quaternion. public static void RotationY(float angle, out Quaternion result) { float halfAngle = angle * 0.5f; result = new Quaternion(0.0f, (float)Math.Sin(halfAngle), 0.0f, (float)Math.Cos(halfAngle)); } /// /// Creates a quaternion that rotates around the y-axis. /// /// Angle of rotation in radians. /// The created rotation quaternion. public static Quaternion RotationY(float angle) { Quaternion result; RotationY(angle, out result); return result; } /// /// Creates a quaternion that rotates around the z-axis. /// /// Angle of rotation in radians. /// When the method completes, contains the newly created quaternion. public static void RotationZ(float angle, out Quaternion result) { float halfAngle = angle * 0.5f; result = new Quaternion(0.0f, 0.0f, (float)Math.Sin(halfAngle), (float)Math.Cos(halfAngle)); } /// /// Creates a quaternion that rotates around the z-axis. /// /// Angle of rotation in radians. /// The created rotation quaternion. public static Quaternion RotationZ(float angle) { Quaternion result; RotationZ(angle, out result); return result; } /// /// Calculate the yaw/pitch/roll rotation equivalent to the provided quaternion. /// /// The input quaternion /// The yaw component /// The pitch component /// The roll component public static void RotationYawPitchRoll(ref Quaternion rotation, out float yaw, out float pitch, out float roll) { // Equivalent to: // Matrix rotationMatrix; // Matrix.Rotation(ref cachedRotation, out rotationMatrix); // rotationMatrix.DecomposeXYZ(out rotationEuler); var xx = rotation.X * rotation.X; var yy = rotation.Y * rotation.Y; var zz = rotation.Z * rotation.Z; var xy = rotation.X * rotation.Y; var zw = rotation.Z * rotation.W; var zx = rotation.Z * rotation.X; var yw = rotation.Y * rotation.W; var yz = rotation.Y * rotation.Z; var xw = rotation.X * rotation.W; pitch = (float)Math.Asin(2.0f * (xw - yz)); double test = Math.Cos(pitch); if (test > MathUtil.ZeroTolerance) { roll = (float)Math.Atan2(2.0f * (xy + zw), 1.0f - (2.0f * (zz + xx))); yaw = (float)Math.Atan2(2.0f * (zx + yw), 1.0f - (2.0f * (yy + xx))); } else { roll = (float)Math.Atan2(-2.0f * (xy - zw), 1.0f - (2.0f * (yy + zz))); yaw = 0.0f; } } /// /// Creates a quaternion given a yaw, pitch, and roll value (angles in radians). /// /// The yaw of rotation in radians. /// The pitch of rotation in radians. /// The roll of rotation in radians. /// When the method completes, contains the newly created quaternion. public static void RotationYawPitchRoll(float yaw, float pitch, float roll, out Quaternion result) { var halfRoll = roll * 0.5f; var halfPitch = pitch * 0.5f; var halfYaw = yaw * 0.5f; var sinRoll = (float)Math.Sin(halfRoll); var cosRoll = (float)Math.Cos(halfRoll); var sinPitch = (float)Math.Sin(halfPitch); var cosPitch = (float)Math.Cos(halfPitch); var sinYaw = (float)Math.Sin(halfYaw); var cosYaw = (float)Math.Cos(halfYaw); var cosYawPitch = cosYaw * cosPitch; var sinYawPitch = sinYaw * sinPitch; result.X = (cosYaw * sinPitch * cosRoll) + (sinYaw * cosPitch * sinRoll); result.Y = (sinYaw * cosPitch * cosRoll) - (cosYaw * sinPitch * sinRoll); result.Z = (cosYawPitch * sinRoll) - (sinYawPitch * cosRoll); result.W = (cosYawPitch * cosRoll) + (sinYawPitch * sinRoll); } /// /// Creates a quaternion given a yaw, pitch, and roll value. /// /// The yaw of rotation. /// The pitch of rotation. /// The roll of rotation. /// The newly created quaternion. public static Quaternion RotationYawPitchRoll(float yaw, float pitch, float roll) { Quaternion result; RotationYawPitchRoll(yaw, pitch, roll, out result); return result; } /// /// Computes a quaternion corresponding to the rotation transforming the vector to the vector . /// /// The source vector of the transformation. /// The target vector of the transformation. /// The resulting quaternion corresponding to the transformation of the source vector to the target vector. public static Quaternion BetweenDirections(Vec3 source, Vec3 target) { Quaternion result; BetweenDirections(ref source, ref target, out result); return result; } /// /// Computes a quaternion corresponding to the rotation transforming the vector to the vector . /// /// The source vector of the transformation. /// The target vector of the transformation. /// The resulting quaternion corresponding to the transformation of the source vector to the target vector. public static void BetweenDirections(ref Vec3 source, ref Vec3 target, out Quaternion result) { var norms = (float)Math.Sqrt(source.LengthSquared() * target.LengthSquared()); var real = norms + Vec3.Dot(source, target); if (real < MathUtil.ZeroTolerance * norms) { // If source and target are exactly opposite, rotate 180 degrees around an arbitrary orthogonal axis. // Axis normalisation can happen later, when we normalise the quaternion. result = Math.Abs(source.X) > Math.Abs(source.Z) ? new Quaternion(-source.Y, source.X, 0.0f, 0.0f) : new Quaternion(0.0f, -source.Z, source.Y, 0.0f); } else { // Otherwise, build quaternion the standard way. var axis = Vec3.Cross(source, target); result = new Quaternion(axis, real); } result.Normalize(); } /// /// Interpolates between two quaternions, using spherical linear interpolation. /// /// Start quaternion. /// End quaternion. /// Value between 0 and 1 indicating the weight of . /// When the method completes, contains the spherical linear interpolation of the two quaternions. public static void Slerp(ref Quaternion start, ref Quaternion end, float amount, out Quaternion result) { float opposite; float inverse; float dot = Dot(start, end); if (Math.Abs(dot) > 1.0f - MathUtil.ZeroTolerance) { inverse = 1.0f - amount; opposite = amount * Math.Sign(dot); } else { float acos = (float)Math.Acos(Math.Abs(dot)); float invSin = (float)(1.0 / Math.Sin(acos)); inverse = (float)Math.Sin((1.0f - amount) * acos) * invSin; opposite = (float)Math.Sin(amount * acos) * invSin * Math.Sign(dot); } result.X = (inverse * start.X) + (opposite * end.X); result.Y = (inverse * start.Y) + (opposite * end.Y); result.Z = (inverse * start.Z) + (opposite * end.Z); result.W = (inverse * start.W) + (opposite * end.W); } /// /// Interpolates between two quaternions, using spherical linear interpolation. /// /// Start quaternion. /// End quaternion. /// Value between 0 and 1 indicating the weight of . /// The spherical linear interpolation of the two quaternions. public static Quaternion Slerp(Quaternion start, Quaternion end, float amount) { Quaternion result; Slerp(ref start, ref end, amount, out result); return result; } /// /// Interpolates between quaternions, using spherical quadrangle interpolation. /// /// First source quaternion. /// Second source quaternion. /// Thrid source quaternion. /// Fourth source quaternion. /// Value between 0 and 1 indicating the weight of interpolation. /// When the method completes, contains the spherical quadrangle interpolation of the quaternions. public static void Squad(ref Quaternion value1, ref Quaternion value2, ref Quaternion value3, ref Quaternion value4, float amount, out Quaternion result) { Quaternion start, end; Slerp(ref value1, ref value4, amount, out start); Slerp(ref value2, ref value3, amount, out end); Slerp(ref start, ref end, 2.0f * amount * (1.0f - amount), out result); } /// /// Interpolates between quaternions, using spherical quadrangle interpolation. /// /// First source quaternion. /// Second source quaternion. /// Thrid source quaternion. /// Fourth source quaternion. /// Value between 0 and 1 indicating the weight of interpolation. /// The spherical quadrangle interpolation of the quaternions. public static Quaternion Squad(Quaternion value1, Quaternion value2, Quaternion value3, Quaternion value4, float amount) { Quaternion result; Squad(ref value1, ref value2, ref value3, ref value4, amount, out result); return result; } /// /// Sets up control points for spherical quadrangle interpolation. /// /// First source quaternion. /// Second source quaternion. /// Third source quaternion. /// Fourth source quaternion. /// An array of three quaternions that represent control points for spherical quadrangle interpolation. public static Quaternion[] SquadSetup(Quaternion value1, Quaternion value2, Quaternion value3, Quaternion value4) { Quaternion q0 = (value1 + value2).LengthSquared() < (value1 - value2).LengthSquared() ? -value1 : value1; Quaternion q2 = (value2 + value3).LengthSquared() < (value2 - value3).LengthSquared() ? -value3 : value3; Quaternion q3 = (value3 + value4).LengthSquared() < (value3 - value4).LengthSquared() ? -value4 : value4; Quaternion q1 = value2; Quaternion q1Exp, q2Exp; Exponential(ref q1, out q1Exp); Exponential(ref q2, out q2Exp); Quaternion[] results = new Quaternion[3]; results[0] = q1 * Exponential(-0.25f * (Logarithm(q1Exp * q2) + Logarithm(q1Exp * q0))); results[1] = q2 * Exponential(-0.25f * (Logarithm(q2Exp * q3) + Logarithm(q2Exp * q1))); results[2] = q2; return results; } /// /// Adds two quaternions. /// /// The first quaternion to add. /// The second quaternion to add. /// The sum of the two quaternions. public static Quaternion operator +(Quaternion left, Quaternion right) { Quaternion result; Add(ref left, ref right, out result); return result; } /// /// Subtracts two quaternions. /// /// The first quaternion to subtract. /// The second quaternion to subtract. /// The difference of the two quaternions. public static Quaternion operator -(Quaternion left, Quaternion right) { Quaternion result; Subtract(ref left, ref right, out result); return result; } /// /// Reverses the direction of a given quaternion. /// /// The quaternion to negate. /// A quaternion facing in the opposite direction. public static Quaternion operator -(Quaternion value) { Quaternion result; Negate(ref value, out result); return result; } /// /// Scales a quaternion by the given value. /// /// The quaternion to scale. /// The amount by which to scale the quaternion. /// The scaled quaternion. public static Quaternion operator *(float scale, Quaternion value) { Quaternion result; Multiply(ref value, scale, out result); return result; } /// /// Scales a quaternion by the given value. /// /// The quaternion to scale. /// The amount by which to scale the quaternion. /// The scaled quaternion. public static Quaternion operator *(Quaternion value, float scale) { Quaternion result; Multiply(ref value, scale, out result); return result; } /// /// Multiplies a quaternion by another. /// /// The first quaternion to multiply. /// The second quaternion to multiply. /// The multiplied quaternion. public static Quaternion operator *(Quaternion left, Quaternion right) { Quaternion result; Multiply(ref left, ref right, out result); return result; } /// /// Tests for equality between two objects. /// /// The first value to compare. /// The second value to compare. /// true if has the same value as ; otherwise, false. public static bool operator ==(Quaternion left, Quaternion right) { return left.Equals(right); } /// /// Tests for inequality between two objects. /// /// The first value to compare. /// The second value to compare. /// true if has a different value than ; otherwise, false. public static bool operator !=(Quaternion left, Quaternion right) { return !left.Equals(right); } /// /// Returns a that represents this instance. /// /// /// A that represents this instance. /// public override string ToString() { return string.Format(CultureInfo.CurrentCulture, "X:{0} Y:{1} Z:{2} W:{3}", X, Y, Z, W); } /// /// Returns a that represents this instance. /// /// The format. /// /// A that represents this instance. /// public string ToString(string format) { if (format == null) return ToString(); return string.Format(CultureInfo.CurrentCulture, "X:{0} Y:{1} Z:{2} W:{3}", X.ToString(format, CultureInfo.CurrentCulture), Y.ToString(format, CultureInfo.CurrentCulture), Z.ToString(format, CultureInfo.CurrentCulture), W.ToString(format, CultureInfo.CurrentCulture)); } /// /// Returns a that represents this instance. /// /// The format provider. /// /// A that represents this instance. /// public string ToString(IFormatProvider formatProvider) { return string.Format(formatProvider, "X:{0} Y:{1} Z:{2} W:{3}", X, Y, Z, W); } /// /// Returns a that represents this instance. /// /// The format. /// The format provider. /// /// A that represents this instance. /// public string ToString(string format, IFormatProvider formatProvider) { if (format == null) return ToString(formatProvider); return string.Format(formatProvider, "X:{0} Y:{1} Z:{2} W:{3}", X.ToString(format, formatProvider), Y.ToString(format, formatProvider), Z.ToString(format, formatProvider), W.ToString(format, formatProvider)); } /// /// Returns a hash code for this instance. /// /// /// A hash code for this instance, suitable for use in hashing algorithms and data structures like a hash table. /// public override int GetHashCode() { return X.GetHashCode() + Y.GetHashCode() + Z.GetHashCode() + W.GetHashCode(); } /// /// Determines whether the specified is equal to this instance. /// /// The to compare with this instance. /// /// true if the specified is equal to this instance; otherwise, false. /// public bool Equals(Quaternion other) { return ((float)Math.Abs(other.X - X) < MathUtil.ZeroTolerance && (float)Math.Abs(other.Y - Y) < MathUtil.ZeroTolerance && (float)Math.Abs(other.Z - Z) < MathUtil.ZeroTolerance && (float)Math.Abs(other.W - W) < MathUtil.ZeroTolerance); } /// /// Determines whether the specified is equal to this instance. /// /// The to compare with this instance. /// /// true if the specified is equal to this instance; otherwise, false. /// public override bool Equals(object value) { if (value == null) return false; if (value.GetType() != GetType()) return false; return Equals((Quaternion)value); } #if SlimDX1xInterop /// /// Performs an implicit conversion from to . /// /// The value. /// The result of the conversion. public static implicit operator SlimDX.Quaternion(Quaternion value) { return new SlimDX.Quaternion(value.X, value.Y, value.Z, value.W); } /// /// Performs an implicit conversion from to . /// /// The value. /// The result of the conversion. public static implicit operator Quaternion(SlimDX.Quaternion value) { return new Quaternion(value.X, value.Y, value.Z, value.W); } #endif #if WPFInterop /// /// Performs an implicit conversion from to . /// /// The value. /// The result of the conversion. public static implicit operator System.Windows.Media.Media3D.Quaternion(Quaternion value) { return new System.Windows.Media.Media3D.Quaternion(value.X, value.Y, value.Z, value.W); } /// /// Performs an explicit conversion from to . /// /// The value. /// The result of the conversion. public static explicit operator Quaternion(System.Windows.Media.Media3D.Quaternion value) { return new Quaternion((float)value.X, (float)value.Y, (float)value.Z, (float)value.W); } #endif #if XnaInterop /// /// Performs an implicit conversion from to . /// /// The value. /// The result of the conversion. public static implicit operator Microsoft.Xna.Framework.Quaternion(Quaternion value) { return new Microsoft.Xna.Framework.Quaternion(value.X, value.Y, value.Z, value.W); } /// /// Performs an implicit conversion from to . /// /// The value. /// The result of the conversion. public static implicit operator Quaternion(Microsoft.Xna.Framework.Quaternion value) { return new Quaternion(value.X, value.Y, value.Z, value.W); } #endif } }