mirror of
https://github.com/Ryujinx/Opentk.git
synced 2024-12-25 05:35:38 +00:00
527fb19364
Fixed ToAxisAngle implementation at 0-degree angles. Added ToAxisAngle overload that returns a Vector4 struct.
580 lines
17 KiB
C#
580 lines
17 KiB
C#
#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;
|
|
|
|
namespace OpenTK.Math
|
|
{
|
|
/// <summary>
|
|
/// Represents a Quaternion
|
|
/// </summary>
|
|
[Serializable]
|
|
[StructLayout(LayoutKind.Sequential)]
|
|
public struct Quaternion : IEquatable<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;
|
|
|
|
#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 Public Members
|
|
|
|
#region Properties
|
|
|
|
/// <summary>
|
|
/// Gets or sets the X component of this instance.
|
|
/// </summary>
|
|
public float X
|
|
{
|
|
get { return XYZ.X; }
|
|
set { XYZ.X = value; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets or sets the Y component of this instance.
|
|
/// </summary>
|
|
public float Y
|
|
{
|
|
get { return XYZ.Y; }
|
|
set { XYZ.Y = value; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets or sets the Z component of this instance.
|
|
/// </summary>
|
|
public float Z
|
|
{
|
|
get { return XYZ.Z; }
|
|
set { XYZ.Z = value; }
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Instance
|
|
|
|
#region ToAxisAngle
|
|
|
|
/// <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)
|
|
{
|
|
Vector4 result = ToAxisAngle();
|
|
axis = result.Xyz;
|
|
angle = result.W;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert this instance to an axis-angle representation.
|
|
/// </summary>
|
|
/// <returns>A Vector4 that is the axis-angle representation of this quaternion.</returns>
|
|
public Vector4 ToAxisAngle()
|
|
{
|
|
Quaternion q = this;
|
|
if (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
|
|
|
|
/// <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 Static
|
|
|
|
#region Fields
|
|
|
|
/// <summary>
|
|
/// Defines the identity quaternion.
|
|
/// </summary>
|
|
public static Quaternion Identity = new Quaternion(0, 0, 0, 1);
|
|
|
|
#endregion
|
|
|
|
#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 Operators
|
|
|
|
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;
|
|
}
|
|
|
|
public static bool operator ==(Quaternion left, Quaternion right)
|
|
{
|
|
return left.Equals(right);
|
|
}
|
|
|
|
public static bool operator !=(Quaternion left, Quaternion right)
|
|
{
|
|
return !left.Equals(right);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Overrides
|
|
|
|
#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
|
|
|
|
#region public override bool Equals (object o)
|
|
|
|
/// <summary>
|
|
/// Compares this object instance to another object for equality.
|
|
/// </summary>
|
|
/// <param name="other">The other object to be used in the comparison.</param>
|
|
/// <returns>True if both objects are Quaternions of equal value. Otherwise it returns false.</returns>
|
|
public override bool Equals(object o)
|
|
{
|
|
if (o is Quaternion == false) return false;
|
|
return this == (Quaternion)o;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region public override int GetHashCode ()
|
|
|
|
/// <summary>
|
|
/// Provides the hash code for this object.
|
|
/// </summary>
|
|
/// <returns>A hash code formed from the bitwise XOR of this objects members.</returns>
|
|
public override int GetHashCode()
|
|
{
|
|
return XYZ.GetHashCode() ^ W.GetHashCode();
|
|
}
|
|
|
|
#endregion
|
|
|
|
#endregion
|
|
|
|
#endregion
|
|
|
|
#region IEquatable<Quaternion> Members
|
|
|
|
/// <summary>
|
|
/// Compares this Quaternion instance to another Quaternion for equality.
|
|
/// </summary>
|
|
/// <param name="other">The other Quaternion to be used in the comparison.</param>
|
|
/// <returns>True if both instances are equal; false otherwise.</returns>
|
|
public bool Equals(Quaternion other)
|
|
{
|
|
return XYZ == other.XYZ && W == other.W;
|
|
}
|
|
|
|
#endregion
|
|
}
|
|
}
|