#region --- License ---
/*
Copyright (c) 2006 - 2008 The Open Toolkit library.
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.
*/
#endregion
using System;
using System.Runtime.InteropServices;
using System.ComponentModel;
using System.Xml.Serialization;
namespace OpenTK
{
///
/// Represents a Quaternion.
///
[Serializable]
[StructLayout(LayoutKind.Sequential)]
public struct Quaternion : IEquatable
{
#region Fields
Vector3 xyz;
float w;
#endregion
#region Constructors
///
/// Construct a new Quaternion from vector and w components
///
/// The vector part
/// The w part
public Quaternion(Vector3 v, float w)
{
this.xyz = v;
this.w = w;
}
///
/// Construct a new Quaternion
///
/// The x component
/// The y component
/// The z component
/// The w component
public Quaternion(float x, float y, float z, float w)
: this(new Vector3(x, y, z), w)
{ }
///
/// Construct a new Quaternion from given Euler angles
///
/// The pitch (attitude), rotation around X axis
/// The yaw (heading), rotation around Y axis
/// The roll (bank), rotation around Z axis
public Quaternion(float pitch, float yaw, float roll)
{
yaw *= 0.5f;
pitch *= 0.5f;
roll *= 0.5f;
float c1 = (float)Math.Cos(yaw);
float c2 = (float)Math.Cos(pitch);
float c3 = (float)Math.Cos(roll);
float s1 = (float)Math.Sin(yaw);
float s2 = (float)Math.Sin(pitch);
float s3 = (float)Math.Sin(roll);
this.w = c1 * c2 * c3 - s1 * s2 * s3;
this.xyz.X = s1 * s2 * c3 + c1 * c2 * s3;
this.xyz.Y = s1 * c2 * c3 + c1 * s2 * s3;
this.xyz.Z = c1 * s2 * c3 - s1 * c2 * s3;
}
///
/// Construct a new Quaternion from given Euler angles
///
/// The euler angles as a Vector3
public Quaternion(Vector3 eulerAngles)
:this(eulerAngles.X, eulerAngles.Y, eulerAngles.Z)
{ }
#endregion
#region Public Members
#region Properties
#pragma warning disable 3005 // Identifier differing only in case is not CLS-compliant, compiler bug in Mono 3.4.0
///
/// Gets or sets an OpenTK.Vector3 with the X, Y and Z components of this instance.
///
[Obsolete("Use Xyz property instead.")]
[EditorBrowsable(EditorBrowsableState.Never)]
[XmlIgnore]
[CLSCompliant(false)]
public Vector3 XYZ { get { return Xyz; } set { Xyz = value; } }
///
/// Gets or sets an OpenTK.Vector3 with the X, Y and Z components of this instance.
///
[CLSCompliant(false)]
public Vector3 Xyz { get { return xyz; } set { xyz = value; } }
#pragma warning restore 3005
///
/// Gets or sets the X component of this instance.
///
[XmlIgnore]
public float X { get { return xyz.X; } set { xyz.X = value; } }
///
/// Gets or sets the Y component of this instance.
///
[XmlIgnore]
public float Y { get { return xyz.Y; } set { xyz.Y = value; } }
///
/// Gets or sets the Z component of this instance.
///
[XmlIgnore]
public float Z { get { return xyz.Z; } set { xyz.Z = value; } }
///
/// Gets or sets the W component of this instance.
///
public float W { get { return w; } set { w = value; } }
#endregion
#region Instance
#region ToAxisAngle
///
/// Convert the current quaternion to axis angle representation
///
/// The resultant axis
/// The resultant angle
public void ToAxisAngle(out Vector3 axis, out float angle)
{
Vector4 result = ToAxisAngle();
axis = result.Xyz;
angle = result.W;
}
///
/// Convert this instance to an axis-angle representation.
///
/// A Vector4 that is the axis-angle representation of this quaternion.
public Vector4 ToAxisAngle()
{
Quaternion q = this;
if (Math.Abs(q.W) > 1.0f)
q.Normalize();
Vector4 result = new Vector4();
result.W = 2.0f * (float)System.Math.Acos(q.W); // angle
float den = (float)System.Math.Sqrt(1.0 - q.W * q.W);
if (den > 0.0001f)
{
result.Xyz = q.Xyz / den;
}
else
{
// This occurs when the angle is zero.
// Not a problem: just set an arbitrary normalized axis.
result.Xyz = Vector3.UnitX;
}
return result;
}
#endregion
#region public float Length
///
/// Gets the length (magnitude) of the quaternion.
///
///
public float Length
{
get
{
return (float)System.Math.Sqrt(W * W + Xyz.LengthSquared);
}
}
#endregion
#region public float LengthSquared
///
/// Gets the square of the quaternion length (magnitude).
///
public float LengthSquared
{
get
{
return W * W + Xyz.LengthSquared;
}
}
#endregion
///
/// Returns a copy of the Quaternion scaled to unit length.
///
public Quaternion Normalized()
{
Quaternion q = this;
q.Normalize();
return q;
}
///
/// Reverses the rotation angle of this Quaterniond.
///
public void Invert()
{
W = -W;
}
///
/// Returns a copy of this Quaterniond with its rotation angle reversed.
///
public Quaternion Inverted()
{
var q = this;
q.Invert();
return q;
}
#region public void Normalize()
///
/// Scales the Quaternion to unit length.
///
public void Normalize()
{
float scale = 1.0f / this.Length;
Xyz *= scale;
W *= scale;
}
#endregion
#region public void Conjugate()
///
/// Inverts the Vector3 component of this Quaternion.
///
public void Conjugate()
{
Xyz = -Xyz;
}
#endregion
#endregion
#region Static
#region Fields
///
/// Defines the identity quaternion.
///
public static readonly Quaternion Identity = new Quaternion(0, 0, 0, 1);
#endregion
#region Add
///
/// Add two quaternions
///
/// The first operand
/// The second operand
/// The result of the addition
public static Quaternion Add(Quaternion left, Quaternion right)
{
return new Quaternion(
left.Xyz + right.Xyz,
left.W + right.W);
}
///
/// Add two quaternions
///
/// The first operand
/// The second operand
/// The result of the addition
public static void Add(ref Quaternion left, ref Quaternion right, out Quaternion result)
{
result = new Quaternion(
left.Xyz + right.Xyz,
left.W + right.W);
}
#endregion
#region Sub
///
/// Subtracts two instances.
///
/// The left instance.
/// The right instance.
/// The result of the operation.
public static Quaternion Sub(Quaternion left, Quaternion right)
{
return new Quaternion(
left.Xyz - right.Xyz,
left.W - right.W);
}
///
/// Subtracts two instances.
///
/// The left instance.
/// The right instance.
/// The result of the operation.
public static void Sub(ref Quaternion left, ref Quaternion right, out Quaternion result)
{
result = new Quaternion(
left.Xyz - right.Xyz,
left.W - right.W);
}
#endregion
#region Mult
///
/// Multiplies two instances.
///
/// The first instance.
/// The second instance.
/// A new instance containing the result of the calculation.
[Obsolete("Use Multiply instead.")]
public static Quaternion Mult(Quaternion left, Quaternion right)
{
return new Quaternion(
right.W * left.Xyz + left.W * right.Xyz + Vector3.Cross(left.Xyz, right.Xyz),
left.W * right.W - Vector3.Dot(left.Xyz, right.Xyz));
}
///
/// Multiplies two instances.
///
/// The first instance.
/// The second instance.
/// A new instance containing the result of the calculation.
[Obsolete("Use Multiply instead.")]
public static void Mult(ref Quaternion left, ref Quaternion right, out Quaternion result)
{
result = new Quaternion(
right.W * left.Xyz + left.W * right.Xyz + Vector3.Cross(left.Xyz, right.Xyz),
left.W * right.W - Vector3.Dot(left.Xyz, right.Xyz));
}
///
/// Multiplies two instances.
///
/// The first instance.
/// The second instance.
/// A new instance containing the result of the calculation.
public static Quaternion Multiply(Quaternion left, Quaternion right)
{
Quaternion result;
Multiply(ref left, ref right, out result);
return result;
}
///
/// Multiplies two instances.
///
/// The first instance.
/// The second instance.
/// A new instance containing the result of the calculation.
public static void Multiply(ref Quaternion left, ref Quaternion right, out Quaternion result)
{
result = new Quaternion(
right.W * left.Xyz + left.W * right.Xyz + Vector3.Cross(left.Xyz, right.Xyz),
left.W * right.W - Vector3.Dot(left.Xyz, right.Xyz));
}
///
/// Multiplies an instance by a scalar.
///
/// The instance.
/// The scalar.
/// A new instance containing the result of the calculation.
public static void Multiply(ref Quaternion quaternion, float scale, out Quaternion result)
{
result = new Quaternion(quaternion.X * scale, quaternion.Y * scale, quaternion.Z * scale, quaternion.W * scale);
}
///
/// Multiplies an instance by a scalar.
///
/// The instance.
/// The scalar.
/// A new instance containing the result of the calculation.
public static Quaternion Multiply(Quaternion quaternion, float scale)
{
return new Quaternion(quaternion.X * scale, quaternion.Y * scale, quaternion.Z * scale, quaternion.W * scale);
}
#endregion
#region Conjugate
///
/// Get the conjugate of the given quaternion
///
/// The quaternion
/// The conjugate of the given quaternion
public static Quaternion Conjugate(Quaternion q)
{
return new Quaternion(-q.Xyz, q.W);
}
///
/// Get the conjugate of the given quaternion
///
/// The quaternion
/// The conjugate of the given quaternion
public static void Conjugate(ref Quaternion q, out Quaternion result)
{
result = new Quaternion(-q.Xyz, q.W);
}
#endregion
#region Invert
///
/// Get the inverse of the given quaternion
///
/// The quaternion to invert
/// The inverse of the given quaternion
public static Quaternion Invert(Quaternion q)
{
Quaternion result;
Invert(ref q, out result);
return result;
}
///
/// Get the inverse of the given quaternion
///
/// The quaternion to invert
/// The inverse of the given quaternion
public static void Invert(ref Quaternion q, out Quaternion result)
{
float lengthSq = q.LengthSquared;
if (lengthSq != 0.0)
{
float i = 1.0f / lengthSq;
result = new Quaternion(q.Xyz * -i, q.W * i);
}
else
{
result = q;
}
}
#endregion
#region Normalize
///
/// Scale the given quaternion to unit length
///
/// The quaternion to normalize
/// The normalized quaternion
public static Quaternion Normalize(Quaternion q)
{
Quaternion result;
Normalize(ref q, out result);
return result;
}
///
/// Scale the given quaternion to unit length
///
/// The quaternion to normalize
/// The normalized quaternion
public static void Normalize(ref Quaternion q, out Quaternion result)
{
float scale = 1.0f / q.Length;
result = new Quaternion(q.Xyz * scale, q.W * scale);
}
#endregion
#region FromAxisAngle
///
/// Build a quaternion from the given axis and angle
///
/// The axis to rotate about
/// The rotation angle in radians
/// The equivalent quaternion
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 FromEulerAngles
///
/// Builds a Quaternion from the given euler angles
///
/// The pitch (attitude), rotation around X axis
/// The yaw (heading), rotation around Y axis
/// The roll (bank), rotation around Z axis
///
public static Quaternion FromEulerAngles(float pitch, float yaw, float roll)
{
return new Quaternion(pitch, yaw, roll);
}
///
/// Builds a Quaternion from the given euler angles
///
/// The euler angles as a vector
/// The equivalent Quaternion
public static Quaternion FromEulerAngles(Vector3 eulerAngles)
{
return new Quaternion(eulerAngles);
}
///
/// Builds a Quaternion from the given euler angles
///
/// The euler angles a vector
/// The equivalent Quaternion
public static void FromEulerAngles(ref Vector3 eulerAngles, out Quaternion result)
{
float c1 = (float)Math.Cos(eulerAngles.Y * 0.5f);
float c2 = (float)Math.Cos(eulerAngles.X * 0.5f);
float c3 = (float)Math.Cos(eulerAngles.Z * 0.5f);
float s1 = (float)Math.Sin(eulerAngles.Y * 0.5f);
float s2 = (float)Math.Sin(eulerAngles.X * 0.5f);
float s3 = (float)Math.Sin(eulerAngles.Z * 0.5f);
result.w = c1 * c2 * c3 - s1 * s2 * s3;
result.xyz.X = s1 * s2 * c3 + c1 * c2 * s3;
result.xyz.Y = s1 * c2 * c3 + c1 * s2 * s3;
result.xyz.Z = c1 * s2 * c3 - s1 * c2 * s3;
}
#endregion
#region FromMatrix
///
/// Builds a quaternion from the given rotation matrix
///
/// A rotation matrix
/// The equivalent quaternion
public static Quaternion FromMatrix(Matrix3 matrix)
{
Quaternion result;
FromMatrix(ref matrix, out result);
return result;
}
///
/// Builds a quaternion from the given rotation matrix
///
/// A rotation matrix
/// The equivalent quaternion
public static void FromMatrix(ref Matrix3 matrix, out Quaternion result)
{
float trace = matrix.Trace;
if (trace > 0)
{
float s = (float)Math.Sqrt(trace + 1) * 2;
float invS = 1f / s;
result.w = s * 0.25f;
result.xyz.X = (matrix.Row2.Y - matrix.Row1.Z) * invS;
result.xyz.Y = (matrix.Row0.Z - matrix.Row2.X) * invS;
result.xyz.Z = (matrix.Row1.X - matrix.Row0.Y) * invS;
}
else
{
float m00 = matrix.Row0.X, m11 = matrix.Row1.Y, m22 = matrix.Row2.Z;
if (m00 > m11 && m00 > m22)
{
float s = (float)Math.Sqrt(1 + m00 - m11 - m22) * 2;
float invS = 1f / s;
result.w = (matrix.Row2.Y - matrix.Row1.Z) * invS;
result.xyz.X = s * 0.25f;
result.xyz.Y = (matrix.Row0.Y + matrix.Row1.X) * invS;
result.xyz.Z = (matrix.Row0.Z + matrix.Row2.X) * invS;
}
else if (m11 > m22)
{
float s = (float)Math.Sqrt(1 + m11 - m00 - m22) * 2;
float invS = 1f / s;
result.w = (matrix.Row0.Z - matrix.Row2.X) * invS;
result.xyz.X = (matrix.Row0.Y + matrix.Row1.X) * invS;
result.xyz.Y = s * 0.25f;
result.xyz.Z = (matrix.Row1.Z + matrix.Row2.Y) * invS;
}
else
{
float s = (float)Math.Sqrt(1 + m22 - m00 - m11) * 2;
float invS = 1f / s;
result.w = (matrix.Row1.X - matrix.Row0.Y) * invS;
result.xyz.X = (matrix.Row0.Z + matrix.Row2.X) * invS;
result.xyz.Y = (matrix.Row1.Z + matrix.Row2.Y) * invS;
result.xyz.Z = s * 0.25f;
}
}
}
#endregion
#region Slerp
///
/// Do Spherical linear interpolation between two quaternions
///
/// The first quaternion
/// The second quaternion
/// The blend factor
/// A smooth blend between the given quaternions
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 Operators
///
/// Adds two instances.
///
/// The first instance.
/// The second instance.
/// The result of the calculation.
public static Quaternion operator +(Quaternion left, Quaternion right)
{
left.Xyz += right.Xyz;
left.W += right.W;
return left;
}
///
/// Subtracts two instances.
///
/// The first instance.
/// The second instance.
/// The result of the calculation.
public static Quaternion operator -(Quaternion left, Quaternion right)
{
left.Xyz -= right.Xyz;
left.W -= right.W;
return left;
}
///
/// Multiplies two instances.
///
/// The first instance.
/// The second instance.
/// The result of the calculation.
public static Quaternion operator *(Quaternion left, Quaternion right)
{
Multiply(ref left, ref right, out left);
return left;
}
///
/// Multiplies an instance by a scalar.
///
/// The instance.
/// The scalar.
/// A new instance containing the result of the calculation.
public static Quaternion operator *(Quaternion quaternion, float scale)
{
Multiply(ref quaternion, scale, out quaternion);
return quaternion;
}
///
/// Multiplies an instance by a scalar.
///
/// The instance.
/// The scalar.
/// A new instance containing the result of the calculation.
public static Quaternion operator *(float scale, Quaternion quaternion)
{
return new Quaternion(quaternion.X * scale, quaternion.Y * scale, quaternion.Z * scale, quaternion.W * scale);
}
///
/// Compares two instances for equality.
///
/// The first instance.
/// The second instance.
/// True, if left equals right; false otherwise.
public static bool operator ==(Quaternion left, Quaternion right)
{
return left.Equals(right);
}
///
/// Compares two instances for inequality.
///
/// The first instance.
/// The second instance.
/// True, if left does not equal right; false otherwise.
public static bool operator !=(Quaternion left, Quaternion right)
{
return !left.Equals(right);
}
#endregion
#region Overrides
#region public override string ToString()
///
/// Returns a System.String that represents the current Quaternion.
///
///
public override string ToString()
{
return String.Format("V: {0}, W: {1}", Xyz, W);
}
#endregion
#region public override bool Equals (object o)
///
/// Compares this object instance to another object for equality.
///
/// The other object to be used in the comparison.
/// True if both objects are Quaternions of equal value. Otherwise it returns false.
public override bool Equals(object other)
{
if (other is Quaternion == false) return false;
return this == (Quaternion)other;
}
#endregion
#region public override int GetHashCode ()
///
/// Provides the hash code for this object.
///
/// A hash code formed from the bitwise XOR of this objects members.
public override int GetHashCode()
{
unchecked
{
return (this.xyz.GetHashCode() * 397) ^ this.w.GetHashCode();
}
}
#endregion
#endregion
#endregion
#region IEquatable Members
///
/// Compares this Quaternion instance to another Quaternion for equality.
///
/// The other Quaternion to be used in the comparison.
/// True if both instances are equal; false otherwise.
public bool Equals(Quaternion other)
{
return Xyz == other.Xyz && W == other.W;
}
#endregion
}
}