#region --- License --- /* Copyright (c) 2006, 2007 the OpenTK team * See license.txt for license info * * Implemented by Andy Gill */ #endregion using System; using System.Collections.Generic; using System.Text; using System.Runtime.InteropServices; namespace OpenTK.Math { /// <summary> /// Represents a Quaternion /// </summary> [StructLayout(LayoutKind.Sequential)] public struct Quaternion { #region Fields /// <summary> /// The vector part of the quaternion /// </summary> public Vector3 XYZ; /// <summary> /// The w component of the quaternion /// </summary> public float W; public static Quaternion Identity = new Quaternion(0, 0, 0, 1); #endregion #region Constructors /// <summary> /// Construct a new Quaternion from vector and w components /// </summary> /// <param name="v">The vector part</param> /// <param name="w">The w part</param> public Quaternion(Vector3 v, float w) { XYZ = v; W = w; } /// <summary> /// Construct a new Quaternion /// </summary> /// <param name="x">The x component</param> /// <param name="y">The y component</param> /// <param name="z">The z component</param> /// <param name="w">The w component</param> public Quaternion(float x, float y, float z, float w) { XYZ = new Vector3(x, y, z); W = w; } #endregion #region Functions #region pubilc void ToAxisAngle(out Vector3 axis, out float angle) /// <summary> /// Convert the current quaternion to axis angle representation /// </summary> /// <param name="axis">The resultant axis</param> /// <param name="angle">The resultant angle</param> public void ToAxisAngle(out Vector3 axis, out float angle) { Quaternion q = this; if (q.W > 1.0f) q.Normalize(); angle = 2.0f * (float)System.Math.Acos(q.W); float den = (float)System.Math.Sqrt(1.0 - q.W * q.W); axis = q.XYZ; if (den > 0.0001f) { axis = q.XYZ / den; } } #endregion #region public float Length /// <summary> /// Gets the length (magnitude) of the quaternion. /// </summary> /// <seealso cref="LengthSquared"/> public float Length { get { return (float)System.Math.Sqrt(W * W + XYZ.LengthSquared); } } #endregion #region public float LengthSquared /// <summary> /// Gets the square of the quaternion length (magnitude). /// </summary> public float LengthSquared { get { return W * W + XYZ.LengthSquared; } } #endregion #region public void Normalize() /// <summary> /// Scales the Quaternion to unit length. /// </summary> public void Normalize() { float scale = 1.0f / this.Length; XYZ *= scale; W *= scale; } #endregion #region public void Conjugate() /// <summary> /// Convert this quaternion to its conjugate /// </summary> public void Conjugate() { XYZ = -XYZ; } #endregion #endregion #region Operator overloads public static Quaternion operator +(Quaternion left, Quaternion right) { left.XYZ += right.XYZ; left.W += right.W; return left; } public static Quaternion operator -(Quaternion left, Quaternion right) { left.XYZ -= right.XYZ; left.W -= right.W; return left; } public static Quaternion operator *(Quaternion left, Quaternion right) { float w = left.W * right.W - Vector3.Dot(left.XYZ, right.XYZ); left.XYZ = right.W * left.XYZ + left.W * right.XYZ + Vector3.Cross(left.XYZ, right.XYZ); left.W = w; return left; } [CLSCompliant(false)] unsafe public static explicit operator float*(Quaternion q) { return &q.XYZ.X; } public static explicit operator IntPtr(Quaternion q) { unsafe { return (IntPtr)(&q.XYZ.X); } } #endregion #region Static functions #region Add /// <summary> /// Add two quaternions /// </summary> /// <param name="left">The first operand</param> /// <param name="right">The second operand</param> /// <returns>The result of the addition</returns> public static Quaternion Add(Quaternion left, Quaternion right) { left.XYZ += right.XYZ; left.W += right.W; return left; } /// <summary> /// Add two quaternions /// </summary> /// <param name="left">The first operand</param> /// <param name="right">The second operand</param> /// <param name="result">The result of the addition</param> public static void Add(ref Quaternion left, ref Quaternion right, out Quaternion result) { result.XYZ = left.XYZ + right.XYZ; result.W = left.W + right.W; } #endregion #region Sub public static Quaternion Sub(Quaternion left, Quaternion right) { left.XYZ -= right.XYZ; left.W -= right.W; return left; } public static void Sub(ref Quaternion left, ref Quaternion right, out Quaternion result) { result.XYZ = left.XYZ - right.XYZ; result.W = left.W - right.W; } #endregion #region Mult public static Quaternion Mult(Quaternion left, Quaternion right) { float w = left.W * right.W - Vector3.Dot(left.XYZ, right.XYZ); left.XYZ = right.W * left.XYZ + left.W * right.XYZ + Vector3.Cross(left.XYZ, right.XYZ); left.W = w; return left; } public static void Mult(ref Quaternion left, ref Quaternion right, out Quaternion result) { result.W = left.W * right.W - Vector3.Dot(left.XYZ, right.XYZ); result.XYZ = right.W * left.XYZ + left.W * right.XYZ + Vector3.Cross(left.XYZ, right.XYZ); } #endregion #region Conjugate /// <summary> /// Get the conjugate of the given quaternion /// </summary> /// <param name="q">The quaternion</param> /// <returns>The conjugate of the given quaternion</returns> public static Quaternion Conjugate(Quaternion q) { q.XYZ = -q.XYZ; return q; } /// <summary> /// Get the conjugate of the given quaternion /// </summary> /// <param name="q">The quaternion</param> /// <param name="result">The conjugate of the given quaternion</param> public static void Conjugate(ref Quaternion q, out Quaternion result) { result.XYZ = -q.XYZ; result.W = q.W; } #endregion #region Invert /// <summary> /// Get the inverse of the given quaternion /// </summary> /// <param name="q">The quaternion to invert</param> /// <returns>The inverse of the given quaternion</returns> public static Quaternion Invert(Quaternion q) { float lengthSq = q.LengthSquared; if (lengthSq != 0.0) { float i = 1.0f / lengthSq; q.XYZ *= -i; q.W *= i; } return q; } /// <summary> /// Get the inverse of the given quaternion /// </summary> /// <param name="q">The quaternion to invert</param> /// <param name="result">The inverse of the given quaternion</param> public static void Invert(ref Quaternion q, out Quaternion result) { float lengthSq = q.LengthSquared; if (lengthSq != 0.0) { float i = 1.0f / lengthSq; result.XYZ = q.XYZ * -i; result.W = q.W * i; } else { result = q; } } #endregion #region Normalize /// <summary> /// Scale the given quaternion to unit length /// </summary> /// <param name="q">The quaternion to normalize</param> /// <returns>The normalized quaternion</returns> public static Quaternion Normalize(Quaternion q) { float scale = 1.0f / q.Length; q.XYZ *= scale; q.W *= scale; return q; } /// <summary> /// Scale the given quaternion to unit length /// </summary> /// <param name="q">The quaternion to normalize</param> /// <param name="result">The normalized quaternion</param> public static void Normalize(ref Quaternion q, out Quaternion result) { float scale = 1.0f / q.Length; result.XYZ = q.XYZ * scale; result.W = q.W * scale; } #endregion #region FromAxisAngle /// <summary> /// Build a quaternion from the given axis and angle /// </summary> /// <param name="axis">The axis to rotate about</param> /// <param name="angle">The rotation angle in radians</param> /// <returns></returns> public static Quaternion FromAxisAngle(Vector3 axis, float angle) { if (axis.LengthSquared == 0.0f) return Identity; Quaternion result = Identity; angle *= 0.5f; axis.Normalize(); result.XYZ = axis * (float)System.Math.Sin(angle); result.W = (float)System.Math.Cos(angle); return Normalize(result); } #endregion #region Slerp /// <summary> /// Do Spherical linear interpolation between two quaternions /// </summary> /// <param name="q1">The first quaternion</param> /// <param name="q2">The second quaternion</param> /// <param name="blend">The blend factor</param> /// <returns>A smooth blend between the given quaternions</returns> public static Quaternion Slerp(Quaternion q1, Quaternion q2, float blend) { // if either input is zero, return the other. if (q1.LengthSquared == 0.0f) { if (q2.LengthSquared == 0.0f) { return Identity; } return q2; } else if (q2.LengthSquared == 0.0f) { return q1; } float cosHalfAngle = q1.W * q2.W + Vector3.Dot(q1.XYZ, q2.XYZ); if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) { // angle = 0.0f, so just return one input. return q1; } else if (cosHalfAngle < 0.0f) { q2.XYZ = -q2.XYZ; q2.W = -q2.W; cosHalfAngle = -cosHalfAngle; } float blendA; float blendB; if (cosHalfAngle < 0.99f) { // do proper slerp for big angles float halfAngle = (float)System.Math.Acos(cosHalfAngle); float sinHalfAngle = (float)System.Math.Sin(halfAngle); float oneOverSinHalfAngle = 1.0f / sinHalfAngle; blendA = (float)System.Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle; blendB = (float)System.Math.Sin(halfAngle * blend) * oneOverSinHalfAngle; } else { // do lerp if angle is really small. blendA = 1.0f - blend; blendB = blend; } Quaternion result = new Quaternion(blendA * q1.XYZ + blendB * q2.XYZ, blendA * q1.W + blendB * q2.W); if (result.LengthSquared > 0.0f) return Normalize(result); else return Identity; } #endregion #endregion #region public override string ToString() /// <summary> /// Returns a System.String that represents the current Quaternion. /// </summary> /// <returns></returns> public override string ToString() { return String.Format("V: {0}, W: {1}", XYZ, W); } #endregion } }