// Copyright (c) Microsoft. All rights reserved. // Licensed under the MIT license. See LICENSE file in the project root for full license information. using System.Globalization; namespace System.Numerics { /// /// A structure encapsulating a four-dimensional vector (x,y,z,w), /// which is used to efficiently rotate an object about the (x,y,z) vector by the angle theta, where w = cos(theta/2). /// public struct Quaternion : IEquatable { /// /// Specifies the X-value of the vector component of the Quaternion. /// public float X; /// /// Specifies the Y-value of the vector component of the Quaternion. /// public float Y; /// /// Specifies the Z-value of the vector component of the Quaternion. /// public float Z; /// /// Specifies the rotation component of the Quaternion. /// public float W; /// /// Returns a Quaternion representing no rotation. /// public static Quaternion Identity { get { return new Quaternion(0, 0, 0, 1); } } /// /// Returns whether the Quaternion is the identity Quaternion. /// public bool IsIdentity { get { return X == 0f && Y == 0f && Z == 0f && W == 1f; } } /// /// Constructs a Quaternion from the given components. /// /// The X component of the Quaternion. /// The Y component of the Quaternion. /// The Z component of the Quaternion. /// The W component of the Quaternion. 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 from the given vector and rotation parts. /// /// The vector part of the Quaternion. /// The rotation part of the Quaternion. public Quaternion(Vector3 vectorPart, float scalarPart) { X = vectorPart.X; Y = vectorPart.Y; Z = vectorPart.Z; W = scalarPart; } /// /// Calculates the length of the Quaternion. /// /// The computed length of the Quaternion. public float Length() { float ls = X * X + Y * Y + Z * Z + W * W; return (float)Math.Sqrt((double)ls); } /// /// Calculates the length squared of the Quaternion. This operation is cheaper than Length(). /// /// The length squared of the Quaternion. public float LengthSquared() { return X * X + Y * Y + Z * Z + W * W; } /// /// Divides each component of the Quaternion by the length of the Quaternion. /// /// The source Quaternion. /// The normalized Quaternion. public static Quaternion Normalize(Quaternion value) { Quaternion ans; float ls = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W; float invNorm = 1.0f / (float)Math.Sqrt((double)ls); ans.X = value.X * invNorm; ans.Y = value.Y * invNorm; ans.Z = value.Z * invNorm; ans.W = value.W * invNorm; return ans; } /// /// Creates the conjugate of a specified Quaternion. /// /// The Quaternion of which to return the conjugate. /// A new Quaternion that is the conjugate of the specified one. public static Quaternion Conjugate(Quaternion value) { Quaternion ans; ans.X = -value.X; ans.Y = -value.Y; ans.Z = -value.Z; ans.W = value.W; return ans; } /// /// Returns the inverse of a Quaternion. /// /// The source Quaternion. /// The inverted Quaternion. public static Quaternion Inverse(Quaternion value) { // -1 ( a -v ) // q = ( ------------- ------------- ) // ( a^2 + |v|^2 , a^2 + |v|^2 ) Quaternion ans; float ls = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W; float invNorm = 1.0f / ls; ans.X = -value.X * invNorm; ans.Y = -value.Y * invNorm; ans.Z = -value.Z * invNorm; ans.W = value.W * invNorm; return ans; } /// /// Creates a Quaternion from a vector and an angle to rotate about the vector. /// /// The vector to rotate around. /// The angle, in radians, to rotate around the vector. /// The created Quaternion. public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle) { Quaternion ans; float halfAngle = angle * 0.5f; float s = (float)Math.Sin(halfAngle); float c = (float)Math.Cos(halfAngle); ans.X = axis.X * s; ans.Y = axis.Y * s; ans.Z = axis.Z * s; ans.W = c; return ans; } /// /// Creates a new Quaternion from the given yaw, pitch, and roll, in radians. /// /// The yaw angle, in radians, around the Y-axis. /// The pitch angle, in radians, around the X-axis. /// The roll angle, in radians, around the Z-axis. /// public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll) { // Roll first, about axis the object is facing, then // pitch upward, then yaw to face into the new heading float sr, cr, sp, cp, sy, cy; float halfRoll = roll * 0.5f; sr = (float)Math.Sin(halfRoll); cr = (float)Math.Cos(halfRoll); float halfPitch = pitch * 0.5f; sp = (float)Math.Sin(halfPitch); cp = (float)Math.Cos(halfPitch); float halfYaw = yaw * 0.5f; sy = (float)Math.Sin(halfYaw); cy = (float)Math.Cos(halfYaw); Quaternion result; result.X = cy * sp * cr + sy * cp * sr; result.Y = sy * cp * cr - cy * sp * sr; result.Z = cy * cp * sr - sy * sp * cr; result.W = cy * cp * cr + sy * sp * sr; return result; } /// /// Creates a Quaternion from the given rotation matrix. /// /// The rotation matrix. /// The created Quaternion. public static Quaternion CreateFromRotationMatrix(Matrix4x4 matrix) { float trace = matrix.M11 + matrix.M22 + matrix.M33; Quaternion q = new Quaternion(); if (trace > 0.0f) { float s = (float)Math.Sqrt(trace + 1.0f); q.W = s * 0.5f; s = 0.5f / s; q.X = (matrix.M23 - matrix.M32) * s; q.Y = (matrix.M31 - matrix.M13) * s; q.Z = (matrix.M12 - matrix.M21) * s; } else { if (matrix.M11 >= matrix.M22 && matrix.M11 >= matrix.M33) { float s = (float)Math.Sqrt(1.0f + matrix.M11 - matrix.M22 - matrix.M33); float invS = 0.5f / s; q.X = 0.5f * s; q.Y = (matrix.M12 + matrix.M21) * invS; q.Z = (matrix.M13 + matrix.M31) * invS; q.W = (matrix.M23 - matrix.M32) * invS; } else if (matrix.M22 > matrix.M33) { float s = (float)Math.Sqrt(1.0f + matrix.M22 - matrix.M11 - matrix.M33); float invS = 0.5f / s; q.X = (matrix.M21 + matrix.M12) * invS; q.Y = 0.5f * s; q.Z = (matrix.M32 + matrix.M23) * invS; q.W = (matrix.M31 - matrix.M13) * invS; } else { float s = (float)Math.Sqrt(1.0f + matrix.M33 - matrix.M11 - matrix.M22); float invS = 0.5f / s; q.X = (matrix.M31 + matrix.M13) * invS; q.Y = (matrix.M32 + matrix.M23) * invS; q.Z = 0.5f * s; q.W = (matrix.M12 - matrix.M21) * invS; } } return q; } /// /// Calculates the dot product of two Quaternions. /// /// The first source Quaternion. /// The second source Quaternion. /// The dot product of the 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; } /// /// Interpolates between two quaternions, using spherical linear interpolation. /// /// The first source Quaternion. /// The second source Quaternion. /// The relative weight of the second source Quaternion in the interpolation. /// The interpolated Quaternion. public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount) { const float epsilon = 1e-6f; float t = amount; float cosOmega = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W; bool flip = false; if (cosOmega < 0.0f) { flip = true; cosOmega = -cosOmega; } float s1, s2; if (cosOmega > (1.0f - epsilon)) { // Too close, do straight linear interpolation. s1 = 1.0f - t; s2 = (flip) ? -t : t; } else { float omega = (float)Math.Acos(cosOmega); float invSinOmega = (float)(1 / Math.Sin(omega)); s1 = (float)Math.Sin((1.0f - t) * omega) * invSinOmega; s2 = (flip) ? (float)-Math.Sin(t * omega) * invSinOmega : (float)Math.Sin(t * omega) * invSinOmega; } Quaternion ans; ans.X = s1 * quaternion1.X + s2 * quaternion2.X; ans.Y = s1 * quaternion1.Y + s2 * quaternion2.Y; ans.Z = s1 * quaternion1.Z + s2 * quaternion2.Z; ans.W = s1 * quaternion1.W + s2 * quaternion2.W; return ans; } /// /// Linearly interpolates between two quaternions. /// /// The first source Quaternion. /// The second source Quaternion. /// The relative weight of the second source Quaternion in the interpolation. /// The interpolated Quaternion. public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, float amount) { float t = amount; float t1 = 1.0f - t; Quaternion r = new Quaternion(); float dot = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y + quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W; if (dot >= 0.0f) { r.X = t1 * quaternion1.X + t * quaternion2.X; r.Y = t1 * quaternion1.Y + t * quaternion2.Y; r.Z = t1 * quaternion1.Z + t * quaternion2.Z; r.W = t1 * quaternion1.W + t * quaternion2.W; } else { r.X = t1 * quaternion1.X - t * quaternion2.X; r.Y = t1 * quaternion1.Y - t * quaternion2.Y; r.Z = t1 * quaternion1.Z - t * quaternion2.Z; r.W = t1 * quaternion1.W - t * quaternion2.W; } // Normalize it. float ls = r.X * r.X + r.Y * r.Y + r.Z * r.Z + r.W * r.W; float invNorm = 1.0f / (float)Math.Sqrt((double)ls); r.X *= invNorm; r.Y *= invNorm; r.Z *= invNorm; r.W *= invNorm; return r; } /// /// Concatenates two Quaternions; the result represents the value1 rotation followed by the value2 rotation. /// /// The first Quaternion rotation in the series. /// The second Quaternion rotation in the series. /// A new Quaternion representing the concatenation of the value1 rotation followed by the value2 rotation. public static Quaternion Concatenate(Quaternion value1, Quaternion value2) { Quaternion ans; // Concatenate rotation is actually q2 * q1 instead of q1 * q2. // So that's why value2 goes q1 and value1 goes q2. float q1x = value2.X; float q1y = value2.Y; float q1z = value2.Z; float q1w = value2.W; float q2x = value1.X; float q2y = value1.Y; float q2z = value1.Z; float q2w = value1.W; // cross(av, bv) float cx = q1y * q2z - q1z * q2y; float cy = q1z * q2x - q1x * q2z; float cz = q1x * q2y - q1y * q2x; float dot = q1x * q2x + q1y * q2y + q1z * q2z; ans.X = q1x * q2w + q2x * q1w + cx; ans.Y = q1y * q2w + q2y * q1w + cy; ans.Z = q1z * q2w + q2z * q1w + cz; ans.W = q1w * q2w - dot; return ans; } /// /// Flips the sign of each component of the quaternion. /// /// The source Quaternion. /// The negated Quaternion. public static Quaternion Negate(Quaternion value) { Quaternion ans; ans.X = -value.X; ans.Y = -value.Y; ans.Z = -value.Z; ans.W = -value.W; return ans; } /// /// Adds two Quaternions element-by-element. /// /// The first source Quaternion. /// The second source Quaternion. /// The result of adding the Quaternions. public static Quaternion Add(Quaternion value1, Quaternion value2) { Quaternion ans; ans.X = value1.X + value2.X; ans.Y = value1.Y + value2.Y; ans.Z = value1.Z + value2.Z; ans.W = value1.W + value2.W; return ans; } /// /// Subtracts one Quaternion from another. /// /// The first source Quaternion. /// The second Quaternion, to be subtracted from the first. /// The result of the subtraction. public static Quaternion Subtract(Quaternion value1, Quaternion value2) { Quaternion ans; ans.X = value1.X - value2.X; ans.Y = value1.Y - value2.Y; ans.Z = value1.Z - value2.Z; ans.W = value1.W - value2.W; return ans; } /// /// Multiplies two Quaternions together. /// /// The Quaternion on the left side of the multiplication. /// The Quaternion on the right side of the multiplication. /// The result of the multiplication. public static Quaternion Multiply(Quaternion value1, Quaternion value2) { Quaternion ans; float q1x = value1.X; float q1y = value1.Y; float q1z = value1.Z; float q1w = value1.W; float q2x = value2.X; float q2y = value2.Y; float q2z = value2.Z; float q2w = value2.W; // cross(av, bv) float cx = q1y * q2z - q1z * q2y; float cy = q1z * q2x - q1x * q2z; float cz = q1x * q2y - q1y * q2x; float dot = q1x * q2x + q1y * q2y + q1z * q2z; ans.X = q1x * q2w + q2x * q1w + cx; ans.Y = q1y * q2w + q2y * q1w + cy; ans.Z = q1z * q2w + q2z * q1w + cz; ans.W = q1w * q2w - dot; return ans; } /// /// Multiplies a Quaternion by a scalar value. /// /// The source Quaternion. /// The scalar value. /// The result of the multiplication. public static Quaternion Multiply(Quaternion value1, float value2) { Quaternion ans; ans.X = value1.X * value2; ans.Y = value1.Y * value2; ans.Z = value1.Z * value2; ans.W = value1.W * value2; return ans; } /// /// Divides a Quaternion by another Quaternion. /// /// The source Quaternion. /// The divisor. /// The result of the division. public static Quaternion Divide(Quaternion value1, Quaternion value2) { Quaternion ans; float q1x = value1.X; float q1y = value1.Y; float q1z = value1.Z; float q1w = value1.W; //------------------------------------- // Inverse part. float ls = value2.X * value2.X + value2.Y * value2.Y + value2.Z * value2.Z + value2.W * value2.W; float invNorm = 1.0f / ls; float q2x = -value2.X * invNorm; float q2y = -value2.Y * invNorm; float q2z = -value2.Z * invNorm; float q2w = value2.W * invNorm; //------------------------------------- // Multiply part. // cross(av, bv) float cx = q1y * q2z - q1z * q2y; float cy = q1z * q2x - q1x * q2z; float cz = q1x * q2y - q1y * q2x; float dot = q1x * q2x + q1y * q2y + q1z * q2z; ans.X = q1x * q2w + q2x * q1w + cx; ans.Y = q1y * q2w + q2y * q1w + cy; ans.Z = q1z * q2w + q2z * q1w + cz; ans.W = q1w * q2w - dot; return ans; } /// /// Flips the sign of each component of the quaternion. /// /// The source Quaternion. /// The negated Quaternion. public static Quaternion operator -(Quaternion value) { Quaternion ans; ans.X = -value.X; ans.Y = -value.Y; ans.Z = -value.Z; ans.W = -value.W; return ans; } /// /// Adds two Quaternions element-by-element. /// /// The first source Quaternion. /// The second source Quaternion. /// The result of adding the Quaternions. public static Quaternion operator +(Quaternion value1, Quaternion value2) { Quaternion ans; ans.X = value1.X + value2.X; ans.Y = value1.Y + value2.Y; ans.Z = value1.Z + value2.Z; ans.W = value1.W + value2.W; return ans; } /// /// Subtracts one Quaternion from another. /// /// The first source Quaternion. /// The second Quaternion, to be subtracted from the first. /// The result of the subtraction. public static Quaternion operator -(Quaternion value1, Quaternion value2) { Quaternion ans; ans.X = value1.X - value2.X; ans.Y = value1.Y - value2.Y; ans.Z = value1.Z - value2.Z; ans.W = value1.W - value2.W; return ans; } /// /// Multiplies two Quaternions together. /// /// The Quaternion on the left side of the multiplication. /// The Quaternion on the right side of the multiplication. /// The result of the multiplication. public static Quaternion operator *(Quaternion value1, Quaternion value2) { Quaternion ans; float q1x = value1.X; float q1y = value1.Y; float q1z = value1.Z; float q1w = value1.W; float q2x = value2.X; float q2y = value2.Y; float q2z = value2.Z; float q2w = value2.W; // cross(av, bv) float cx = q1y * q2z - q1z * q2y; float cy = q1z * q2x - q1x * q2z; float cz = q1x * q2y - q1y * q2x; float dot = q1x * q2x + q1y * q2y + q1z * q2z; ans.X = q1x * q2w + q2x * q1w + cx; ans.Y = q1y * q2w + q2y * q1w + cy; ans.Z = q1z * q2w + q2z * q1w + cz; ans.W = q1w * q2w - dot; return ans; } /// /// Multiplies a Quaternion by a scalar value. /// /// The source Quaternion. /// The scalar value. /// The result of the multiplication. public static Quaternion operator *(Quaternion value1, float value2) { Quaternion ans; ans.X = value1.X * value2; ans.Y = value1.Y * value2; ans.Z = value1.Z * value2; ans.W = value1.W * value2; return ans; } /// /// Divides a Quaternion by another Quaternion. /// /// The source Quaternion. /// The divisor. /// The result of the division. public static Quaternion operator /(Quaternion value1, Quaternion value2) { Quaternion ans; float q1x = value1.X; float q1y = value1.Y; float q1z = value1.Z; float q1w = value1.W; //------------------------------------- // Inverse part. float ls = value2.X * value2.X + value2.Y * value2.Y + value2.Z * value2.Z + value2.W * value2.W; float invNorm = 1.0f / ls; float q2x = -value2.X * invNorm; float q2y = -value2.Y * invNorm; float q2z = -value2.Z * invNorm; float q2w = value2.W * invNorm; //------------------------------------- // Multiply part. // cross(av, bv) float cx = q1y * q2z - q1z * q2y; float cy = q1z * q2x - q1x * q2z; float cz = q1x * q2y - q1y * q2x; float dot = q1x * q2x + q1y * q2y + q1z * q2z; ans.X = q1x * q2w + q2x * q1w + cx; ans.Y = q1y * q2w + q2y * q1w + cy; ans.Z = q1z * q2w + q2z * q1w + cz; ans.W = q1w * q2w - dot; return ans; } /// /// Returns a boolean indicating whether the two given Quaternions are equal. /// /// The first Quaternion to compare. /// The second Quaternion to compare. /// True if the Quaternions are equal; False otherwise. public static bool operator ==(Quaternion value1, Quaternion value2) { return (value1.X == value2.X && value1.Y == value2.Y && value1.Z == value2.Z && value1.W == value2.W); } /// /// Returns a boolean indicating whether the two given Quaternions are not equal. /// /// The first Quaternion to compare. /// The second Quaternion to compare. /// True if the Quaternions are not equal; False if they are equal. public static bool operator !=(Quaternion value1, Quaternion value2) { return (value1.X != value2.X || value1.Y != value2.Y || value1.Z != value2.Z || value1.W != value2.W); } /// /// Returns a boolean indicating whether the given Quaternion is equal to this Quaternion instance. /// /// The Quaternion to compare this instance to. /// True if the other Quaternion is equal to this instance; False otherwise. public bool Equals(Quaternion other) { return (X == other.X && Y == other.Y && Z == other.Z && W == other.W); } /// /// Returns a boolean indicating whether the given Object is equal to this Quaternion instance. /// /// The Object to compare against. /// True if the Object is equal to this Quaternion; False otherwise. public override bool Equals(object obj) { if (obj is Quaternion) { return Equals((Quaternion)obj); } return false; } /// /// Returns a String representing this Quaternion instance. /// /// The string representation. public override string ToString() { CultureInfo ci = CultureInfo.CurrentCulture; return String.Format(ci, "{{X:{0} Y:{1} Z:{2} W:{3}}}", X.ToString(ci), Y.ToString(ci), Z.ToString(ci), W.ToString(ci)); } /// /// Returns the hash code for this instance. /// /// The hash code. public override int GetHashCode() { return X.GetHashCode() + Y.GetHashCode() + Z.GetHashCode() + W.GetHashCode(); } } }