1438 lines
60 KiB
C#
1438 lines
60 KiB
C#
// Copyright (c) Xenko contributors (https://xenko.com) and Silicon Studio Corp. (https://www.siliconstudio.co.jp)
|
|
// Distributed under the MIT license. See the LICENSE.md file in the project root for more information.
|
|
//
|
|
// -----------------------------------------------------------------------------
|
|
// Original code from SlimMath project. http://code.google.com/p/slimmath/
|
|
// Greetings to SlimDX Group. Original code published with the following license:
|
|
// -----------------------------------------------------------------------------
|
|
/*
|
|
* Copyright (c) 2007-2011 SlimDX Group
|
|
*
|
|
* 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.
|
|
*/
|
|
using System;
|
|
using System.Globalization;
|
|
using System.Runtime.InteropServices;
|
|
using System.Runtime.Serialization;
|
|
|
|
namespace math
|
|
{
|
|
/// <summary>
|
|
/// Represents a four dimensional mathematical quaternion.
|
|
/// </summary>
|
|
[DataContract( Name = "quaternion" )]
|
|
[DataStyle( DataStyle.Compact )]
|
|
[StructLayout( LayoutKind.Sequential, Pack = 4 )]
|
|
public struct Quaternion : IEquatable<Quaternion>, IFormattable
|
|
{
|
|
/// <summary>
|
|
/// The size of the <see cref="math.Quaternion"/> type, in bytes.
|
|
/// </summary>
|
|
public static readonly int SizeInBytes = lib.Util.SizeOf<Quaternion>();
|
|
|
|
/// <summary>
|
|
/// A <see cref="math.Quaternion"/> with all of its components set to zero.
|
|
/// </summary>
|
|
public static readonly Quaternion Zero = new Quaternion();
|
|
|
|
/// <summary>
|
|
/// A <see cref="math.Quaternion"/> with all of its components set to one.
|
|
/// </summary>
|
|
public static readonly Quaternion One = new Quaternion( 1.0f, 1.0f, 1.0f, 1.0f );
|
|
|
|
/// <summary>
|
|
/// The identity <see cref="math.Quaternion"/> (0, 0, 0, 1).
|
|
/// </summary>
|
|
public static readonly Quaternion Identity = new Quaternion( 0.0f, 0.0f, 0.0f, 1.0f );
|
|
|
|
/// <summary>
|
|
/// The X component of the quaternion.
|
|
/// </summary>
|
|
public float X;
|
|
|
|
/// <summary>
|
|
/// The Y component of the quaternion.
|
|
/// </summary>
|
|
public float Y;
|
|
|
|
/// <summary>
|
|
/// The Z component of the quaternion.
|
|
/// </summary>
|
|
public float Z;
|
|
|
|
/// <summary>
|
|
/// The W component of the quaternion.
|
|
/// </summary>
|
|
public float W;
|
|
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="math.Quaternion"/> struct.
|
|
/// </summary>
|
|
/// <param name="value">The value that will be assigned to all components.</param>
|
|
public Quaternion( float value )
|
|
{
|
|
X = value;
|
|
Y = value;
|
|
Z = value;
|
|
W = value;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="math.Quaternion"/> struct.
|
|
/// </summary>
|
|
/// <param name="value">A vector containing the values with which to initialize the components.</param>
|
|
public Quaternion( Vec4 value )
|
|
{
|
|
X = value.X;
|
|
Y = value.Y;
|
|
Z = value.Z;
|
|
W = value.W;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="math.Quaternion"/> struct.
|
|
/// </summary>
|
|
/// <param name="value">A vector containing the values with which to initialize the X, Y, and Z components.</param>
|
|
/// <param name="w">Initial value for the W component of the quaternion.</param>
|
|
public Quaternion( Vec3 value, float w )
|
|
{
|
|
X = value.X;
|
|
Y = value.Y;
|
|
Z = value.Z;
|
|
W = w;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="math.Quaternion"/> struct.
|
|
/// </summary>
|
|
/// <param name="value">A vector containing the values with which to initialize the X and Y components.</param>
|
|
/// <param name="z">Initial value for the Z component of the quaternion.</param>
|
|
/// <param name="w">Initial value for the W component of the quaternion.</param>
|
|
public Quaternion( Vec2 value, float z, float w )
|
|
{
|
|
X = value.X;
|
|
Y = value.Y;
|
|
Z = z;
|
|
W = w;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="math.Quaternion"/> struct.
|
|
/// </summary>
|
|
/// <param name="x">Initial value for the X component of the quaternion.</param>
|
|
/// <param name="y">Initial value for the Y component of the quaternion.</param>
|
|
/// <param name="z">Initial value for the Z component of the quaternion.</param>
|
|
/// <param name="w">Initial value for the W component of the quaternion.</param>
|
|
public Quaternion( float x, float y, float z, float w )
|
|
{
|
|
X = x;
|
|
Y = y;
|
|
Z = z;
|
|
W = w;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="math.Quaternion"/> struct.
|
|
/// </summary>
|
|
/// <param name="values">The values to assign to the X, Y, Z, and W components of the quaternion. This must be an array with four elements.</param>
|
|
/// <exception cref="ArgumentNullException">Thrown when <paramref name="values"/> is <c>null</c>.</exception>
|
|
/// <exception cref="ArgumentOutOfRangeException">Thrown when <paramref name="values"/> contains more or less than four elements.</exception>
|
|
public Quaternion( float[] values )
|
|
{
|
|
if( values == null )
|
|
throw new ArgumentNullException( "values" );
|
|
if( values.Length != 4 )
|
|
throw new ArgumentOutOfRangeException( "values", "There must be four and only four input values for Quaternion." );
|
|
|
|
X = values[0];
|
|
Y = values[1];
|
|
Z = values[2];
|
|
W = values[3];
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets a value indicating whether this instance is equivalent to the identity quaternion.
|
|
/// </summary>
|
|
/// <value>
|
|
/// <c>true</c> if this instance is an identity quaternion; otherwise, <c>false</c>.
|
|
/// </value>
|
|
public bool IsIdentity
|
|
{
|
|
get { return this.Equals( Identity ); }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets a value indicting whether this instance is normalized.
|
|
/// </summary>
|
|
public bool IsNormalized
|
|
{
|
|
get { return Math.Abs( ( X * X ) + ( Y * Y ) + ( Z * Z ) + ( W * W ) - 1f ) < MathUtil.ZeroTolerance; }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the angle of the quaternion.
|
|
/// </summary>
|
|
/// <value>The quaternion's angle.</value>
|
|
public float Angle
|
|
{
|
|
get
|
|
{
|
|
float length = ( X * X ) + ( Y * Y ) + ( Z * Z );
|
|
if( length < MathUtil.ZeroTolerance )
|
|
return 0.0f;
|
|
|
|
return (float)( 2.0 * Math.Acos( W ) );
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the axis components of the quaternion.
|
|
/// </summary>
|
|
/// <value>The axis components of the quaternion.</value>
|
|
public Vec3 Axis
|
|
{
|
|
get
|
|
{
|
|
float length = ( X * X ) + ( Y * Y ) + ( Z * Z );
|
|
if( length < MathUtil.ZeroTolerance )
|
|
return Vec3.UnitX;
|
|
|
|
float inv = 1.0f / length;
|
|
return new Vec3( X * inv, Y * inv, Z * inv );
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets yaw/pitch/roll equivalent of the quaternion
|
|
/// </summary>
|
|
public Vec3 YawPitchRoll
|
|
{
|
|
get
|
|
{
|
|
Vec3 yawPitchRoll;
|
|
RotationYawPitchRoll( ref this, out yawPitchRoll.X, out yawPitchRoll.Y, out yawPitchRoll.Z );
|
|
return yawPitchRoll;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets or sets the component at the specified index.
|
|
/// </summary>
|
|
/// <value>The value of the X, Y, Z, or W component, depending on the index.</value>
|
|
/// <param name="index">The index of the component to access. Use 0 for the X component, 1 for the Y component, 2 for the Z component, and 3 for the W component.</param>
|
|
/// <returns>The value of the component at the specified index.</returns>
|
|
/// <exception cref="System.ArgumentOutOfRangeException">Thrown when the <paramref name="index"/> is out of the range [0, 3].</exception>
|
|
public float this[int index]
|
|
{
|
|
get
|
|
{
|
|
switch( index )
|
|
{
|
|
case 0:
|
|
return X;
|
|
case 1:
|
|
return Y;
|
|
case 2:
|
|
return Z;
|
|
case 3:
|
|
return W;
|
|
}
|
|
|
|
throw new ArgumentOutOfRangeException( "index", "Indices for Quaternion run from 0 to 3, inclusive." );
|
|
}
|
|
|
|
set
|
|
{
|
|
switch( index )
|
|
{
|
|
case 0:
|
|
X = value;
|
|
break;
|
|
case 1:
|
|
Y = value;
|
|
break;
|
|
case 2:
|
|
Z = value;
|
|
break;
|
|
case 3:
|
|
W = value;
|
|
break;
|
|
default:
|
|
throw new ArgumentOutOfRangeException( "index", "Indices for Quaternion run from 0 to 3, inclusive." );
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Conjugates the quaternion.
|
|
/// </summary>
|
|
public void Conjugate()
|
|
{
|
|
X = -X;
|
|
Y = -Y;
|
|
Z = -Z;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Conjugates and renormalizes the quaternion.
|
|
/// </summary>
|
|
public void Invert()
|
|
{
|
|
float lengthSq = LengthSquared();
|
|
if( lengthSq > MathUtil.ZeroTolerance )
|
|
{
|
|
lengthSq = 1.0f / lengthSq;
|
|
|
|
X = -X * lengthSq;
|
|
Y = -Y * lengthSq;
|
|
Z = -Z * lengthSq;
|
|
W = W * lengthSq;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculates the length of the quaternion.
|
|
/// </summary>
|
|
/// <returns>The length of the quaternion.</returns>
|
|
/// <remarks>
|
|
/// <see cref="math.Quaternion.LengthSquared"/> may be preferred when only the relative length is needed
|
|
/// and speed is of the essence.
|
|
/// </remarks>
|
|
public float Length()
|
|
{
|
|
return (float)Math.Sqrt( ( X * X ) + ( Y * Y ) + ( Z * Z ) + ( W * W ) );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculates the squared length of the quaternion.
|
|
/// </summary>
|
|
/// <returns>The squared length of the quaternion.</returns>
|
|
/// <remarks>
|
|
/// This method may be preferred to <see cref="math.Quaternion.Length"/> when only a relative length is needed
|
|
/// and speed is of the essence.
|
|
/// </remarks>
|
|
public float LengthSquared()
|
|
{
|
|
return ( X * X ) + ( Y * Y ) + ( Z * Z ) + ( W * W );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts the quaternion into a unit quaternion.
|
|
/// </summary>
|
|
public void Normalize()
|
|
{
|
|
float length = Length();
|
|
if( length > MathUtil.ZeroTolerance )
|
|
{
|
|
float inverse = 1.0f / length;
|
|
X *= inverse;
|
|
Y *= inverse;
|
|
Z *= inverse;
|
|
W *= inverse;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates an array containing the elements of the quaternion.
|
|
/// </summary>
|
|
/// <returns>A four-element array containing the components of the quaternion.</returns>
|
|
public float[] ToArray()
|
|
{
|
|
return new float[] { X, Y, Z, W };
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds two quaternions.
|
|
/// </summary>
|
|
/// <param name="left">The first quaternion to add.</param>
|
|
/// <param name="right">The second quaternion to add.</param>
|
|
/// <param name="result">When the method completes, contains the sum of the two quaternions.</param>
|
|
public static void Add( ref Quaternion left, ref Quaternion right, out Quaternion result )
|
|
{
|
|
result.X = left.X + right.X;
|
|
result.Y = left.Y + right.Y;
|
|
result.Z = left.Z + right.Z;
|
|
result.W = left.W + right.W;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds two quaternions.
|
|
/// </summary>
|
|
/// <param name="left">The first quaternion to add.</param>
|
|
/// <param name="right">The second quaternion to add.</param>
|
|
/// <returns>The sum of the two quaternions.</returns>
|
|
public static Quaternion Add( Quaternion left, Quaternion right )
|
|
{
|
|
Quaternion result;
|
|
Add( ref left, ref right, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Subtracts two quaternions.
|
|
/// </summary>
|
|
/// <param name="left">The first quaternion to subtract.</param>
|
|
/// <param name="right">The second quaternion to subtract.</param>
|
|
/// <param name="result">When the method completes, contains the difference of the two quaternions.</param>
|
|
public static void Subtract( ref Quaternion left, ref Quaternion right, out Quaternion result )
|
|
{
|
|
result.X = left.X - right.X;
|
|
result.Y = left.Y - right.Y;
|
|
result.Z = left.Z - right.Z;
|
|
result.W = left.W - right.W;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Subtracts two quaternions.
|
|
/// </summary>
|
|
/// <param name="left">The first quaternion to subtract.</param>
|
|
/// <param name="right">The second quaternion to subtract.</param>
|
|
/// <returns>The difference of the two quaternions.</returns>
|
|
public static Quaternion Subtract( Quaternion left, Quaternion right )
|
|
{
|
|
Quaternion result;
|
|
Subtract( ref left, ref right, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Scales a quaternion by the given value.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion to scale.</param>
|
|
/// <param name="scale">The amount by which to scale the quaternion.</param>
|
|
/// <param name="result">When the method completes, contains the scaled quaternion.</param>
|
|
public static void Multiply( ref Quaternion value, float scale, out Quaternion result )
|
|
{
|
|
result.X = value.X * scale;
|
|
result.Y = value.Y * scale;
|
|
result.Z = value.Z * scale;
|
|
result.W = value.W * scale;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Scales a quaternion by the given value.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion to scale.</param>
|
|
/// <param name="scale">The amount by which to scale the quaternion.</param>
|
|
/// <returns>The scaled quaternion.</returns>
|
|
public static Quaternion Multiply( Quaternion value, float scale )
|
|
{
|
|
Quaternion result;
|
|
Multiply( ref value, scale, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Modulates a quaternion by another.
|
|
/// </summary>
|
|
/// <param name="left">The first quaternion to modulate.</param>
|
|
/// <param name="right">The second quaternion to modulate.</param>
|
|
/// <param name="result">When the moethod completes, contains the modulated quaternion.</param>
|
|
public static void Multiply( ref Quaternion left, ref Quaternion right, out Quaternion result )
|
|
{
|
|
float lx = left.X;
|
|
float ly = left.Y;
|
|
float lz = left.Z;
|
|
float lw = left.W;
|
|
float rx = right.X;
|
|
float ry = right.Y;
|
|
float rz = right.Z;
|
|
float rw = right.W;
|
|
|
|
result.X = ( rx * lw + lx * rw + ry * lz ) - ( rz * ly );
|
|
result.Y = ( ry * lw + ly * rw + rz * lx ) - ( rx * lz );
|
|
result.Z = ( rz * lw + lz * rw + rx * ly ) - ( ry * lx );
|
|
result.W = ( rw * lw ) - ( rx * lx + ry * ly + rz * lz );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Modulates a quaternion by another.
|
|
/// </summary>
|
|
/// <param name="left">The first quaternion to modulate.</param>
|
|
/// <param name="right">The second quaternion to modulate.</param>
|
|
/// <returns>The modulated quaternion.</returns>
|
|
public static Quaternion Multiply( Quaternion left, Quaternion right )
|
|
{
|
|
Quaternion result;
|
|
Multiply( ref left, ref right, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Reverses the direction of a given quaternion.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion to negate.</param>
|
|
/// <param name="result">When the method completes, contains a quaternion facing in the opposite direction.</param>
|
|
public static void Negate( ref Quaternion value, out Quaternion result )
|
|
{
|
|
result.X = -value.X;
|
|
result.Y = -value.Y;
|
|
result.Z = -value.Z;
|
|
result.W = -value.W;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Reverses the direction of a given quaternion.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion to negate.</param>
|
|
/// <returns>A quaternion facing in the opposite direction.</returns>
|
|
public static Quaternion Negate( Quaternion value )
|
|
{
|
|
Quaternion result;
|
|
Negate( ref value, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a <see cref="math.Quaternion"/> containing the 4D Cartesian coordinates of a point specified in Barycentric coordinates relative to a 2D triangle.
|
|
/// </summary>
|
|
/// <param name="value1">A <see cref="math.Quaternion"/> containing the 4D Cartesian coordinates of vertex 1 of the triangle.</param>
|
|
/// <param name="value2">A <see cref="math.Quaternion"/> containing the 4D Cartesian coordinates of vertex 2 of the triangle.</param>
|
|
/// <param name="value3">A <see cref="math.Quaternion"/> containing the 4D Cartesian coordinates of vertex 3 of the triangle.</param>
|
|
/// <param name="amount1">Barycentric coordinate b2, which expresses the weighting factor toward vertex 2 (specified in <paramref name="value2"/>).</param>
|
|
/// <param name="amount2">Barycentric coordinate b3, which expresses the weighting factor toward vertex 3 (specified in <paramref name="value3"/>).</param>
|
|
/// <param name="result">When the method completes, contains a new <see cref="math.Quaternion"/> containing the 4D Cartesian coordinates of the specified point.</param>
|
|
public static void Barycentric( ref Quaternion value1, ref Quaternion value2, ref Quaternion value3, float amount1, float amount2, out Quaternion result )
|
|
{
|
|
Quaternion start, end;
|
|
Slerp( ref value1, ref value2, amount1 + amount2, out start );
|
|
Slerp( ref value1, ref value3, amount1 + amount2, out end );
|
|
Slerp( ref start, ref end, amount2 / ( amount1 + amount2 ), out result );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a <see cref="math.Quaternion"/> containing the 4D Cartesian coordinates of a point specified in Barycentric coordinates relative to a 2D triangle.
|
|
/// </summary>
|
|
/// <param name="value1">A <see cref="math.Quaternion"/> containing the 4D Cartesian coordinates of vertex 1 of the triangle.</param>
|
|
/// <param name="value2">A <see cref="math.Quaternion"/> containing the 4D Cartesian coordinates of vertex 2 of the triangle.</param>
|
|
/// <param name="value3">A <see cref="math.Quaternion"/> containing the 4D Cartesian coordinates of vertex 3 of the triangle.</param>
|
|
/// <param name="amount1">Barycentric coordinate b2, which expresses the weighting factor toward vertex 2 (specified in <paramref name="value2"/>).</param>
|
|
/// <param name="amount2">Barycentric coordinate b3, which expresses the weighting factor toward vertex 3 (specified in <paramref name="value3"/>).</param>
|
|
/// <returns>A new <see cref="math.Quaternion"/> containing the 4D Cartesian coordinates of the specified point.</returns>
|
|
public static Quaternion Barycentric( Quaternion value1, Quaternion value2, Quaternion value3, float amount1, float amount2 )
|
|
{
|
|
Quaternion result;
|
|
Barycentric( ref value1, ref value2, ref value3, amount1, amount2, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Conjugates a quaternion.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion to conjugate.</param>
|
|
/// <param name="result">When the method completes, contains the conjugated quaternion.</param>
|
|
public static void Conjugate( ref Quaternion value, out Quaternion result )
|
|
{
|
|
result.X = -value.X;
|
|
result.Y = -value.Y;
|
|
result.Z = -value.Z;
|
|
result.W = value.W;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Conjugates a quaternion.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion to conjugate.</param>
|
|
/// <returns>The conjugated quaternion.</returns>
|
|
public static Quaternion Conjugate( Quaternion value )
|
|
{
|
|
Quaternion result;
|
|
Conjugate( ref value, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculates the dot product of two quaternions.
|
|
/// </summary>
|
|
/// <param name="left">First source quaternion.</param>
|
|
/// <param name="right">Second source quaternion.</param>
|
|
/// <param name="result">When the method completes, contains the dot product of the two quaternions.</param>
|
|
public static void Dot( ref Quaternion left, ref Quaternion right, out float result )
|
|
{
|
|
result = ( left.X * right.X ) + ( left.Y * right.Y ) + ( left.Z * right.Z ) + ( left.W * right.W );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculates the dot product of two quaternions.
|
|
/// </summary>
|
|
/// <param name="left">First source quaternion.</param>
|
|
/// <param name="right">Second source quaternion.</param>
|
|
/// <returns>The dot product of the two quaternions.</returns>
|
|
public static float Dot( Quaternion left, Quaternion right )
|
|
{
|
|
return ( left.X * right.X ) + ( left.Y * right.Y ) + ( left.Z * right.Z ) + ( left.W * right.W );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Exponentiates a quaternion.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion to exponentiate.</param>
|
|
/// <param name="result">When the method completes, contains the exponentiated quaternion.</param>
|
|
public static void Exponential( ref Quaternion value, out Quaternion result )
|
|
{
|
|
float angle = (float)Math.Sqrt( ( value.X * value.X ) + ( value.Y * value.Y ) + ( value.Z * value.Z ) );
|
|
float sin = (float)Math.Sin( angle );
|
|
|
|
if( Math.Abs( sin ) >= MathUtil.ZeroTolerance )
|
|
{
|
|
float coeff = sin / angle;
|
|
result.X = coeff * value.X;
|
|
result.Y = coeff * value.Y;
|
|
result.Z = coeff * value.Z;
|
|
}
|
|
else
|
|
{
|
|
result = value;
|
|
}
|
|
|
|
result.W = (float)Math.Cos( angle );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Exponentiates a quaternion.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion to exponentiate.</param>
|
|
/// <returns>The exponentiated quaternion.</returns>
|
|
public static Quaternion Exponential( Quaternion value )
|
|
{
|
|
Quaternion result;
|
|
Exponential( ref value, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Conjugates and renormalizes the quaternion.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion to conjugate and renormalize.</param>
|
|
/// <param name="result">When the method completes, contains the conjugated and renormalized quaternion.</param>
|
|
public static void Invert( ref Quaternion value, out Quaternion result )
|
|
{
|
|
result = value;
|
|
result.Invert();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Conjugates and renormalizes the quaternion.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion to conjugate and renormalize.</param>
|
|
/// <returns>The conjugated and renormalized quaternion.</returns>
|
|
public static Quaternion Invert( Quaternion value )
|
|
{
|
|
Quaternion result;
|
|
Invert( ref value, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Performs a linear interpolation between two quaternions.
|
|
/// </summary>
|
|
/// <param name="start">Start quaternion.</param>
|
|
/// <param name="end">End quaternion.</param>
|
|
/// <param name="amount">Value between 0 and 1 indicating the weight of <paramref name="end"/>.</param>
|
|
/// <param name="result">When the method completes, contains the linear interpolation of the two quaternions.</param>
|
|
/// <remarks>
|
|
/// This method performs the linear interpolation based on the following formula.
|
|
/// <code>start + (end - start) * amount</code>
|
|
/// Passing <paramref name="amount"/> a value of 0 will cause <paramref name="start"/> to be returned; a value of 1 will cause <paramref name="end"/> to be returned.
|
|
/// </remarks>
|
|
public static void Lerp( ref Quaternion start, ref Quaternion end, float amount, out Quaternion result )
|
|
{
|
|
float inverse = 1.0f - amount;
|
|
|
|
if( Dot( start, end ) >= 0.0f )
|
|
{
|
|
result.X = ( inverse * start.X ) + ( amount * end.X );
|
|
result.Y = ( inverse * start.Y ) + ( amount * end.Y );
|
|
result.Z = ( inverse * start.Z ) + ( amount * end.Z );
|
|
result.W = ( inverse * start.W ) + ( amount * end.W );
|
|
}
|
|
else
|
|
{
|
|
result.X = ( inverse * start.X ) - ( amount * end.X );
|
|
result.Y = ( inverse * start.Y ) - ( amount * end.Y );
|
|
result.Z = ( inverse * start.Z ) - ( amount * end.Z );
|
|
result.W = ( inverse * start.W ) - ( amount * end.W );
|
|
}
|
|
|
|
result.Normalize();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Performs a linear interpolation between two quaternion.
|
|
/// </summary>
|
|
/// <param name="start">Start quaternion.</param>
|
|
/// <param name="end">End quaternion.</param>
|
|
/// <param name="amount">Value between 0 and 1 indicating the weight of <paramref name="end"/>.</param>
|
|
/// <returns>The linear interpolation of the two quaternions.</returns>
|
|
/// <remarks>
|
|
/// This method performs the linear interpolation based on the following formula.
|
|
/// <code>start + (end - start) * amount</code>
|
|
/// Passing <paramref name="amount"/> a value of 0 will cause <paramref name="start"/> to be returned; a value of 1 will cause <paramref name="end"/> to be returned.
|
|
/// </remarks>
|
|
public static Quaternion Lerp( Quaternion start, Quaternion end, float amount )
|
|
{
|
|
Quaternion result;
|
|
Lerp( ref start, ref end, amount, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculates the natural logarithm of the specified quaternion.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion whose logarithm will be calculated.</param>
|
|
/// <param name="result">When the method completes, contains the natural logarithm of the quaternion.</param>
|
|
public static void Logarithm( ref Quaternion value, out Quaternion result )
|
|
{
|
|
if( Math.Abs( value.W ) < 1.0 )
|
|
{
|
|
float angle = (float)Math.Acos( value.W );
|
|
float sin = (float)Math.Sin( angle );
|
|
|
|
if( Math.Abs( sin ) >= MathUtil.ZeroTolerance )
|
|
{
|
|
float coeff = angle / sin;
|
|
result.X = value.X * coeff;
|
|
result.Y = value.Y * coeff;
|
|
result.Z = value.Z * coeff;
|
|
}
|
|
else
|
|
{
|
|
result = value;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
result = value;
|
|
}
|
|
|
|
result.W = 0.0f;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculates the natural logarithm of the specified quaternion.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion whose logarithm will be calculated.</param>
|
|
/// <returns>The natural logarithm of the quaternion.</returns>
|
|
public static Quaternion Logarithm( Quaternion value )
|
|
{
|
|
Quaternion result;
|
|
Logarithm( ref value, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts the quaternion into a unit quaternion.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion to normalize.</param>
|
|
/// <param name="result">When the method completes, contains the normalized quaternion.</param>
|
|
public static void Normalize( ref Quaternion value, out Quaternion result )
|
|
{
|
|
Quaternion temp = value;
|
|
result = temp;
|
|
result.Normalize();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts the quaternion into a unit quaternion.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion to normalize.</param>
|
|
/// <returns>The normalized quaternion.</returns>
|
|
public static Quaternion Normalize( Quaternion value )
|
|
{
|
|
value.Normalize();
|
|
return value;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Rotates a Vector3 by the specified quaternion rotation.
|
|
/// </summary>
|
|
/// <param name="vector">The vector to rotate.</param>
|
|
public void Rotate( ref Vec3 vector )
|
|
{
|
|
var pureQuaternion = new Quaternion( vector, 0 );
|
|
pureQuaternion = Conjugate( this ) * pureQuaternion * this;
|
|
|
|
vector.X = pureQuaternion.X;
|
|
vector.Y = pureQuaternion.Y;
|
|
vector.Z = pureQuaternion.Z;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates a quaternion given a rotation and an axis.
|
|
/// </summary>
|
|
/// <param name="axis">The axis of rotation.</param>
|
|
/// <param name="angle">The angle of rotation.</param>
|
|
/// <param name="result">When the method completes, contains the newly created quaternion.</param>
|
|
public static void RotationAxis( ref Vec3 axis, float angle, out Quaternion result )
|
|
{
|
|
Vec3 normalized;
|
|
Vec3.Normalize( ref axis, out normalized );
|
|
|
|
float half = angle * 0.5f;
|
|
float sin = (float)Math.Sin( half );
|
|
float cos = (float)Math.Cos( half );
|
|
|
|
result.X = normalized.X * sin;
|
|
result.Y = normalized.Y * sin;
|
|
result.Z = normalized.Z * sin;
|
|
result.W = cos;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates a quaternion given a rotation and an axis.
|
|
/// </summary>
|
|
/// <param name="axis">The axis of rotation.</param>
|
|
/// <param name="angle">The angle of rotation.</param>
|
|
/// <returns>The newly created quaternion.</returns>
|
|
public static Quaternion RotationAxis( Vec3 axis, float angle )
|
|
{
|
|
Quaternion result;
|
|
RotationAxis( ref axis, angle, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates a quaternion given a rotation matrix.
|
|
/// </summary>
|
|
/// <param name="matrix">The rotation matrix.</param>
|
|
/// <param name="result">When the method completes, contains the newly created quaternion.</param>
|
|
public static void RotationMatrix( ref Matrix matrix, out Quaternion result )
|
|
{
|
|
float sqrt;
|
|
float half;
|
|
float scale = matrix.M11 + matrix.M22 + matrix.M33;
|
|
|
|
if( scale > 0.0f )
|
|
{
|
|
sqrt = (float)Math.Sqrt( scale + 1.0f );
|
|
result.W = sqrt * 0.5f;
|
|
sqrt = 0.5f / sqrt;
|
|
|
|
result.X = ( matrix.M23 - matrix.M32 ) * sqrt;
|
|
result.Y = ( matrix.M31 - matrix.M13 ) * sqrt;
|
|
result.Z = ( matrix.M12 - matrix.M21 ) * sqrt;
|
|
}
|
|
else if( ( matrix.M11 >= matrix.M22 ) && ( matrix.M11 >= matrix.M33 ) )
|
|
{
|
|
sqrt = (float)Math.Sqrt( 1.0f + matrix.M11 - matrix.M22 - matrix.M33 );
|
|
half = 0.5f / sqrt;
|
|
|
|
result.X = 0.5f * sqrt;
|
|
result.Y = ( matrix.M12 + matrix.M21 ) * half;
|
|
result.Z = ( matrix.M13 + matrix.M31 ) * half;
|
|
result.W = ( matrix.M23 - matrix.M32 ) * half;
|
|
}
|
|
else if( matrix.M22 > matrix.M33 )
|
|
{
|
|
sqrt = (float)Math.Sqrt( 1.0f + matrix.M22 - matrix.M11 - matrix.M33 );
|
|
half = 0.5f / sqrt;
|
|
|
|
result.X = ( matrix.M21 + matrix.M12 ) * half;
|
|
result.Y = 0.5f * sqrt;
|
|
result.Z = ( matrix.M32 + matrix.M23 ) * half;
|
|
result.W = ( matrix.M31 - matrix.M13 ) * half;
|
|
}
|
|
else
|
|
{
|
|
sqrt = (float)Math.Sqrt( 1.0f + matrix.M33 - matrix.M11 - matrix.M22 );
|
|
half = 0.5f / sqrt;
|
|
|
|
result.X = ( matrix.M31 + matrix.M13 ) * half;
|
|
result.Y = ( matrix.M32 + matrix.M23 ) * half;
|
|
result.Z = 0.5f * sqrt;
|
|
result.W = ( matrix.M12 - matrix.M21 ) * half;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates a quaternion given a rotation matrix.
|
|
/// </summary>
|
|
/// <param name="matrix">The rotation matrix.</param>
|
|
/// <returns>The newly created quaternion.</returns>
|
|
public static Quaternion RotationMatrix( Matrix matrix )
|
|
{
|
|
Quaternion result;
|
|
RotationMatrix( ref matrix, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates a quaternion that rotates around the x-axis.
|
|
/// </summary>
|
|
/// <param name="angle">Angle of rotation in radians.</param>
|
|
/// <param name="result">When the method completes, contains the newly created quaternion.</param>
|
|
public static void RotationX( float angle, out Quaternion result )
|
|
{
|
|
float halfAngle = angle * 0.5f;
|
|
result = new Quaternion( (float)Math.Sin( halfAngle ), 0.0f, 0.0f, (float)Math.Cos( halfAngle ) );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates a quaternion that rotates around the x-axis.
|
|
/// </summary>
|
|
/// <param name="angle">Angle of rotation in radians.</param>
|
|
/// <returns>The created rotation quaternion.</returns>
|
|
public static Quaternion RotationX( float angle )
|
|
{
|
|
Quaternion result;
|
|
RotationX( angle, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates a quaternion that rotates around the y-axis.
|
|
/// </summary>
|
|
/// <param name="angle">Angle of rotation in radians.</param>
|
|
/// <param name="result">When the method completes, contains the newly created quaternion.</param>
|
|
public static void RotationY( float angle, out Quaternion result )
|
|
{
|
|
float halfAngle = angle * 0.5f;
|
|
result = new Quaternion( 0.0f, (float)Math.Sin( halfAngle ), 0.0f, (float)Math.Cos( halfAngle ) );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates a quaternion that rotates around the y-axis.
|
|
/// </summary>
|
|
/// <param name="angle">Angle of rotation in radians.</param>
|
|
/// <returns>The created rotation quaternion.</returns>
|
|
public static Quaternion RotationY( float angle )
|
|
{
|
|
Quaternion result;
|
|
RotationY( angle, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates a quaternion that rotates around the z-axis.
|
|
/// </summary>
|
|
/// <param name="angle">Angle of rotation in radians.</param>
|
|
/// <param name="result">When the method completes, contains the newly created quaternion.</param>
|
|
public static void RotationZ( float angle, out Quaternion result )
|
|
{
|
|
float halfAngle = angle * 0.5f;
|
|
result = new Quaternion( 0.0f, 0.0f, (float)Math.Sin( halfAngle ), (float)Math.Cos( halfAngle ) );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates a quaternion that rotates around the z-axis.
|
|
/// </summary>
|
|
/// <param name="angle">Angle of rotation in radians.</param>
|
|
/// <returns>The created rotation quaternion.</returns>
|
|
public static Quaternion RotationZ( float angle )
|
|
{
|
|
Quaternion result;
|
|
RotationZ( angle, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate the yaw/pitch/roll rotation equivalent to the provided quaternion.
|
|
/// </summary>
|
|
/// <param name="rotation">The input quaternion</param>
|
|
/// <param name="yaw">The yaw component</param>
|
|
/// <param name="pitch">The pitch component</param>
|
|
/// <param name="roll">The roll component</param>
|
|
public static void RotationYawPitchRoll( ref Quaternion rotation, out float yaw, out float pitch, out float roll )
|
|
{
|
|
// Equivalent to:
|
|
// Matrix rotationMatrix;
|
|
// Matrix.Rotation(ref cachedRotation, out rotationMatrix);
|
|
// rotationMatrix.DecomposeXYZ(out rotationEuler);
|
|
|
|
var xx = rotation.X * rotation.X;
|
|
var yy = rotation.Y * rotation.Y;
|
|
var zz = rotation.Z * rotation.Z;
|
|
var xy = rotation.X * rotation.Y;
|
|
var zw = rotation.Z * rotation.W;
|
|
var zx = rotation.Z * rotation.X;
|
|
var yw = rotation.Y * rotation.W;
|
|
var yz = rotation.Y * rotation.Z;
|
|
var xw = rotation.X * rotation.W;
|
|
|
|
pitch = (float)Math.Asin( 2.0f * ( xw - yz ) );
|
|
double test = Math.Cos( pitch );
|
|
if( test > MathUtil.ZeroTolerance )
|
|
{
|
|
roll = (float)Math.Atan2( 2.0f * ( xy + zw ), 1.0f - ( 2.0f * ( zz + xx ) ) );
|
|
yaw = (float)Math.Atan2( 2.0f * ( zx + yw ), 1.0f - ( 2.0f * ( yy + xx ) ) );
|
|
}
|
|
else
|
|
{
|
|
roll = (float)Math.Atan2( -2.0f * ( xy - zw ), 1.0f - ( 2.0f * ( yy + zz ) ) );
|
|
yaw = 0.0f;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates a quaternion given a yaw, pitch, and roll value (angles in radians).
|
|
/// </summary>
|
|
/// <param name="yaw">The yaw of rotation in radians.</param>
|
|
/// <param name="pitch">The pitch of rotation in radians.</param>
|
|
/// <param name="roll">The roll of rotation in radians.</param>
|
|
/// <param name="result">When the method completes, contains the newly created quaternion.</param>
|
|
public static void RotationYawPitchRoll( float yaw, float pitch, float roll, out Quaternion result )
|
|
{
|
|
var halfRoll = roll * 0.5f;
|
|
var halfPitch = pitch * 0.5f;
|
|
var halfYaw = yaw * 0.5f;
|
|
|
|
var sinRoll = (float)Math.Sin( halfRoll );
|
|
var cosRoll = (float)Math.Cos( halfRoll );
|
|
var sinPitch = (float)Math.Sin( halfPitch );
|
|
var cosPitch = (float)Math.Cos( halfPitch );
|
|
var sinYaw = (float)Math.Sin( halfYaw );
|
|
var cosYaw = (float)Math.Cos( halfYaw );
|
|
|
|
var cosYawPitch = cosYaw * cosPitch;
|
|
var sinYawPitch = sinYaw * sinPitch;
|
|
|
|
result.X = ( cosYaw * sinPitch * cosRoll ) + ( sinYaw * cosPitch * sinRoll );
|
|
result.Y = ( sinYaw * cosPitch * cosRoll ) - ( cosYaw * sinPitch * sinRoll );
|
|
result.Z = ( cosYawPitch * sinRoll ) - ( sinYawPitch * cosRoll );
|
|
result.W = ( cosYawPitch * cosRoll ) + ( sinYawPitch * sinRoll );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates a quaternion given a yaw, pitch, and roll value.
|
|
/// </summary>
|
|
/// <param name="yaw">The yaw of rotation.</param>
|
|
/// <param name="pitch">The pitch of rotation.</param>
|
|
/// <param name="roll">The roll of rotation.</param>
|
|
/// <returns>The newly created quaternion.</returns>
|
|
public static Quaternion RotationYawPitchRoll( float yaw, float pitch, float roll )
|
|
{
|
|
Quaternion result;
|
|
RotationYawPitchRoll( yaw, pitch, roll, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes a quaternion corresponding to the rotation transforming the vector <paramref name="source"/> to the vector <paramref name="target"/>.
|
|
/// </summary>
|
|
/// <param name="source">The source vector of the transformation.</param>
|
|
/// <param name="target">The target vector of the transformation.</param>
|
|
/// <returns>The resulting quaternion corresponding to the transformation of the source vector to the target vector.</returns>
|
|
public static Quaternion BetweenDirections( Vec3 source, Vec3 target )
|
|
{
|
|
Quaternion result;
|
|
BetweenDirections( ref source, ref target, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Computes a quaternion corresponding to the rotation transforming the vector <paramref name="source"/> to the vector <paramref name="target"/>.
|
|
/// </summary>
|
|
/// <param name="source">The source vector of the transformation.</param>
|
|
/// <param name="target">The target vector of the transformation.</param>
|
|
/// <param name="result">The resulting quaternion corresponding to the transformation of the source vector to the target vector.</param>
|
|
public static void BetweenDirections( ref Vec3 source, ref Vec3 target, out Quaternion result )
|
|
{
|
|
var norms = (float)Math.Sqrt( source.LengthSquared() * target.LengthSquared() );
|
|
var real = norms + Vec3.Dot( source, target );
|
|
if( real < MathUtil.ZeroTolerance * norms )
|
|
{
|
|
// If source and target are exactly opposite, rotate 180 degrees around an arbitrary orthogonal axis.
|
|
// Axis normalisation can happen later, when we normalise the quaternion.
|
|
result = Math.Abs( source.X ) > Math.Abs( source.Z )
|
|
? new Quaternion( -source.Y, source.X, 0.0f, 0.0f )
|
|
: new Quaternion( 0.0f, -source.Z, source.Y, 0.0f );
|
|
}
|
|
else
|
|
{
|
|
// Otherwise, build quaternion the standard way.
|
|
var axis = Vec3.Cross( source, target );
|
|
result = new Quaternion( axis, real );
|
|
}
|
|
result.Normalize();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolates between two quaternions, using spherical linear interpolation.
|
|
/// </summary>
|
|
/// <param name="start">Start quaternion.</param>
|
|
/// <param name="end">End quaternion.</param>
|
|
/// <param name="amount">Value between 0 and 1 indicating the weight of <paramref name="end"/>.</param>
|
|
/// <param name="result">When the method completes, contains the spherical linear interpolation of the two quaternions.</param>
|
|
public static void Slerp( ref Quaternion start, ref Quaternion end, float amount, out Quaternion result )
|
|
{
|
|
float opposite;
|
|
float inverse;
|
|
float dot = Dot( start, end );
|
|
|
|
if( Math.Abs( dot ) > 1.0f - MathUtil.ZeroTolerance )
|
|
{
|
|
inverse = 1.0f - amount;
|
|
opposite = amount * Math.Sign( dot );
|
|
}
|
|
else
|
|
{
|
|
float acos = (float)Math.Acos( Math.Abs( dot ) );
|
|
float invSin = (float)( 1.0 / Math.Sin( acos ) );
|
|
|
|
inverse = (float)Math.Sin( ( 1.0f - amount ) * acos ) * invSin;
|
|
opposite = (float)Math.Sin( amount * acos ) * invSin * Math.Sign( dot );
|
|
}
|
|
|
|
result.X = ( inverse * start.X ) + ( opposite * end.X );
|
|
result.Y = ( inverse * start.Y ) + ( opposite * end.Y );
|
|
result.Z = ( inverse * start.Z ) + ( opposite * end.Z );
|
|
result.W = ( inverse * start.W ) + ( opposite * end.W );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolates between two quaternions, using spherical linear interpolation.
|
|
/// </summary>
|
|
/// <param name="start">Start quaternion.</param>
|
|
/// <param name="end">End quaternion.</param>
|
|
/// <param name="amount">Value between 0 and 1 indicating the weight of <paramref name="end"/>.</param>
|
|
/// <returns>The spherical linear interpolation of the two quaternions.</returns>
|
|
public static Quaternion Slerp( Quaternion start, Quaternion end, float amount )
|
|
{
|
|
Quaternion result;
|
|
Slerp( ref start, ref end, amount, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolates between quaternions, using spherical quadrangle interpolation.
|
|
/// </summary>
|
|
/// <param name="value1">First source quaternion.</param>
|
|
/// <param name="value2">Second source quaternion.</param>
|
|
/// <param name="value3">Thrid source quaternion.</param>
|
|
/// <param name="value4">Fourth source quaternion.</param>
|
|
/// <param name="amount">Value between 0 and 1 indicating the weight of interpolation.</param>
|
|
/// <param name="result">When the method completes, contains the spherical quadrangle interpolation of the quaternions.</param>
|
|
public static void Squad( ref Quaternion value1, ref Quaternion value2, ref Quaternion value3, ref Quaternion value4, float amount, out Quaternion result )
|
|
{
|
|
Quaternion start, end;
|
|
Slerp( ref value1, ref value4, amount, out start );
|
|
Slerp( ref value2, ref value3, amount, out end );
|
|
Slerp( ref start, ref end, 2.0f * amount * ( 1.0f - amount ), out result );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolates between quaternions, using spherical quadrangle interpolation.
|
|
/// </summary>
|
|
/// <param name="value1">First source quaternion.</param>
|
|
/// <param name="value2">Second source quaternion.</param>
|
|
/// <param name="value3">Thrid source quaternion.</param>
|
|
/// <param name="value4">Fourth source quaternion.</param>
|
|
/// <param name="amount">Value between 0 and 1 indicating the weight of interpolation.</param>
|
|
/// <returns>The spherical quadrangle interpolation of the quaternions.</returns>
|
|
public static Quaternion Squad( Quaternion value1, Quaternion value2, Quaternion value3, Quaternion value4, float amount )
|
|
{
|
|
Quaternion result;
|
|
Squad( ref value1, ref value2, ref value3, ref value4, amount, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Sets up control points for spherical quadrangle interpolation.
|
|
/// </summary>
|
|
/// <param name="value1">First source quaternion.</param>
|
|
/// <param name="value2">Second source quaternion.</param>
|
|
/// <param name="value3">Third source quaternion.</param>
|
|
/// <param name="value4">Fourth source quaternion.</param>
|
|
/// <returns>An array of three quaternions that represent control points for spherical quadrangle interpolation.</returns>
|
|
public static Quaternion[] SquadSetup( Quaternion value1, Quaternion value2, Quaternion value3, Quaternion value4 )
|
|
{
|
|
Quaternion q0 = ( value1 + value2 ).LengthSquared() < ( value1 - value2 ).LengthSquared() ? -value1 : value1;
|
|
Quaternion q2 = ( value2 + value3 ).LengthSquared() < ( value2 - value3 ).LengthSquared() ? -value3 : value3;
|
|
Quaternion q3 = ( value3 + value4 ).LengthSquared() < ( value3 - value4 ).LengthSquared() ? -value4 : value4;
|
|
Quaternion q1 = value2;
|
|
|
|
Quaternion q1Exp, q2Exp;
|
|
Exponential( ref q1, out q1Exp );
|
|
Exponential( ref q2, out q2Exp );
|
|
|
|
Quaternion[] results = new Quaternion[3];
|
|
results[0] = q1 * Exponential( -0.25f * ( Logarithm( q1Exp * q2 ) + Logarithm( q1Exp * q0 ) ) );
|
|
results[1] = q2 * Exponential( -0.25f * ( Logarithm( q2Exp * q3 ) + Logarithm( q2Exp * q1 ) ) );
|
|
results[2] = q2;
|
|
|
|
return results;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Adds two quaternions.
|
|
/// </summary>
|
|
/// <param name="left">The first quaternion to add.</param>
|
|
/// <param name="right">The second quaternion to add.</param>
|
|
/// <returns>The sum of the two quaternions.</returns>
|
|
public static Quaternion operator +( Quaternion left, Quaternion right )
|
|
{
|
|
Quaternion result;
|
|
Add( ref left, ref right, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Subtracts two quaternions.
|
|
/// </summary>
|
|
/// <param name="left">The first quaternion to subtract.</param>
|
|
/// <param name="right">The second quaternion to subtract.</param>
|
|
/// <returns>The difference of the two quaternions.</returns>
|
|
public static Quaternion operator -( Quaternion left, Quaternion right )
|
|
{
|
|
Quaternion result;
|
|
Subtract( ref left, ref right, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Reverses the direction of a given quaternion.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion to negate.</param>
|
|
/// <returns>A quaternion facing in the opposite direction.</returns>
|
|
public static Quaternion operator -( Quaternion value )
|
|
{
|
|
Quaternion result;
|
|
Negate( ref value, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Scales a quaternion by the given value.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion to scale.</param>
|
|
/// <param name="scale">The amount by which to scale the quaternion.</param>
|
|
/// <returns>The scaled quaternion.</returns>
|
|
public static Quaternion operator *( float scale, Quaternion value )
|
|
{
|
|
Quaternion result;
|
|
Multiply( ref value, scale, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Scales a quaternion by the given value.
|
|
/// </summary>
|
|
/// <param name="value">The quaternion to scale.</param>
|
|
/// <param name="scale">The amount by which to scale the quaternion.</param>
|
|
/// <returns>The scaled quaternion.</returns>
|
|
public static Quaternion operator *( Quaternion value, float scale )
|
|
{
|
|
Quaternion result;
|
|
Multiply( ref value, scale, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Multiplies a quaternion by another.
|
|
/// </summary>
|
|
/// <param name="left">The first quaternion to multiply.</param>
|
|
/// <param name="right">The second quaternion to multiply.</param>
|
|
/// <returns>The multiplied quaternion.</returns>
|
|
public static Quaternion operator *( Quaternion left, Quaternion right )
|
|
{
|
|
Quaternion result;
|
|
Multiply( ref left, ref right, out result );
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Tests for equality between two objects.
|
|
/// </summary>
|
|
/// <param name="left">The first value to compare.</param>
|
|
/// <param name="right">The second value to compare.</param>
|
|
/// <returns><c>true</c> if <paramref name="left"/> has the same value as <paramref name="right"/>; otherwise, <c>false</c>.</returns>
|
|
public static bool operator ==( Quaternion left, Quaternion right )
|
|
{
|
|
return left.Equals( right );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Tests for inequality between two objects.
|
|
/// </summary>
|
|
/// <param name="left">The first value to compare.</param>
|
|
/// <param name="right">The second value to compare.</param>
|
|
/// <returns><c>true</c> if <paramref name="left"/> has a different value than <paramref name="right"/>; otherwise, <c>false</c>.</returns>
|
|
public static bool operator !=( Quaternion left, Quaternion right )
|
|
{
|
|
return !left.Equals( right );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a <see cref="string"/> that represents this instance.
|
|
/// </summary>
|
|
/// <returns>
|
|
/// A <see cref="string"/> that represents this instance.
|
|
/// </returns>
|
|
public override string ToString()
|
|
{
|
|
return string.Format( CultureInfo.CurrentCulture, "X:{0} Y:{1} Z:{2} W:{3}", X, Y, Z, W );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a <see cref="string"/> that represents this instance.
|
|
/// </summary>
|
|
/// <param name="format">The format.</param>
|
|
/// <returns>
|
|
/// A <see cref="string"/> that represents this instance.
|
|
/// </returns>
|
|
public string ToString( string format )
|
|
{
|
|
if( format == null )
|
|
return ToString();
|
|
|
|
return string.Format( CultureInfo.CurrentCulture, "X:{0} Y:{1} Z:{2} W:{3}", X.ToString( format, CultureInfo.CurrentCulture ),
|
|
Y.ToString( format, CultureInfo.CurrentCulture ), Z.ToString( format, CultureInfo.CurrentCulture ), W.ToString( format, CultureInfo.CurrentCulture ) );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a <see cref="string"/> that represents this instance.
|
|
/// </summary>
|
|
/// <param name="formatProvider">The format provider.</param>
|
|
/// <returns>
|
|
/// A <see cref="string"/> that represents this instance.
|
|
/// </returns>
|
|
public string ToString( IFormatProvider formatProvider )
|
|
{
|
|
return string.Format( formatProvider, "X:{0} Y:{1} Z:{2} W:{3}", X, Y, Z, W );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a <see cref="string"/> that represents this instance.
|
|
/// </summary>
|
|
/// <param name="format">The format.</param>
|
|
/// <param name="formatProvider">The format provider.</param>
|
|
/// <returns>
|
|
/// A <see cref="string"/> that represents this instance.
|
|
/// </returns>
|
|
public string ToString( string format, IFormatProvider formatProvider )
|
|
{
|
|
if( format == null )
|
|
return ToString( formatProvider );
|
|
|
|
return string.Format( formatProvider, "X:{0} Y:{1} Z:{2} W:{3}", X.ToString( format, formatProvider ),
|
|
Y.ToString( format, formatProvider ), Z.ToString( format, formatProvider ), W.ToString( format, formatProvider ) );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Returns a hash code for this instance.
|
|
/// </summary>
|
|
/// <returns>
|
|
/// A hash code for this instance, suitable for use in hashing algorithms and data structures like a hash table.
|
|
/// </returns>
|
|
public override int GetHashCode()
|
|
{
|
|
return X.GetHashCode() + Y.GetHashCode() + Z.GetHashCode() + W.GetHashCode();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Determines whether the specified <see cref="math.Quaternion"/> is equal to this instance.
|
|
/// </summary>
|
|
/// <param name="other">The <see cref="math.Quaternion"/> to compare with this instance.</param>
|
|
/// <returns>
|
|
/// <c>true</c> if the specified <see cref="math.Quaternion"/> is equal to this instance; otherwise, <c>false</c>.
|
|
/// </returns>
|
|
public bool Equals( Quaternion other )
|
|
{
|
|
return ( (float)Math.Abs( other.X - X ) < MathUtil.ZeroTolerance &&
|
|
(float)Math.Abs( other.Y - Y ) < MathUtil.ZeroTolerance &&
|
|
(float)Math.Abs( other.Z - Z ) < MathUtil.ZeroTolerance &&
|
|
(float)Math.Abs( other.W - W ) < MathUtil.ZeroTolerance );
|
|
}
|
|
|
|
/// <summary>
|
|
/// Determines whether the specified <see cref="object"/> is equal to this instance.
|
|
/// </summary>
|
|
/// <param name="value">The <see cref="object"/> to compare with this instance.</param>
|
|
/// <returns>
|
|
/// <c>true</c> if the specified <see cref="object"/> is equal to this instance; otherwise, <c>false</c>.
|
|
/// </returns>
|
|
public override bool Equals( object value )
|
|
{
|
|
if( value == null )
|
|
return false;
|
|
|
|
if( value.GetType() != GetType() )
|
|
return false;
|
|
|
|
return Equals( (Quaternion)value );
|
|
}
|
|
|
|
#if SlimDX1xInterop
|
|
/// <summary>
|
|
/// Performs an implicit conversion from <see cref="math.Quaternion"/> to <see cref="SlimDX.Quaternion"/>.
|
|
/// </summary>
|
|
/// <param name="value">The value.</param>
|
|
/// <returns>The result of the conversion.</returns>
|
|
public static implicit operator SlimDX.Quaternion(Quaternion value)
|
|
{
|
|
return new SlimDX.Quaternion(value.X, value.Y, value.Z, value.W);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Performs an implicit conversion from <see cref="SlimDX.Quaternion"/> to <see cref="math.Quaternion"/>.
|
|
/// </summary>
|
|
/// <param name="value">The value.</param>
|
|
/// <returns>The result of the conversion.</returns>
|
|
public static implicit operator Quaternion(SlimDX.Quaternion value)
|
|
{
|
|
return new Quaternion(value.X, value.Y, value.Z, value.W);
|
|
}
|
|
#endif
|
|
|
|
#if WPFInterop
|
|
/// <summary>
|
|
/// Performs an implicit conversion from <see cref="math.Quaternion"/> to <see cref="System.Windows.Media.Media3D.Quaternion"/>.
|
|
/// </summary>
|
|
/// <param name="value">The value.</param>
|
|
/// <returns>The result of the conversion.</returns>
|
|
public static implicit operator System.Windows.Media.Media3D.Quaternion(Quaternion value)
|
|
{
|
|
return new System.Windows.Media.Media3D.Quaternion(value.X, value.Y, value.Z, value.W);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Performs an explicit conversion from <see cref="System.Windows.Media.Media3D.Quaternion"/> to <see cref="math.Quaternion"/>.
|
|
/// </summary>
|
|
/// <param name="value">The value.</param>
|
|
/// <returns>The result of the conversion.</returns>
|
|
public static explicit operator Quaternion(System.Windows.Media.Media3D.Quaternion value)
|
|
{
|
|
return new Quaternion((float)value.X, (float)value.Y, (float)value.Z, (float)value.W);
|
|
}
|
|
#endif
|
|
|
|
#if XnaInterop
|
|
/// <summary>
|
|
/// Performs an implicit conversion from <see cref="math.Quaternion"/> to <see cref="Microsoft.Xna.Framework.Quaternion"/>.
|
|
/// </summary>
|
|
/// <param name="value">The value.</param>
|
|
/// <returns>The result of the conversion.</returns>
|
|
public static implicit operator Microsoft.Xna.Framework.Quaternion(Quaternion value)
|
|
{
|
|
return new Microsoft.Xna.Framework.Quaternion(value.X, value.Y, value.Z, value.W);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Performs an implicit conversion from <see cref="Microsoft.Xna.Framework.Quaternion"/> to <see cref="math.Quaternion"/>.
|
|
/// </summary>
|
|
/// <param name="value">The value.</param>
|
|
/// <returns>The result of the conversion.</returns>
|
|
public static implicit operator Quaternion(Microsoft.Xna.Framework.Quaternion value)
|
|
{
|
|
return new Quaternion(value.X, value.Y, value.Z, value.W);
|
|
}
|
|
#endif
|
|
}
|
|
}
|