/// <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); }