/Utilities/Datatypes/Quaternion.cs

using System;

using System.Diagnostics;

using System.Globalization;

using System.IO;

using System.Runtime.InteropServices;

using Delta.Utilities.Helpers;

using NUnit.Framework;





namespace Delta.Utilities.Datatypes

{

	/// <summary>

	/// Quaternion, contains X, Y, Z and W values. Mostly used for cameras.

	/// Note: Sometimes used as a replacement for Vector4 because we only have

	/// Point for Vector2 and Vector for Vector3.

	/// </summary>

	[Serializable]

	[StructLayout(LayoutKind.Explicit)]

	[DebuggerDisplay("Quaternion(Vector=({Vector.X}, {Vector.Y}, {Vector.Z})" +

	                 ", W={W})")]

	public struct Quaternion : ISaveLoadBinary, IEquatable<Quaternion>

	{

		#region Constants

		/// <summary>

		/// Represents the size in bytes of a Quaternion (4 * 4 = 16 bytes).

		/// </summary>

		public const int DataSize = 4 * 4;

		#endregion



		#region Static

		/// <summary>

		/// Returns a identity quaternion (all values 0, except W is 1)

		/// </summary>

		public static readonly Quaternion Identity =

			new Quaternion(0, 0, 0, 1);

		#endregion



		#region Framework Union Defines (Public)

		#endregion



		#region Public

		/// <summary>

		/// X value of the Vector.

		/// </summary>

		[FieldOffset(0)]

		public float X;



		/// <summary>

		/// Y value of the Vector.

		/// </summary>

		[FieldOffset(4)]

		public float Y;



		/// <summary>

		/// Z value of the Vector.

		/// </summary>

		[FieldOffset(8)]

		public float Z;



		/// <summary>

		/// W value of the Vector.

		/// </summary>

		[FieldOffset(12)]

		public float W;



		/// <summary>

		/// We can also use X, Y, Z as a vector (same data)

		/// </summary>

		[FieldOffset(0)]

		public Vector Vector;

		#endregion



		#region Constructor

		/// <summary>

		/// Create quaternion

		/// </summary>

		/// <param name="scalarPart">scalarPart</param>

		/// <param name="vectorPart">vectorPart</param>

		public Quaternion(Vector vectorPart, float scalarPart)

			: this()

		{

			Vector = vectorPart;

			W = scalarPart;

		}



		/// <summary>

		/// Create quaternion

		/// </summary>

		/// <param name="setW">setW</param>

		/// <param name="setX">setX</param>

		/// <param name="setY">setY</param>

		/// <param name="setZ">setZ</param>

		public Quaternion(float setX, float setY, float setZ, float setW)

			: this()

		{

			X = setX;

			Y = setY;

			Z = setZ;

			W = setW;

		}

		#endregion



		#region Equals

		/// <summary>

		/// Check if another quaternion has the same values.

		/// </summary>

		/// <param name="other">Other quaternion to compare against.</param>

		/// <returns>True if the other quaternion has the same values.</returns>

		public bool Equals(Quaternion other)

		{

			return this == other;

		}



		/// <summary>

		/// Check if another object is an quaternion and has the same values.

		/// </summary>

		/// <param name="obj">Other object to compare against.</param>

		/// <returns>True if the other quaternion has the same values.</returns>

		public override bool Equals(object obj)

		{

			return obj is Quaternion &&

			       Equals((Quaternion)obj);

		}

		#endregion



		#region GetHashCode

		/// <summary>

		/// Get hash code

		/// </summary>

		/// <returns>Hash code</returns>

		public override int GetHashCode()

		{

			return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode() ^

			       W.GetHashCode();

		}

		#endregion



		#region ToString

		/// <summary>

		/// To string

		/// </summary>

		/// <returns>string</returns>

		public override string ToString()

		{

			CultureInfo culture = CultureInfo.CurrentCulture;

			return "{X:" + X.ToString(culture) +

			       " Y:" + Y.ToString(culture) +

			       " Z:" + Z.ToString(culture) +

			       " W:" + W.ToString(culture) + "}";

		}

