/// <summary>
/// Performs spherical interpolation between two quaternions. Spherical interpolation neatly interpolates between
/// two rotations without modifying the size of the vector it is applied to (unlike linear interpolation).
/// </summary>
/// <param name="from">Start quaternion.</param>
/// <param name="to">End quaternion.</param>
/// <param name="t">Interpolation factor in range [0, 1] that determines how much to interpolate between
/// <paramref name="from"/> and <paramref name="to"/>.</param>
/// <param name="shortestPath">Should the interpolation be performed between the shortest or longest path between
/// the two quaternions.</param>
/// <returns>Interpolated quaternion representing a rotation between <paramref name="from"/> and
/// <paramref name="to"/>.</returns>
public static Quaternion Slerp(Quaternion from, Quaternion to, float t, bool shortestPath = true)
{
float dot = Dot(from, to);
Quaternion quat;
if (dot < 0.0f && shortestPath)
{
dot = -dot;
quat = -to;
}
else
{
quat = to;
}
if (MathEx.Abs(dot) < (1 - epsilon))
{
float sin = MathEx.Sqrt(1 - (dot * dot));
Radian angle = MathEx.Atan2(sin, dot);
float invSin = 1.0f / sin;
float a = MathEx.Sin((1.0f - t) * angle) * invSin;
float b = MathEx.Sin(t * angle) * invSin;
return(a * from + b * quat);
}
else
{
Quaternion ret = (1.0f - t) * from + t * quat;
ret.Normalize();
return(ret);
}
}
/// <summary>
/// Normalizes the quaternion.
/// </summary>
/// <returns>Length of the quaternion prior to normalization.</returns>
public float Normalize()
{
float len = w * w + x * x + y * y + z * z;
float factor = 1.0f / (float)MathEx.Sqrt(len);
x *= factor;
y *= factor;
z *= factor;
w *= factor;
return(len);
}
/// <summary>
/// Creates a quaternion from a rotation matrix.
/// </summary>
/// <param name="rotMatrix">Rotation matrix to convert to quaternion.</param>
/// <returns>Newly created quaternion that has equivalent rotation as the provided rotation matrix.</returns>
public static Quaternion FromRotationMatrix(Matrix3 rotMatrix)
{
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
// article "Quaternion Calculus and Fast Animation".
Quaternion quat = new Quaternion();
float trace = rotMatrix.m00 + rotMatrix.m11 + rotMatrix.m22;
float root;
if (trace > 0.0f)
{
// |w| > 1/2, may as well choose w > 1/2
root = MathEx.Sqrt(trace + 1.0f); // 2w
quat.w = 0.5f * root;
root = 0.5f / root; // 1/(4w)
quat.x = (rotMatrix.m21 - rotMatrix.m12) * root;
quat.y = (rotMatrix.m02 - rotMatrix.m20) * root;
quat.z = (rotMatrix.m10 - rotMatrix.m01) * root;
}
else
{
// |w| <= 1/2
int[] nextLookup = { 1, 2, 0 };
int i = 0;
if (rotMatrix.m11 > rotMatrix.m00)
{
i = 1;
}
if (rotMatrix.m22 > rotMatrix[i, i])
{
i = 2;
}
int j = nextLookup[i];
int k = nextLookup[j];
root = MathEx.Sqrt(rotMatrix[i, i] - rotMatrix[j, j] - rotMatrix[k, k] + 1.0f);
quat[i] = 0.5f * root;
root = 0.5f / root;
quat.w = (rotMatrix[k, j] - rotMatrix[j, k]) * root;
quat[j] = (rotMatrix[j, i] + rotMatrix[i, j]) * root;
quat[k] = (rotMatrix[k, i] + rotMatrix[i, k]) * root;
}
quat.Normalize();
return(quat);
}
/// <summary>
/// Initializes the quaternion with rotation that rotates from one direction to another.
/// </summary>
/// <param name="fromDirection">Rotation to start at.</param>
/// <param name="toDirection">Rotation to end at.</param>
/// <param name="fallbackAxis">Fallback axis to use if the from/to vectors are almost completely opposite.
/// Fallback axis should be perpendicular to both vectors.</param>
public void SetFromToRotation(Vector3 fromDirection, Vector3 toDirection, Vector3 fallbackAxis)
{
fromDirection.Normalize();
toDirection.Normalize();
float d = Vector3.Dot(fromDirection, toDirection);
// If dot == 1, vectors are the same
if (d >= 1.0f)
{
this = Identity;
return;
}
if (d < (1e-6f - 1.0f))
{
if (fallbackAxis != Vector3.Zero)
{
// Rotate 180 degrees about the fallback axis
this = FromAxisAngle(fallbackAxis, MathEx.Pi);
}
else
{
// Generate an axis
Vector3 axis = Vector3.Cross(Vector3.XAxis, fromDirection);
if (axis.SqrdLength < ((1e-06f * 1e-06f))) // Pick another if collinear
{
axis = Vector3.Cross(Vector3.YAxis, fromDirection);
}
axis.Normalize();
this = FromAxisAngle(axis, MathEx.Pi);
}
}
else
{
float s = MathEx.Sqrt((1 + d) * 2);
float invs = 1 / s;
Vector3 c = Vector3.Cross(fromDirection, toDirection);
x = c.x * invs;
y = c.y * invs;
z = c.z * invs;
w = s * 0.5f;
Normalize();
}
}
/// <summary>
/// Calculates the distance between two points.
/// </summary>
/// <param name="a">First three dimensional point.</param>
/// <param name="b">Second three dimensional point.</param>
/// <returns>Distance between the two points.</returns>
public static float Distance(Vector3 a, Vector3 b)
{
Vector3 vector3 = new Vector3(a.x - b.x, a.y - b.y, a.z - b.z);
return(MathEx.Sqrt(vector3.x * vector3.x + vector3.y * vector3.y + vector3.z * vector3.z));
}
/// <summary>
/// Calculates the magnitude of the provided vector.
/// </summary>
/// <param name="v">Vector to calculate the magnitude for.</param>
/// <returns>Magnitude of the vector.</returns>
public static float Magnitude(Vector3 v)
{
return(MathEx.Sqrt(v.x * v.x + v.y * v.y + v.z * v.z));
}
/// <summary>
/// Converts an orthonormal matrix to axis angle representation.
/// </summary>
/// <param name="axis">Axis around which the rotation is performed.</param>
/// <param name="angle">Amount of rotation.</param>
public void ToAxisAngle(out Vector3 axis, out Degree angle)
{
float trace = m00 + m11 + m22;
float cos = 0.5f * (trace - 1.0f);
Radian radians = (Radian)MathEx.Acos(cos); // In [0, PI]
angle = radians;
if (radians > (Radian)0.0f)
{
if (radians < MathEx.Pi)
{
axis.x = m21 - m12;
axis.y = m02 - m20;
axis.z = m10 - m01;
axis.Normalize();
}
else
{
// Angle is PI
float halfInverse;
if (m00 >= m11)
{
// r00 >= r11
if (m00 >= m22)
{
// r00 is maximum diagonal term
axis.x = 0.5f * MathEx.Sqrt(m00 - m11 - m22 + 1.0f);
halfInverse = 0.5f / axis.x;
axis.y = halfInverse * m01;
axis.z = halfInverse * m02;
}
else
{
// r22 is maximum diagonal term
axis.z = 0.5f * MathEx.Sqrt(m22 - m00 - m11 + 1.0f);
halfInverse = 0.5f / axis.z;
axis.x = halfInverse * m02;
axis.y = halfInverse * m12;
}
}
else
{
// r11 > r00
if (m11 >= m22)
{
// r11 is maximum diagonal term
axis.y = 0.5f * MathEx.Sqrt(m11 - m00 - m22 + 1.0f);
halfInverse = 0.5f / axis.y;
axis.x = halfInverse * m01;
axis.z = halfInverse * m12;
}
else
{
// r22 is maximum diagonal term
axis.z = 0.5f * MathEx.Sqrt(m22 - m00 - m11 + 1.0f);
halfInverse = 0.5f / axis.z;
axis.x = halfInverse * m02;
axis.y = halfInverse * m12;
}
}
}
}
else
{
// The angle is 0 and the matrix is the identity. Any axis will
// work, so just use the x-axis.
axis.x = 1.0f;
axis.y = 0.0f;
axis.z = 0.0f;
}
}
/// <summary>
/// Calculates the distance between two points.
/// </summary>
/// <param name="a">First four dimensional point.</param>
/// <param name="b">Second four dimensional point.</param>
/// <returns>Distance between the two points.</returns>
public static float Distance(Vector4 a, Vector4 b)
{
Vector4 vector4 = new Vector4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
return(MathEx.Sqrt(vector4.x * vector4.x + vector4.y * vector4.y + vector4.z * vector4.z + vector4.w * vector4.w));
}
/// <summary>
/// Calculates the distance between two points.
/// </summary>
/// <param name="a">First two dimensional point.</param>
/// <param name="b">Second two dimensional point.</param>
/// <returns>Distance between the two points.</returns>
public static float Distance(Vector2 a, Vector2 b)
{
Vector2 vector2 = new Vector2(a.x - b.x, a.y - b.y);
return(MathEx.Sqrt(vector2.x * vector2.x + vector2.y * vector2.y));
}
