PageRenderTime 47ms CodeModel.GetById 22ms app.highlight 19ms RepoModel.GetById 1ms app.codeStats 0ms

/Utilities/Datatypes/Quaternion.cs

#
C# | 979 lines | 548 code | 90 blank | 341 comment | 20 complexity | 904220030f204be120611f8cd6a1a4ba MD5 | raw file
  1using System;
  2using System.Diagnostics;
  3using System.Globalization;
  4using System.IO;
  5using System.Runtime.InteropServices;
  6using Delta.Utilities.Helpers;
  7using NUnit.Framework;
  8
  9
 10namespace Delta.Utilities.Datatypes
 11{
 12	/// <summary>
 13	/// Quaternion, contains X, Y, Z and W values. Mostly used for cameras.
 14	/// Note: Sometimes used as a replacement for Vector4 because we only have
 15	/// Point for Vector2 and Vector for Vector3.
 16	/// </summary>
 17	[Serializable]
 18	[StructLayout(LayoutKind.Explicit)]
 19	[DebuggerDisplay("Quaternion(Vector=({Vector.X}, {Vector.Y}, {Vector.Z})" +
 20	                 ", W={W})")]
 21	public struct Quaternion : ISaveLoadBinary, IEquatable<Quaternion>
 22	{
 23		#region Constants
 24		/// <summary>
 25		/// Represents the size in bytes of a Quaternion (4 * 4 = 16 bytes).
 26		/// </summary>
 27		public const int DataSize = 4 * 4;
 28		#endregion
 29
 30		#region Static
 31		/// <summary>
 32		/// Returns a identity quaternion (all values 0, except W is 1)
 33		/// </summary>
 34		public static readonly Quaternion Identity =
 35			new Quaternion(0, 0, 0, 1);
 36		#endregion
 37
 38		#region Framework Union Defines (Public)
 39		#endregion
 40
 41		#region Public
 42		/// <summary>
 43		/// X value of the Vector.
 44		/// </summary>
 45		[FieldOffset(0)]
 46		public float X;
 47
 48		/// <summary>
 49		/// Y value of the Vector.
 50		/// </summary>
 51		[FieldOffset(4)]
 52		public float Y;
 53
 54		/// <summary>
 55		/// Z value of the Vector.
 56		/// </summary>
 57		[FieldOffset(8)]
 58		public float Z;
 59
 60		/// <summary>
 61		/// W value of the Vector.
 62		/// </summary>
 63		[FieldOffset(12)]
 64		public float W;
 65
 66		/// <summary>
 67		/// We can also use X, Y, Z as a vector (same data)
 68		/// </summary>
 69		[FieldOffset(0)]
 70		public Vector Vector;
 71		#endregion
 72
 73		#region Constructor
 74		/// <summary>
 75		/// Create quaternion
 76		/// </summary>
 77		/// <param name="scalarPart">scalarPart</param>
 78		/// <param name="vectorPart">vectorPart</param>
 79		public Quaternion(Vector vectorPart, float scalarPart)
 80			: this()
 81		{
 82			Vector = vectorPart;
 83			W = scalarPart;
 84		}
 85
 86		/// <summary>
 87		/// Create quaternion
 88		/// </summary>
 89		/// <param name="setW">setW</param>
 90		/// <param name="setX">setX</param>
 91		/// <param name="setY">setY</param>
 92		/// <param name="setZ">setZ</param>
 93		public Quaternion(float setX, float setY, float setZ, float setW)
 94			: this()
 95		{
 96			X = setX;
 97			Y = setY;
 98			Z = setZ;
 99			W = setW;
100		}
101		#endregion
102
103		#region Equals
104		/// <summary>
105		/// Check if another quaternion has the same values.
106		/// </summary>
107		/// <param name="other">Other quaternion to compare against.</param>
108		/// <returns>True if the other quaternion has the same values.</returns>
109		public bool Equals(Quaternion other)
110		{
111			return this == other;
112		}
113
114		/// <summary>
115		/// Check if another object is an quaternion and has the same values.
116		/// </summary>
117		/// <param name="obj">Other object to compare against.</param>
118		/// <returns>True if the other quaternion has the same values.</returns>
119		public override bool Equals(object obj)
120		{
121			return obj is Quaternion &&
122			       Equals((Quaternion)obj);
123		}
124		#endregion
125
126		#region GetHashCode
127		/// <summary>
128		/// Get hash code
129		/// </summary>
130		/// <returns>Hash code</returns>
131		public override int GetHashCode()
132		{
133			return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode() ^
134			       W.GetHashCode();
135		}
136		#endregion
137
138		#region ToString
139		/// <summary>
140		/// To string
141		/// </summary>
142		/// <returns>string</returns>
143		public override string ToString()
144		{
145			CultureInfo culture = CultureInfo.CurrentCulture;
146			return "{X:" + X.ToString(culture) +
147			       " Y:" + Y.ToString(culture) +
148			       " Z:" + Z.ToString(culture) +
149			       " W:" + W.ToString(culture) + "}";
150		}
151		#endregion
152
153		#region Conjugate
154		/// <summary>
155		/// Conjugate
156		/// </summary>
157		public void Conjugate()
158		{
159			X = -X;
160			Y = -Y;
161			Z = -Z;
162		}
163		#endregion
164
165		#region CreateFromAxisAngle
166		/// <summary>
167		/// Create from axis angle
168		/// </summary>
169		/// <param name="angle">angle</param>
170		/// <param name="axis">axis</param>
171		/// <returns>Quaternion with the rotation around the axis</returns>
172		public static Quaternion CreateFromAxisAngle(Vector axis, float angle)
173		{
174			// Create the resulting quaternion by applying the angle
175			return new Quaternion(
176				MathHelper.Sin(angle * 0.5f) * axis,
177				MathHelper.Cos(angle * 0.5f));
178		}
179		#endregion
180
181		#region CreateFromYawPitchRoll
182		/// <summary>
183		/// Create from yaw pitch roll
184		/// </summary>
185		/// <param name="pitch">pitch</param>
186		/// <param name="roll">roll</param>
187		/// <param name="yaw">yaw</param>
188		/// <returns>Quaternion with the rotation around yaw, pitch, roll</returns>
189		public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch,
190			float roll)
191		{
192			//Note: Still needs to be refactored!
193			yaw *= 0.5f;
194			pitch *= 0.5f;
195			roll *= 0.5f;
196			float rollSin = MathHelper.Sin(roll);
197			float rollCos = MathHelper.Cos(roll);
198			float pitchSin = MathHelper.Sin(pitch);
199			float pitchCos = MathHelper.Cos(pitch);
200			float yawSin = MathHelper.Sin(yaw);
201			float yawCos = MathHelper.Cos(yaw);
202
203			// create the resulting quaternion
204			Quaternion result = new Quaternion(
205				// X
206				((yawCos * pitchSin) * rollCos) + ((yawSin * pitchCos) * rollSin),
207				// Y
208				((yawCos * pitchCos) * rollSin) - ((yawSin * pitchSin) * rollCos),
209				// Z
210				((yawSin * pitchCos) * rollCos) - ((yawCos * pitchSin) * rollSin),
211				// W
212				((yawCos * pitchCos) * rollCos) + ((yawSin * pitchSin) * rollSin));
213			return result;
214		}
215		#endregion
216
217		#region Concatenate
218		/// <summary>
219		/// Concatenate
220		/// </summary>
221		/// <param name="value1">Value 1</param>
222		/// <param name="value2">Value 2</param>
223		/// <returns>Concatenated quaternion</returns>
224		public static Quaternion Concatenate(Quaternion value1, Quaternion value2)
225		{
226			float x1 = value1.X;
227			float y1 = value1.Y;
228			float z1 = value1.Z;
229			float w1 = value1.W;
230
231			float x2 = value2.X;
232			float y2 = value2.Y;
233			float z2 = value2.Z;
234			float w2 = value2.W;
235
236			float num12 = (y2 * z1) - (z2 * y1);
237			float num11 = (z2 * x1) - (x2 * z1);
238			float num10 = (x2 * y1) - (y2 * x1);
239			float num9 = (x2 * x1) + (y2 * y1) + (z2 * z1);
240
241			// create the resulting quaternion
242			return new Quaternion(
243				// X
244				(x2 * w1) + (x1 * w2) + num12,
245				// Y
246				(y2 * w1) + (y1 * w2) + num11,
247				// Z
248				(z2 * w1) + (z1 * w2) + num10,
249				// W
250				(w2 * w1) - num9);
251		}
252		#endregion
253
254		#region CreateFromRotationMatrix
255		/// <summary>
256		/// Create from rotation matrix
257		/// </summary>
258		/// <param name="matrix">matrix</param>
259		/// <returns>Quaternion</returns>
260		public static Quaternion CreateFromRotationMatrix(Matrix matrix)
261		{
262			float num8 = matrix.M11 + matrix.M22 + matrix.M33;
263			Quaternion quaternion = new Quaternion();
264			if (num8 > 0f)
265			{
266				float num = MathHelper.Sqrt(num8 + 1f);
267				quaternion.W = num * 0.5f;
268				num = 0.5f / num;
269				quaternion.X = (matrix.M23 - matrix.M32) * num;
270				quaternion.Y = (matrix.M31 - matrix.M13) * num;
271				quaternion.Z = (matrix.M12 - matrix.M21) * num;
272				return quaternion;
273			}
274
275			if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
276			{
277				float num7 = MathHelper.Sqrt(1f + matrix.M11 - matrix.M22 - matrix.M33);
278				float num4 = 0.5f / num7;
279				quaternion.X = 0.5f * num7;
280				quaternion.Y = (matrix.M12 + matrix.M21) * num4;
281				quaternion.Z = (matrix.M13 + matrix.M31) * num4;
282				quaternion.W = (matrix.M23 - matrix.M32) * num4;
283				return quaternion;
284			}
285
286			if (matrix.M22 > matrix.M33)
287			{
288				float num6 = MathHelper.Sqrt(1f + matrix.M22 - matrix.M11 - matrix.M33);
289				float num3 = 0.5f / num6;
290				quaternion.X = (matrix.M21 + matrix.M12) * num3;
291				quaternion.Y = 0.5f * num6;
292				quaternion.Z = (matrix.M32 + matrix.M23) * num3;
293				quaternion.W = (matrix.M31 - matrix.M13) * num3;
294				return quaternion;
295			}
296
297			float num5 = MathHelper.Sqrt(1f + matrix.M33 - matrix.M11 - matrix.M22);
298			float num2 = 0.5f / num5;
299			quaternion.X = (matrix.M31 + matrix.M13) * num2;
300			quaternion.Y = (matrix.M32 + matrix.M23) * num2;
301			quaternion.Z = 0.5f * num5;
302			quaternion.W = (matrix.M12 - matrix.M21) * num2;
303			return quaternion;
304		}
305		#endregion
306
307		#region Length
308		/// <summary>
309		/// Length, also called Magnitude for Quaternions sometimes.
310		/// </summary>
311		/// <returns>Length</returns>
312		public float Length()
313		{
314			return MathHelper.Sqrt(X * X + Y * Y + Z * Z + W * W);
315		}
316		#endregion
317
318		#region LengthSquared
319		/// <summary>
320		/// Length squared, also called SquareMagnitude for Quaternions sometimes.
321		/// </summary>
322		/// <returns>Squared length</returns>
323		public float LengthSquared()
324		{
325			return X * X + Y * Y + Z * Z + W * W;
326		}
327		#endregion
328
329		#region Normalize
330		/// <summary>
331		/// Normalize this quaternion.
332		/// </summary>
333		public void Normalize()
334		{
335			float length = Length();
336			if (length < float.Epsilon ||
337			    MathHelper.Abs(length - 1.0f) < float.Epsilon)
338			{
339				return;
340			}
341
342			float invertedLength = 1.0f / length;
343			Vector *= invertedLength;
344			W *= invertedLength;
345		}
346		#endregion
347
348		#region Invert
349		/// <summary>
350		/// Invert this quaternion
351		/// </summary>
352		public void Invert()
353		{
354			float lengthSquared = LengthSquared();
355			if (lengthSquared < float.Epsilon)
356			{
357				return;
358			}
359
360			float invertedLengthSquared = -1.0f / lengthSquared;
361			Vector *= invertedLengthSquared;
362			W *= -invertedLengthSquared;
363		}
364		#endregion
365
366		#region Lerp
367		/// <summary>
368		/// Used to interpolate between two quaternions. This is not just
369		/// quat1*amount+quat2*(1-amount) because we need to rotate correctly.
370		/// </summary>
371		/// <param name="quat1">Value 1</param>
372		/// <param name="quat2">Value 2</param>
373		/// <param name="amount">Interpolation amount</param>
374		/// <returns>Interpolated quaternion</returns>
375		public static Quaternion Lerp(Quaternion quat1, Quaternion quat2,
376			float amount)
377		{
378			float cos = Dot(quat1, quat2);
379			float sin = MathHelper.Sqrt(MathHelper.Abs(1.0f - cos * cos));
380			if (MathHelper.Abs(sin) < float.Epsilon)
381			{
382				return quat1;
383			}
384
385			float angle = MathHelper.Atan(sin, cos);
386			float invertedSin = 1.0f / sin;
387			float c0 = MathHelper.Sin((1 - amount) * angle) * invertedSin;
388			float c1 = MathHelper.Sin(amount * angle) * invertedSin;
389			return (quat1 * c0) + (quat2 * c1);
390		}
391		#endregion
392
393		#region Dot
394		/// <summary>
395		/// Dot product of two quaternions
396		/// </summary>
397		/// <param name="quaternion1">Quaternion 1</param>
398		/// <param name="quaternion2">Quaternion 2</param>
399		/// <returns>Dot product result</returns>
400		public static float Dot(Quaternion quaternion1, Quaternion quaternion2)
401		{
402			return (quaternion1.X * quaternion2.X) +
403			       (quaternion1.Y * quaternion2.Y) +
404			       (quaternion1.Z * quaternion2.Z) +
405			       (quaternion1.W * quaternion2.W);
406		}
407		#endregion
408
409		#region Operators
410
411		#region Equation
412		/// <summary>
413		/// Operator for equality
414		/// </summary>
415		/// <param name="quaternion1">Quaternion 1</param>
416		/// <param name="quaternion2">Quaternion 2</param>
417		/// <returns>True if both quaternions are equal</returns>
418		public static bool operator ==(Quaternion quaternion1,
419			Quaternion quaternion2)
420		{
421			return (quaternion1.X == quaternion2.X) &&
422			       (quaternion1.Y == quaternion2.Y) &&
423			       (quaternion1.Z == quaternion2.Z) &&
424			       (quaternion1.W == quaternion2.W);
425		}
426
427		/// <summary>
428		/// Operator for inequality
429		/// </summary>
430		/// <param name="quaternion1">Quaternion 1</param>
431		/// <param name="quaternion2">Quaternion 2</param>
432		/// <returns>True if both quaternions are not equal</returns>
433		public static bool operator !=(Quaternion quaternion1,
434			Quaternion quaternion2)
435		{
436			return (quaternion1.X != quaternion2.X) ||
437			       (quaternion1.Y != quaternion2.Y) ||
438			       (quaternion1.Z != quaternion2.Z) ||
439			       (quaternion1.W != quaternion2.W);
440		}
441		#endregion
442
443		#region Negation
444		/// <summary>
445		/// Operator for unary negation
446		/// </summary>
447		/// <param name="value">Quaternion to negate</param>
448		/// <returns>Negated value</returns>
449		public static Quaternion operator -(Quaternion value)
450		{
451			return new Quaternion(-value.Vector, -value.W);
452		}
453		#endregion
454
455		#region Addition
456		/// <summary>
457		/// Operator for addition
458		/// </summary>
459		/// <param name="value1">Quaternion 1</param>
460		/// <param name="value2">Quaternion 2</param>
461		/// <returns>Added quaternion</returns>
462		public static Quaternion operator +(Quaternion value1, Quaternion value2)
463		{
464			return new Quaternion(value1.Vector + value2.Vector,
465				value1.W + value2.W);
466		}
467		#endregion
468
469		#region Multiply
470		/// <summary>
471		/// Operator for multiply
472		/// </summary>
473		/// <param name="value1">Quaternion 1</param>
474		/// <param name="value2">Quaternion 2</param>
475		/// <returns>Multiplied quaternion</returns>
476		public static Quaternion operator *(Quaternion value1, Quaternion value2)
477		{
478			return new Quaternion(Vector.Cross(value1.Vector, value2.Vector) +
479			                      (value1.W * value2.Vector) + (value1.Vector * value2.W),
480				(value1.W * value2.W) - Vector.Dot(value1.Vector, value2.Vector));
481		}
482
483		/// <summary>
484		/// Operator for multiply
485		/// </summary>
486		/// <param name="value">Quaternion to multiply with</param>
487		/// <param name="scalar">Scalar to multiply with</param>
488		/// <returns>Multiplied quaternion</returns>
489		public static Quaternion operator *(Quaternion value, float scalar)
490		{
491			return new Quaternion(value.Vector * scalar, value.W * scalar);
492		}
493
494		/// <summary>
495		/// Operator for multiply
496		/// </summary>
497		/// <param name="scalar">Scalar to multiply with</param>
498		/// <param name="value">Quaternion to multiply with</param>
499		/// <returns>Multiplied quaternion</returns>
500		public static Quaternion operator *(float scalar, Quaternion value)
501		{
502			return new Quaternion(value.Vector * scalar, value.W * scalar);
503		}
504		#endregion
505
506		#region Division
507		/// <summary>
508		/// Operator for division
509		/// </summary>
510		/// <param name="value1">Quaternion 1</param>
511		/// <param name="value2">Quaternion 2</param>
512		/// <returns>Divided quaternion</returns>
513		public static Quaternion operator /(Quaternion value1, Quaternion value2)
514		{
515			value2.Invert();
516			return value1 * value2;
517		}
518
519		/// <summary>
520		/// Operator for division
521		/// </summary>
522		/// <param name="value">Quaternion</param>
523		/// <param name="scalar">Scalar to divide through</param>
524		/// <returns>Divided quaternion</returns>
525		public static Quaternion operator /(Quaternion value, float scalar)
526		{
527			float invertedScalar = 1.0f / scalar;
528			return new Quaternion(value.Vector * invertedScalar,
529				value.W * invertedScalar);
530		}
531		#endregion
532
533		#region Cast operators
534		/// <summary>
535		/// Operator to explicitly convert a quaternion to a matrix.
536		/// </summary>
537		/// <param name="quat">Quaternion to convert</param>
538		/// <returns>Matrix from quaternion</returns>
539		public static explicit operator Matrix(Quaternion quat)
540		{
541			Matrix ret = Matrix.Identity;
542			// A cast to a matrix only works with normalized quaternions!
543			quat.Normalize();
544
545			float xs = 2.0f * quat.X;
546			float ys = 2.0f * quat.Y;
547			float zs = 2.0f * quat.Z;
548
549			float wx = quat.W * xs;
550			float wy = quat.W * ys;
551			float wz = quat.W * zs;
552			float xx = quat.X * xs;
553			float xy = quat.X * ys;
554			float xz = quat.X * zs;
555			float yy = quat.Y * ys;
556			float yz = quat.Y * zs;
557			float zz = quat.Z * zs;
558
559			ret.M11 = 1.0f - yy - zz;
560			ret.M21 = xy - wz;
561			ret.M31 = xz + wy;
562			ret.M41 = 0.0f;
563
564			ret.M12 = xy + wz;
565			ret.M22 = 1.0f - xx - zz;
566			ret.M32 = yz - wx;
567			ret.M42 = 0.0f;
568
569			ret.M13 = xz - wy;
570			ret.M23 = yz + wx;
571			ret.M33 = 1.0f - xx - yy;
572			ret.M43 = 0.0f;
573
574			return ret;
575		}
576		#endregion
577
578		#endregion
579
580		#region ISaveLoadBinary Methods
581
582		#region Save
583		/// <summary>
584		/// Saves the point to a stream.
585		/// </summary>
586		/// <param name="writer">The stream that will be used.</param>
587		public void Save(BinaryWriter writer)
588		{
589			writer.Write(X);
590			writer.Write(Y);
591			writer.Write(Z);
592			writer.Write(W);
593		}
594		#endregion
595
596		#region Load
597		/// <summary>
598		/// Load the point values from a stream.
599		/// </summary>
600		/// <param name="reader">The stream that will be used.</param>
601		public void Load(BinaryReader reader)
602		{
603			X = reader.ReadSingle();
604			Y = reader.ReadSingle();
605			Z = reader.ReadSingle();
606			W = reader.ReadSingle();
607		}
608		#endregion
609
610		#endregion
611
612		/// <summary>
613		/// Tests
614		/// </summary>
615		internal class QuaternionTests
616		{
617			#region Helpers
618			/// <summary>
619			/// Length test
620			/// 
621			/// Calculation:
622			///   10 * 10 =  100
623			/// + 40 * 40 = 1600
624			/// +  4 *  4 =   16
625			/// +  2 *  2 =    4
626			/// ----------------
627			///             _____
628			///           -/1720'
629			///               41.47288
630			/// </summary>
631			public static void Length()
632			{
633				Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f);
634				Assert.NearlyEqual(quat.Length(), 41.47288f);
635			}
636			#endregion
637
638			#region Equality (Static)
639			/// <summary>
640			/// Equality test
641			/// </summary>
642			[Test]
643			public static void Equality()
644			{
645				Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f);
646				Quaternion quat1 = new Quaternion(new Vector(10f, 40f, 4f), 2f);
647				Quaternion quat2 = new Quaternion(new Vector(5f, 0f, 43f), 21f);
648
649				// using the Equals methods
650				Assert.True(quat.Equals(quat));
651				Assert.True(quat.Equals(quat1));
652				Assert.True(quat2.Equals(quat2));
653				Assert.True(quat.Equals((object)quat));
654
655				Assert.False(quat.Equals(quat2));
656				Assert.False(quat.Equals(1234));
657
658				// using the operators
659				Assert.True(quat1 == quat);
660				Assert.True(quat == quat1);
661				Assert.True(quat1 != quat2);
662				Assert.False(quat1 == quat2);
663			}
664			#endregion
665
666			#region ArithmeticOperators (Static)
667			/// <summary>
668			/// ArithmeticOperators test
669			/// </summary>
670			[Test]
671			public static void ArithmeticOperators()
672			{
673				Quaternion quat1 = new Quaternion(new Vector(10f, 40f, 4f), 2f);
674				Quaternion quat2 = new Quaternion(new Vector(5f, 1f, 43f), 21f);
675
676				// Multiply with scalar
677				Quaternion result = quat1 * 3f;
678				Assert.NearlyEqual(result.X, 30f);
679				Assert.NearlyEqual(result.Y, 120f);
680				Assert.NearlyEqual(result.Z, 12f);
681				Assert.NearlyEqual(result.W, 6f);
682				result = 3f * quat1;
683				Assert.NearlyEqual(result.X, 30f);
684				Assert.NearlyEqual(result.Y, 120f);
685				Assert.NearlyEqual(result.Z, 12f);
686				Assert.NearlyEqual(result.W, 6f);
687
688				// Multiply two quaternions
689				// vec1 = Cross(Vector1, Vector2)
690				// vec1.X = (40 * 43) - ( 4 *  1) = 1716
691				// vec1.Y = ( 4 *  5) - (10 * 43) = -410
692				// vec1.Z = (10 *  1) - (40 *  5) = -190
693				// vec2 = (2 * {5, 1, 43}) = {10, 1, 86}
694				// vec3 = (21 * {10, 40, 4}) = {210, 840, 84}
695				// X = vec1.X + vec2.X + vec3.X = 1936
696				// Y = vec1.Y + vec2.Y + vec3.Y = 432
697				// Z = vec1.Z + vec2.Z + vec3.Z = -20
698
699				// W = (W1 * W2) - Dot(Vector1, Vector2)
700				// W = 42 - (10 * 5 + 40 * 1 + 4 * 43) = -220
701				result = quat1 * quat2;
702				Assert.NearlyEqual(result.X, 1936f);
703				Assert.NearlyEqual(result.Y, 432f);
704				Assert.NearlyEqual(result.Z, -20f);
705				Assert.NearlyEqual(result.W, -220);
706
707				// Negation of a quaternion
708				result = -quat1;
709				Assert.NearlyEqual(result.X, -10f);
710				Assert.NearlyEqual(result.Y, -40f);
711				Assert.NearlyEqual(result.Z, -4f);
712				Assert.NearlyEqual(result.W, -2f);
713
714				// Addition of two quternions
715				result = quat1 + quat2;
716				Assert.NearlyEqual(result.X, 15f);
717				Assert.NearlyEqual(result.Y, 41f);
718				Assert.NearlyEqual(result.Z, 47f);
719				Assert.NearlyEqual(result.W, 23f);
720
721				// Division with scalar
722				// invScalar = 1 / 4 = 0.25
723				// XYZ = {10, 40, 4} * invScalar = {2.5, 10, 1}
724				// W = 2 * invScalar = 0.5
725				result = quat1 / 4f;
726				Assert.NearlyEqual(result.X, 2.5f);
727				Assert.NearlyEqual(result.Y, 10f);
728				Assert.NearlyEqual(result.Z, 1f);
729				Assert.NearlyEqual(result.W, 0.5f);
730			}
731			#endregion
732
733			#region LengthSquared (Static)
734			/// <summary>
735			/// LengthSquared test
736			/// 
737			/// Calculation:
738			///   10 * 10 =  100
739			/// + 40 * 40 = 1600
740			/// +  4 *  4 =   16
741			/// +  2 *  2 =    4
742			/// ----------------
743			///             1720
744			/// </summary>
745			[Test]
746			public static void LengthSquared()
747			{
748				Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f);
749				Assert.NearlyEqual(quat.LengthSquared(), 1720);
750			}
751			#endregion
752
753			#region Normalize (Static)
754			/// <summary>
755			/// Normalize test
756			/// 
757			/// Calculation:
758			/// length = 41.47288 (see test above)
759			/// invLength = 1 / 41.47288 = 0.02411
760			/// X = 10 * invLength = 0.2411
761			/// Y = 40 * invLength = 0.9644
762			/// Z =  4 * invLength = 0.09644
763			/// W =  2 * invLength = 0.04822
764			/// </summary>
765			[Test]
766			public static void Normalize()
767			{
768				Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f);
769				quat.Normalize();
770
771				Assert.NearlyEqual(quat.X, 0.2411f);
772				Assert.NearlyEqual(quat.Y, 0.9644f);
773				Assert.NearlyEqual(quat.Z, 0.09644f);
774				Assert.NearlyEqual(quat.W, 0.04822f);
775			}
776			#endregion
777
778			#region Invert (Static)
779			/// <summary>
780			/// Invert test
781			/// 
782			/// Calculation:
783			/// length = 1720 (see test above)
784			/// 
785			/// invLength = -1 / 1720 = -0.0005814
786			/// X = 10 * invLength = -0.005814
787			/// Y = 40 * invLength = -0.023256
788			/// Z =  4 * invLength = -0.0023256
789			/// W =  2 * -invLength = 0.0011628
790			/// </summary>
791			[Test]
792			public static void Invert()
793			{
794				Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f);
795				quat.Invert();
796
797				Assert.NearlyEqual(quat.X, -0.005814f);
798				Assert.NearlyEqual(quat.Y, -0.023256f);
799				Assert.NearlyEqual(quat.Z, -0.0023256f);
800				Assert.NearlyEqual(quat.W, 0.0011628f);
801			}
802			#endregion
803
804			#region Dot (Static)
805			/// <summary>
806			/// Dot test
807			/// 
808			/// Calculation:
809			/// (x1 * x2) + (y1 * y2) + (z1 * z2) + (w1 * w2)
810			///   10 *  5 =  50
811			/// + 40 *  2 =  80
812			/// +  4 * 50 = 200
813			/// +  2 * 12 =  24
814			/// ---------------
815			///             354
816			/// </summary>
817			[Test]
818			public static void Dot()
819			{
820				Quaternion quat1 = new Quaternion(new Vector(10f, 40f, 4f), 2f);
821				Quaternion quat2 = new Quaternion(new Vector(5f, 2f, 50f), 12f);
822				float dot = Quaternion.Dot(quat1, quat2);
823
824				Assert.NearlyEqual(dot, 354f);
825			}
826			#endregion
827
828			#region Lerp (Static)
829			/// <summary>
830			/// Lerp test, will just interpolate two quaternions.
831			/// </summary>
832			[Test]
833			public static void Lerp()
834			{
835				Quaternion quat1 = new Quaternion(new Vector(10f, 40f, 4f), 2f);
836				Quaternion quat2 = new Quaternion(new Vector(5f, 2f, 50f), 12f);
837
838				Quaternion lerp = Quaternion.Lerp(quat1, quat2, 0.5f);
839
840				Assert.NearlyEqual(lerp.X, 0.01621542f);
841				Assert.NearlyEqual(lerp.Y, 0.04540319f);
842				Assert.NearlyEqual(lerp.Z, 0.05837553f);
843				Assert.NearlyEqual(lerp.W, 0.0151344f);
844			}
845			#endregion
846
847			#region Conjugate (Static)
848			/// <summary>
849			/// Conjugate test
850			/// </summary>
851			[Test]
852			public static void Conjugate()
853			{
854				Quaternion quat = new Quaternion(new Vector(10f, 40f, 4f), 2f);
855				quat.Conjugate();
856
857				Assert.NearlyEqual(quat.X, -10f);
858				Assert.NearlyEqual(quat.Y, -40f);
859				Assert.NearlyEqual(quat.Z, -4f);
860				Assert.NearlyEqual(quat.W, 2f);
861			}
862			#endregion
863
864			#region CreateFromRotationMatrix (Static)
865			/// <summary>
866			/// CreateFromRotationMatrix test
867			/// </summary>
868			[Test]
869			public static void CreateFromRotationMatrix()
870			{
871				Matrix rotMatrixX = Matrix.CreateRotationX(4f);
872				Matrix rotMatrixY = Matrix.CreateRotationY(10f);
873				Matrix rotMatrixZ = Matrix.CreateRotationZ(2f);
874				Matrix rotMatrixCombined = Matrix.Identity;
875				Matrix.Multiply(ref rotMatrixX, ref rotMatrixY,
876					ref rotMatrixCombined);
877				Matrix.Multiply(ref rotMatrixCombined, ref rotMatrixZ,
878					ref rotMatrixCombined);
879
880				Quaternion quat =
881					Quaternion.CreateFromRotationMatrix(rotMatrixCombined);
882
883				Assert.NearlyEqual(quat.X, 0.03324125f);
884				Assert.NearlyEqual(quat.Y, 0.08769615f);
885				Assert.NearlyEqual(quat.Z, 0.01433417f);
886				Assert.NearlyEqual(quat.W, 0.9954893f);
887			}
888			#endregion
889
890			#region CreateFromYawPitchRoll (Static)
891			/// <summary>
892			/// CreateFromYawPitchRoll test
893			/// 
894			/// Calculation:
895			/// yaw = 40 * 0.5 = 20
896			/// pitch = 2 * 0.5 = 1
897			/// roll = 238 * 0.5 = 119
898			/// 
899			/// rollSin = Sin(roll) = 0.342020
900			/// rollCos = Cos(roll) = 0.939692
901			/// pitchSin = Sin(pitch) = 0.017452
902			/// pitchCos = Cos(pitch) = 0.999847
903			/// yawSin = Sin(yaw) = 0.874619
904			/// yawCos = Cos(yaw) = -0.484809
905			/// 
906			/// X = ((yawCos * pitchSin) * rollCos) +
907			///     ((yawSin * pitchCos) * rollSin)
908			///   = ((-0.484809 * 0.017452) * 0.939692) +
909			///     ((0.874619 * 0.999847) * 0.342020)
910			///   = 0.291140794875046
911			/// 
912			/// Y = ((yawSin * pitchCos) * rollCos) -
913			///     ((yawCos * pitchSin) * rollSin)
914			///   = ((0.874619 * 0.999847) * 0.939692) -
915			///     ((-0.484809 * 0.017452) * 0.342020)
916			///   = 0.824640523317155
917			/// 
918			/// Z = ((yawCos * pitchCos) * rollSin) -
919			///     ((yawSin * pitchSin) * rollCos)
920			///   = ((-0.484809 * 0.999847) * 0.342020) -
921			///     ((0.874619 * 0.017452) * 0.939692)
922			///   = -0.180132323055428
923			/// 
924			/// W = ((yawCos * pitchCos) * rollCos) +
925			///     ((yawSin * pitchSin) * rollSin)
926			///   = ((-0.484809 * 0.999847) * 0.939692) +
927			///     ((0.874619 * 0.017452) * 0.342020)
928			///   = -0.450280894197248
929			/// </summary>
930			[Test]
931			public static void CreateFromYawPitchRoll()
932			{
933				Quaternion quat = Quaternion.CreateFromYawPitchRoll(40f, 2f, 238f);
934
935				Assert.NearlyEqual(quat.X, 0.291140f);
936				Assert.NearlyEqual(quat.Y, 0.824640f);
937				Assert.NearlyEqual(quat.Z, -0.180132f);
938				Assert.NearlyEqual(quat.W, -0.450280f);
939			}
940			#endregion
941
942			#region CreateFromAxisAngle (Static)
943			/// <summary>
944			/// CreateFromAxisAngle test
945			/// 
946			/// Calculation:
947			/// sinAngle = Sin(4.5) = 0.0784590957278449
948			/// X = 45 * sinAngle = 3.53065
949			/// Y =  2 * sinAngle = 0.1569
950			/// Z = 14 * sinAngle = 1.09842
951			/// W = Cos(4.5) = 0.99691
952			/// </summary>
953			[Test]
954			public static void CreateFromAxisAngle()
955			{
956				Quaternion quat = Quaternion.CreateFromAxisAngle(
957					new Vector(45f, 2f, 14f), 9f);
958
959				Assert.NearlyEqual(quat.X, 3.53065f);
960				Assert.NearlyEqual(quat.Y, 0.1569f);
961				Assert.NearlyEqual(quat.Z, 1.09842f);
962				Assert.NearlyEqual(quat.W, 0.99691f);
963			}
964			#endregion
965
966			#region SizeOf
967			/// <summary>
968			/// Checks if the size of Point is exactly 8 bytes (2 floats: X and Y)
969			/// </summary>
970			[Test]
971			public void SizeOf()
972			{
973				// Quaternion has 4 floats: X, Y, Z, and W
974				Assert.Equal(4 * 4, Marshal.SizeOf(typeof(Quaternion)));
975			}
976			#endregion
977		}
978	}
979}