/// <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)
{
float opposite;
float inverse;
float dot = DotProduct(start, end);
if (Mathematics.Abs(dot) > 1f - 0f)
{
inverse = 1f - amount;
opposite = amount * Mathematics.Sign(dot);
}
else
{
float acos = Mathematics.Acos(Mathematics.Abs(dot));
float invSin = (float)(1f / Mathematics.Sin(acos));
inverse = Mathematics.Sin((1f - amount) * acos) * invSin;
opposite = Mathematics.Sin(amount * acos) * invSin * Mathematics.Sign(dot);
}
return(new Quaternion(
(inverse * start.X) + (opposite * end.X),
(inverse * start.Y) + (opposite * end.Y),
(inverse * start.Z) + (opposite * end.Z),
(inverse * start.W) + (opposite * end.W)
));
}
/// <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 = new Quaternion();
if (Mathematics.Abs(value.W) < 1f)
{
float angle = Mathematics.Acos(value.W);
float sin = Mathematics.Sin(angle);
if (Mathematics.Abs(sin) >= 0f)
{
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 = 0f;
return(result);
}
/// <summary>
/// Initializes a new instance of the struct
/// </summary>
/// <param name="value">A vector containing the values with which to initialize the X, Y, and Z components</param>
/// <param name="angle">Initial value for the angle of the quaternion</param>
public Quaternion(Vector3 axis, float angle)
{
float angle2 = angle * 0.5f;
float s = Mathematics.Sin(angle2) / Vector3.Magnitude(axis);
X = axis.X * s;
Y = axis.Y * s;
Z = axis.Z * s;
W = Mathematics.Cos(angle2);
}
/// <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(Vector3 axis, float angle)
{
Vector3 normalized;
normalized = Vector3.Normalize(axis);
float half = angle * 0.5f;
float sin = Mathematics.Sin(half);
float cos = Mathematics.Cos(half);
return(new Quaternion(normalized.X * sin, normalized.Y * sin, normalized.Z * sin, cos));
}
/// <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)
{
float halfRoll = roll * 0.5f;
float halfPitch = pitch * 0.5f;
float halfYaw = yaw * 0.5f;
float sinRoll = Mathematics.Sin(halfRoll);
float cosRoll = Mathematics.Cos(halfRoll);
float sinPitch = Mathematics.Sin(halfPitch);
float cosPitch = Mathematics.Cos(halfPitch);
float sinYaw = Mathematics.Sin(halfYaw);
float cosYaw = Mathematics.Cos(halfYaw);
return(new Quaternion(
(cosYaw * sinPitch * cosRoll) + (sinYaw * cosPitch * sinRoll),
(sinYaw * cosPitch * cosRoll) - (cosYaw * sinPitch * sinRoll),
(cosYaw * cosPitch * sinRoll) - (sinYaw * sinPitch * cosRoll),
(cosYaw * cosPitch * cosRoll) + (sinYaw * sinPitch * sinRoll)
));
}
/// <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 = new Quaternion();
float angle = Mathematics.Sqrt((value.X * value.X) + (value.Y * value.Y) + (value.Z * value.Z));
float sin = Mathematics.Sin(angle);
if (Mathematics.Abs(sin) >= 0f)
{
float coeff = sin / angle;
result.X = coeff * value.X;
result.Y = coeff * value.Y;
result.Z = coeff * value.Z;
}
else
{
result = value;
}
result.W = Mathematics.Cos(angle);
return(result);
}