		#endregion



		#region Conjugate

		/// <summary>

		/// Conjugate

		/// </summary>

		public void Conjugate()

		{

			X = -X;

			Y = -Y;

			Z = -Z;

		}

		#endregion



		#region CreateFromAxisAngle

		/// <summary>

		/// Create from axis angle

		/// </summary>

		/// <param name="angle">angle</param>

		/// <param name="axis">axis</param>

		/// <returns>Quaternion with the rotation around the axis</returns>

		public static Quaternion CreateFromAxisAngle(Vector axis, float angle)

		{

			// Create the resulting quaternion by applying the angle

			return new Quaternion(

				MathHelper.Sin(angle * 0.5f) * axis,

				MathHelper.Cos(angle * 0.5f));

		}

		#endregion



		#region CreateFromYawPitchRoll

		/// <summary>

		/// Create from yaw pitch roll

		/// </summary>

		/// <param name="pitch">pitch</param>

		/// <param name="roll">roll</param>

		/// <param name="yaw">yaw</param>

		/// <returns>Quaternion with the rotation around yaw, pitch, roll</returns>

		public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch,

			float roll)

		{

			//Note: Still needs to be refactored!

			yaw *= 0.5f;

			pitch *= 0.5f;

			roll *= 0.5f;

			float rollSin = MathHelper.Sin(roll);

			float rollCos = MathHelper.Cos(roll);

			float pitchSin = MathHelper.Sin(pitch);

			float pitchCos = MathHelper.Cos(pitch);

			float yawSin = MathHelper.Sin(yaw);

			float yawCos = MathHelper.Cos(yaw);



			// create the resulting quaternion

			Quaternion result = new Quaternion(

				// X

				((yawCos * pitchSin) * rollCos) + ((yawSin * pitchCos) * rollSin),

				// Y

				((yawCos * pitchCos) * rollSin) - ((yawSin * pitchSin) * rollCos),

				// Z

				((yawSin * pitchCos) * rollCos) - ((yawCos * pitchSin) * rollSin),

				// W

				((yawCos * pitchCos) * rollCos) + ((yawSin * pitchSin) * rollSin));

			return result;

		}

		#endregion



		#region Concatenate

		/// <summary>

		/// Concatenate

		/// </summary>

		/// <param name="value1">Value 1</param>

		/// <param name="value2">Value 2</param>

		/// <returns>Concatenated quaternion</returns>

		public static Quaternion Concatenate(Quaternion value1, Quaternion value2)

		{

			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;



			float num12 = (y2 * z1) - (z2 * y1);

			float num11 = (z2 * x1) - (x2 * z1);

			float num10 = (x2 * y1) - (y2 * x1);

			float num9 = (x2 * x1) + (y2 * y1) + (z2 * z1);



			// create the resulting quaternion

			return new Quaternion(

				// X

				(x2 * w1) + (x1 * w2) + num12,

				// Y

				(y2 * w1) + (y1 * w2) + num11,

				// Z

				(z2 * w1) + (z1 * w2) + num10,

				// W

				(w2 * w1) - num9);

		}

		#endregion



		#region CreateFromRotationMatrix

		/// <summary>

		/// Create from rotation matrix

		/// </summary>

		/// <param name="matrix">matrix</param>

		/// <returns>Quaternion</returns>

		public static Quaternion CreateFromRotationMatrix(Matrix matrix)

