/// <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;
}
/// <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;
}
/// <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;
}
}
/// <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;
}
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);
}
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;
}
public static Quaternion Sub(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;
}
/// <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>
/// Build a rotation matrix from a quaternion
/// </summary>
/// <param name="q">the quaternion</param>
/// <returns>A rotation matrix</returns>
public static Matrix4 Rotate(Quaternion q)
{
Vector3 axis;
float angle;
q.ToAxisAngle(out axis, out angle);
return Rotate(axis, angle);
}