		{

			float num8 = matrix.M11 + matrix.M22 + matrix.M33;

			Quaternion quaternion = new Quaternion();

			if (num8 > 0f)

			{

				float num = MathHelper.Sqrt(num8 + 1f);

				quaternion.W = num * 0.5f;

				num = 0.5f / num;

				quaternion.X = (matrix.M23 - matrix.M32) * num;

				quaternion.Y = (matrix.M31 - matrix.M13) * num;

				quaternion.Z = (matrix.M12 - matrix.M21) * num;

				return quaternion;

			}



			if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))

			{

				float num7 = MathHelper.Sqrt(1f + matrix.M11 - matrix.M22 - matrix.M33);

				float num4 = 0.5f / num7;

				quaternion.X = 0.5f * num7;

				quaternion.Y = (matrix.M12 + matrix.M21) * num4;

				quaternion.Z = (matrix.M13 + matrix.M31) * num4;

				quaternion.W = (matrix.M23 - matrix.M32) * num4;

				return quaternion;

			}



			if (matrix.M22 > matrix.M33)

			{

				float num6 = MathHelper.Sqrt(1f + matrix.M22 - matrix.M11 - matrix.M33);

				float num3 = 0.5f / num6;

				quaternion.X = (matrix.M21 + matrix.M12) * num3;

				quaternion.Y = 0.5f * num6;

				quaternion.Z = (matrix.M32 + matrix.M23) * num3;

				quaternion.W = (matrix.M31 - matrix.M13) * num3;

				return quaternion;

			}



			float num5 = MathHelper.Sqrt(1f + matrix.M33 - matrix.M11 - matrix.M22);

			float num2 = 0.5f / num5;

			quaternion.X = (matrix.M31 + matrix.M13) * num2;

			quaternion.Y = (matrix.M32 + matrix.M23) * num2;

			quaternion.Z = 0.5f * num5;

			quaternion.W = (matrix.M12 - matrix.M21) * num2;

			return quaternion;

		}

		#endregion



		#region Length

		/// <summary>

		/// Length, also called Magnitude for Quaternions sometimes.

		/// </summary>

		/// <returns>Length</returns>

		public float Length()

		{

			return MathHelper.Sqrt(X * X + Y * Y + Z * Z + W * W);

		}

		#endregion



		#region LengthSquared

		/// <summary>

		/// Length squared, also called SquareMagnitude for Quaternions sometimes.

		/// </summary>

		/// <returns>Squared length</returns>

		public float LengthSquared()

		{

			return X * X + Y * Y + Z * Z + W * W;

		}

		#endregion



		#region Normalize

		/// <summary>

		/// Normalize this quaternion.

		/// </summary>

		public void Normalize()

		{

			float length = Length();

			if (length < float.Epsilon ||

			    MathHelper.Abs(length - 1.0f) < float.Epsilon)

			{

				return;

			}



			float invertedLength = 1.0f / length;

			Vector *= invertedLength;

			W *= invertedLength;

		}

		#endregion



		#region Invert

		/// <summary>

		/// Invert this quaternion

		/// </summary>

		public void Invert()

		{

			float lengthSquared = LengthSquared();

			if (lengthSquared < float.Epsilon)

			{

				return;

			}



			float invertedLengthSquared = -1.0f / lengthSquared;

			Vector *= invertedLengthSquared;

			W *= -invertedLengthSquared;

		}

		#endregion



		#region Lerp

		/// <summary>

		/// Used to interpolate between two quaternions. This is not just

		/// quat1*amount+quat2*(1-amount) because we need to rotate correctly.

		/// </summary>

		/// <param name="quat1">Value 1</param>

		/// <param name="quat2">Value 2</param>

		/// <param name="amount">Interpolation amount</param>

		/// <returns>Interpolated quaternion</returns>

		public static Quaternion Lerp(Quaternion quat1, Quaternion quat2,

			float amount)

		{

			float cos = Dot(quat1, quat2);

			float sin = MathHelper.Sqrt(MathHelper.Abs(1.0f - cos * cos));

			if (MathHelper.Abs(sin) < float.Epsilon)

			{

				return quat1;

			}



			float angle = MathHelper.Atan(sin, cos);

			float invertedSin = 1.0f / sin;

			float c0 = MathHelper.Sin((1 - amount) * angle) * invertedSin;

			float c1 = MathHelper.Sin(amount * angle) * invertedSin;

			return (quat1 * c0) + (quat2 * c1);

		}

		#endregion



		#region Dot

		/// <summary>

		/// Dot product of two quaternions

		/// </summary>

		/// <param name="quaternion1">Quaternion 1</param>

		/// <param name="quaternion2">Quaternion 2</param>

		/// <returns>Dot product result</returns>

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

		}

		#endregion



		#region Operators



		#region Equation

		/// <summary>

		/// Operator for equality

		/// </summary>

		/// <param name="quaternion1">Quaternion 1</param>

		/// <param name="quaternion2">Quaternion 2</param>

		/// <returns>True if both quaternions are equal</returns>

		public static bool operator ==(Quaternion quaternion1,

			Quaternion quaternion2)

		{

			return (quaternion1.X == quaternion2.X) &&

			       (quaternion1.Y == quaternion2.Y) &&

			       (quaternion1.Z == quaternion2.Z) &&

			       (quaternion1.W == quaternion2.W);

		}



		/// <summary>

		/// Operator for inequality

		/// </summary>

		/// <param name="quaternion1">Quaternion 1</param>

		/// <param name="quaternion2">Quaternion 2</param>

		/// <returns>True if both quaternions are not equal</returns>

		public static bool operator !=(Quaternion quaternion1,

			Quaternion quaternion2)

		{

			return (quaternion1.X != quaternion2.X) ||

			       (quaternion1.Y != quaternion2.Y) ||

			       (quaternion1.Z != quaternion2.Z) ||

			       (quaternion1.W != quaternion2.W);

		}

		#endregion



		#region Negation

		/// <summary>

		/// Operator for unary negation

		/// </summary>

		/// <param name="value">Quaternion to negate</param>

		/// <returns>Negated value</returns>

		public static Quaternion operator -(Quaternion value)

		{

			return new Quaternion(-value.Vector, -value.W);

		}

		#endregion



		#region Addition

		/// <summary>

		/// Operator for addition

		/// </summary>

		/// <param name="value1">Quaternion 1</param>

		/// <param name="value2">Quaternion 2</param>

		/// <returns>Added quaternion</returns>

		public static Quaternion operator +(Quaternion value1, Quaternion value2)

		{

			return new Quaternion(value1.Vector + value2.Vector,

				value1.W + value2.W);

		}

		#endregion



		#region Multiply

		/// <summary>

		/// Operator for multiply

		/// </summary>

		/// <param name="value1">Quaternion 1</param>

		/// <param name="value2">Quaternion 2</param>

		/// <returns>Multiplied quaternion</returns>

		public static Quaternion operator *(Quaternion value1, Quaternion value2)

		{

			return new Quaternion(Vector.Cross(value1.Vector, value2.Vector) +

			                      (value1.W * value2.Vector) + (value1.Vector * value2.W),

				(value1.W * value2.W) - Vector.Dot(value1.Vector, value2.Vector));

		}



		/// <summary>

		/// Operator for multiply

		/// </summary>

		/// <param name="value">Quaternion to multiply with</param>

		/// <param name="scalar">Scalar to multiply with</param>

		/// <returns>Multiplied quaternion</returns>

		public static Quaternion operator *(Quaternion value, float scalar)

		{

			return new Quaternion(value.Vector * scalar, value.W * scalar);

		}



		/// <summary>

		/// Operator for multiply

		/// </summary>

		/// <param name="scalar">Scalar to multiply with</param>

		/// <param name="value">Quaternion to multiply with</param>

		/// <returns>Multiplied quaternion</returns>

		public static Quaternion operator *(float scalar, Quaternion value)

		{

			return new Quaternion(value.Vector * scalar, value.W * scalar);

		}

		#endregion



		#region Division

		/// <summary>

		/// Operator for division

		/// </summary>

		/// <param name="value1">Quaternion 1</param>

		/// <param name="value2">Quaternion 2</param>

		/// <returns>Divided quaternion</returns>

		public static Quaternion operator /(Quaternion value1, Quaternion value2)

		{

			value2.Invert();

			return value1 * value2;

		}



		/// <summary>

		/// Operator for division

		/// </summary>

		/// <param name="value">Quaternion</param>

		/// <param name="scalar">Scalar to divide through</param>

		/// <returns>Divided quaternion</returns>

		public static Quaternion operator /(Quaternion value, float scalar)

		{

			float invertedScalar = 1.0f / scalar;

			return new Quaternion(value.Vector * invertedScalar,

				value.W * invertedScalar);

		}

		#endregion



		#region Cast operators

		/// <summary>

		/// Operator to explicitly convert a quaternion to a matrix.

		/// </summary>

		/// <param name="quat">Quaternion to convert</param>

		/// <returns>Matrix from quaternion</returns>

		public static explicit operator Matrix(Quaternion quat)

		{

			Matrix ret = Matrix.Identity;

			// A cast to a matrix only works with normalized quaternions!

			quat.Normalize();



			float xs = 2.0f * quat.X;

			float ys = 2.0f * quat.Y;

			float zs = 2.0f * quat.Z;



			float wx = quat.W * xs;

			float wy = quat.W * ys;

			float wz = quat.W * zs;

			float xx = quat.X * xs;

			float xy = quat.X * ys;

			float xz = quat.X * zs;

			float yy = quat.Y * ys;

			float yz = quat.Y * zs;

			float zz = quat.Z * zs;



			ret.M11 = 1.0f - yy - zz;

			ret.M21 = xy - wz;

			ret.M31 = xz + wy;

			ret.M41 = 0.0f;



			ret.M12 = xy + wz;

			ret.M22 = 1.0f - xx - zz;

			ret.M32 = yz - wx;

			ret.M42 = 0.0f;



			ret.M13 = xz - wy;

			ret.M23 = yz + wx;

			ret.M33 = 1.0f - xx - yy;

			ret.M43 = 0.0f;



			return ret;

		}

		#endregion



		#endregion



		#region ISaveLoadBinary Methods



		#region Save

		/// <summary>

		/// Saves the point to a stream.

		/// </summary>

		/// <param name="writer">The stream that will be used.</param>

		public void Save(BinaryWriter writer)

		{

			writer.Write(X);

			writer.Write(Y);

			writer.Write(Z);

			writer.Write(W);

		}

		#endregion



		#region Load

		/// <summary>

		/// Load the point values from a stream.

		/// </summary>

		/// <param name="reader">The stream that will be used.</param>

		public void Load(BinaryReader reader)

		{

			X = reader.ReadSingle();

			Y = reader.ReadSingle();

			Z = reader.ReadSingle();

			W = reader.ReadSingle();

		}

		#endregion



		#endregion



		/// <summary>

		/// Tests

		/// </summary>

		internal class QuaternionTests

		{

			#region Helpers

			/// <summary>

			/// Length test

			/// 

			/// Calculation:

			///   10 * 10 =  100

			/// + 40 * 40 = 1600

			/// +  4 *  4 =   16

			/// +  2 *  2 =    4

			/// ----------------

			///             _____

			///           -/1720'

			///               41.47288

			/// </summary>

			public static void Length()

			{

				Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f);

				Assert.NearlyEqual(quat.Length(), 41.47288f);

			}

			#endregion



			#region Equality (Static)

			/// <summary>

			/// Equality test

			/// </summary>

			[Test]

			public static void Equality()

			{

				Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f);

				Quaternion quat1 = new Quaternion(new Vector(10f, 40f, 4f), 2f);

				Quaternion quat2 = new Quaternion(new Vector(5f, 0f, 43f), 21f);



				// using the Equals methods

				Assert.True(quat.Equals(quat));

				Assert.True(quat.Equals(quat1));

				Assert.True(quat2.Equals(quat2));

				Assert.True(quat.Equals((object)quat));



				Assert.False(quat.Equals(quat2));

				Assert.False(quat.Equals(1234));



				// using the operators

				Assert.True(quat1 == quat);

				Assert.True(quat == quat1);

				Assert.True(quat1 != quat2);

				Assert.False(quat1 == quat2);

			}

			#endregion



			#region ArithmeticOperators (Static)

			/// <summary>

			/// ArithmeticOperators test

			/// </summary>

			[Test]

			public static void ArithmeticOperators()

			{

				Quaternion quat1 = new Quaternion(new Vector(10f, 40f, 4f), 2f);

				Quaternion quat2 = new Quaternion(new Vector(5f, 1f, 43f), 21f);



				// Multiply with scalar

				Quaternion result = quat1 * 3f;

				Assert.NearlyEqual(result.X, 30f);

				Assert.NearlyEqual(result.Y, 120f);

				Assert.NearlyEqual(result.Z, 12f);

				Assert.NearlyEqual(result.W, 6f);

				result = 3f * quat1;

				Assert.NearlyEqual(result.X, 30f);

				Assert.NearlyEqual(result.Y, 120f);

				Assert.NearlyEqual(result.Z, 12f);

				Assert.NearlyEqual(result.W, 6f);



				// Multiply two quaternions

				// vec1 = Cross(Vector1, Vector2)

				// vec1.X = (40 * 43) - ( 4 *  1) = 1716

				// vec1.Y = ( 4 *  5) - (10 * 43) = -410

				// vec1.Z = (10 *  1) - (40 *  5) = -190

				// vec2 = (2 * {5, 1, 43}) = {10, 1, 86}

				// vec3 = (21 * {10, 40, 4}) = {210, 840, 84}

				// X = vec1.X + vec2.X + vec3.X = 1936

				// Y = vec1.Y + vec2.Y + vec3.Y = 432

				// Z = vec1.Z + vec2.Z + vec3.Z = -20



				// W = (W1 * W2) - Dot(Vector1, Vector2)

				// W = 42 - (10 * 5 + 40 * 1 + 4 * 43) = -220

				result = quat1 * quat2;

				Assert.NearlyEqual(result.X, 1936f);

				Assert.NearlyEqual(result.Y, 432f);

				Assert.NearlyEqual(result.Z, -20f);

				Assert.NearlyEqual(result.W, -220);



				// Negation of a quaternion

				result = -quat1;

				Assert.NearlyEqual(result.X, -10f);

				Assert.NearlyEqual(result.Y, -40f);

				Assert.NearlyEqual(result.Z, -4f);

				Assert.NearlyEqual(result.W, -2f);



				// Addition of two quternions

				result = quat1 + quat2;

				Assert.NearlyEqual(result.X, 15f);

				Assert.NearlyEqual(result.Y, 41f);

				Assert.NearlyEqual(result.Z, 47f);

				Assert.NearlyEqual(result.W, 23f);



				// Division with scalar

				// invScalar = 1 / 4 = 0.25

				// XYZ = {10, 40, 4} * invScalar = {2.5, 10, 1}

				// W = 2 * invScalar = 0.5

				result = quat1 / 4f;

				Assert.NearlyEqual(result.X, 2.5f);

				Assert.NearlyEqual(result.Y, 10f);

				Assert.NearlyEqual(result.Z, 1f);

				Assert.NearlyEqual(result.W, 0.5f);

			}

			#endregion



			#region LengthSquared (Static)

			/// <summary>

			/// LengthSquared test

			/// 

			/// Calculation:

			///   10 * 10 =  100

			/// + 40 * 40 = 1600

			/// +  4 *  4 =   16

			/// +  2 *  2 =    4

			/// ----------------

			///             1720

			/// </summary>

			[Test]

			public static void LengthSquared()

			{

				Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f);

				Assert.NearlyEqual(quat.LengthSquared(), 1720);

			}

			#endregion



			#region Normalize (Static)

			/// <summary>

			/// Normalize test

			/// 

			/// Calculation:

			/// length = 41.47288 (see test above)

			/// invLength = 1 / 41.47288 = 0.02411

			/// X = 10 * invLength = 0.2411

			/// Y = 40 * invLength = 0.9644

			/// Z =  4 * invLength = 0.09644

			/// W =  2 * invLength = 0.04822

			/// </summary>

			[Test]

			public static void Normalize()

			{

				Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f);

				quat.Normalize();



				Assert.NearlyEqual(quat.X, 0.2411f);

				Assert.NearlyEqual(quat.Y, 0.9644f);

				Assert.NearlyEqual(quat.Z, 0.09644f);

				Assert.NearlyEqual(quat.W, 0.04822f);

			}

			#endregion



			#region Invert (Static)

			/// <summary>

			/// Invert test

			/// 

			/// Calculation:

			/// length = 1720 (see test above)

			/// 

			/// invLength = -1 / 1720 = -0.0005814

			/// X = 10 * invLength = -0.005814

			/// Y = 40 * invLength = -0.023256

			/// Z =  4 * invLength = -0.0023256

			/// W =  2 * -invLength = 0.0011628

			/// </summary>

			[Test]

			public static void Invert()

			{

				Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f);

				quat.Invert();



				Assert.NearlyEqual(quat.X, -0.005814f);

				Assert.NearlyEqual(quat.Y, -0.023256f);

				Assert.NearlyEqual(quat.Z, -0.0023256f);

				Assert.NearlyEqual(quat.W, 0.0011628f);

			}

			#endregion



			#region Dot (Static)

			/// <summary>

			/// Dot test

			/// 

			/// Calculation:

			/// (x1 * x2) + (y1 * y2) + (z1 * z2) + (w1 * w2)

			///   10 *  5 =  50

			/// + 40 *  2 =  80

			/// +  4 * 50 = 200

			/// +  2 * 12 =  24

			/// ---------------

			///             354

			/// </summary>

			[Test]

			public static void Dot()

			{

				Quaternion quat1 = new Quaternion(new Vector(10f, 40f, 4f), 2f);

				Quaternion quat2 = new Quaternion(new Vector(5f, 2f, 50f), 12f);

				float dot = Quaternion.Dot(quat1, quat2);



				Assert.NearlyEqual(dot, 354f);

			}

			#endregion



			#region Lerp (Static)

			/// <summary>

			/// Lerp test, will just interpolate two quaternions.

			/// </summary>

			[Test]

			public static void Lerp()

			{

				Quaternion quat1 = new Quaternion(new Vector(10f, 40f, 4f), 2f);

				Quaternion quat2 = new Quaternion(new Vector(5f, 2f, 50f), 12f);



				Quaternion lerp = Quaternion.Lerp(quat1, quat2, 0.5f);



				Assert.NearlyEqual(lerp.X, 0.01621542f);

				Assert.NearlyEqual(lerp.Y, 0.04540319f);

				Assert.NearlyEqual(lerp.Z, 0.05837553f);

				Assert.NearlyEqual(lerp.W, 0.0151344f);

			}

			#endregion



			#region Conjugate (Static)

			/// <summary>

			/// Conjugate test

			/// </summary>

			[Test]

			public static void Conjugate()

			{

				Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f);

				quat.Conjugate();



				Assert.NearlyEqual(quat.X, -10f);

				Assert.NearlyEqual(quat.Y, -40f);

				Assert.NearlyEqual(quat.Z, -4f);

				Assert.NearlyEqual(quat.W, 2f);

			}

			#endregion



			#region CreateFromRotationMatrix (Static)

			/// <summary>

			/// CreateFromRotationMatrix test

			/// </summary>

			[Test]

			public static void CreateFromRotationMatrix()

			{

				Matrix rotMatrixX = Matrix.CreateRotationX(4f);

				Matrix rotMatrixY = Matrix.CreateRotationY(10f);

				Matrix rotMatrixZ = Matrix.CreateRotationZ(2f);

				Matrix rotMatrixCombined = Matrix.Identity;

				Matrix.Multiply(ref rotMatrixX, ref rotMatrixY,

					ref rotMatrixCombined);

				Matrix.Multiply(ref rotMatrixCombined, ref rotMatrixZ,

					ref rotMatrixCombined);



				Quaternion quat =

					Quaternion.CreateFromRotationMatrix(rotMatrixCombined);



				Assert.NearlyEqual(quat.X, 0.03324125f);

				Assert.NearlyEqual(quat.Y, 0.08769615f);

				Assert.NearlyEqual(quat.Z, 0.01433417f);

				Assert.NearlyEqual(quat.W, 0.9954893f);

			}

			#endregion



			#region CreateFromYawPitchRoll (Static)

			/// <summary>

			/// CreateFromYawPitchRoll test

			/// 

			/// Calculation:

			/// yaw = 40 * 0.5 = 20

			/// pitch = 2 * 0.5 = 1

			/// roll = 238 * 0.5 = 119

			/// 

			/// rollSin = Sin(roll) = 0.342020

			/// rollCos = Cos(roll) = 0.939692

			/// pitchSin = Sin(pitch) = 0.017452

			/// pitchCos = Cos(pitch) = 0.999847

			/// yawSin = Sin(yaw) = 0.874619

			/// yawCos = Cos(yaw) = -0.484809

			/// 

			/// X = ((yawCos * pitchSin) * rollCos) +

			///     ((yawSin * pitchCos) * rollSin)

			///   = ((-0.484809 * 0.017452) * 0.939692) +

			///     ((0.874619 * 0.999847) * 0.342020)

			///   = 0.291140794875046

			/// 

			/// Y = ((yawSin * pitchCos) * rollCos) -

			///     ((yawCos * pitchSin) * rollSin)

			///   = ((0.874619 * 0.999847) * 0.939692) -

			///     ((-0.484809 * 0.017452) * 0.342020)

			///   = 0.824640523317155

			/// 

			/// Z = ((yawCos * pitchCos) * rollSin) -

			///     ((yawSin * pitchSin) * rollCos)

			///   = ((-0.484809 * 0.999847) * 0.342020) -

			///     ((0.874619 * 0.017452) * 0.939692)

			///   = -0.180132323055428

			/// 

			/// W = ((yawCos * pitchCos) * rollCos) +

			///     ((yawSin * pitchSin) * rollSin)

			///   = ((-0.484809 * 0.999847) * 0.939692) +

			///     ((0.874619 * 0.017452) * 0.342020)

			///   = -0.450280894197248

			/// </summary>

			[Test]

			public static void CreateFromYawPitchRoll()

			{

				Quaternion quat = Quaternion.CreateFromYawPitchRoll(40f, 2f, 238f);



				Assert.NearlyEqual(quat.X, 0.291140f);

				Assert.NearlyEqual(quat.Y, 0.824640f);

				Assert.NearlyEqual(quat.Z, -0.180132f);

				Assert.NearlyEqual(quat.W, -0.450280f);

			}

			#endregion



			#region CreateFromAxisAngle (Static)

			/// <summary>

			/// CreateFromAxisAngle test

			/// 

			/// Calculation:

			/// sinAngle = Sin(4.5) = 0.0784590957278449

			/// X = 45 * sinAngle = 3.53065

			/// Y =  2 * sinAngle = 0.1569

			/// Z = 14 * sinAngle = 1.09842

			/// W = Cos(4.5) = 0.99691

			/// </summary>

			[Test]

			public static void CreateFromAxisAngle()

			{

				Quaternion quat = Quaternion.CreateFromAxisAngle(

					new Vector(45f, 2f, 14f), 9f);



				Assert.NearlyEqual(quat.X, 3.53065f);

				Assert.NearlyEqual(quat.Y, 0.1569f);

				Assert.NearlyEqual(quat.Z, 1.09842f);

				Assert.NearlyEqual(quat.W, 0.99691f);

			}

			#endregion



			#region SizeOf

			/// <summary>

			/// Checks if the size of Point is exactly 8 bytes (2 floats: X and Y)

			/// </summary>

			[Test]

			public void SizeOf()

			{

				// Quaternion has 4 floats: X, Y, Z, and W

				Assert.Equal(4 * 4, Marshal.SizeOf(typeof(Quaternion)));

			}

			#endregion

		}

	}

}