public static Quaternion4F Slerp(Quaternion4F q0, Quaternion4F q1, float t)
{
var qresult = new Quaternion4F();
// Check for out-of range parameter and return edge points if so
if (t <= 0.0)
{
qresult.CopyFrom(q0);
return(qresult);
}
if (t >= 1.0)
{
qresult.CopyFrom(q1);
return(qresult);
}
// Compute "cosine of angle between quaternions" using dot product
var cosOmega = Dot(q0, q1);
// If negative dot, use -q1. Two quaternions q and -q
// represent the same rotation, but may produce
// different slerp. We chose q or -q to rotate using
// the acute angle.
var q1W = q1._w;
var q1X = q1._x;
var q1Y = q1._y;
var q1Z = q1._z;
if (cosOmega < 0.0)
{
q1W = -q1W;
q1X = -q1X;
q1Y = -q1Y;
q1Z = -q1Z;
cosOmega = -cosOmega;
}
// We should have two unit quaternions, so dot should be <= 1.0
if (!(cosOmega < 1.1f))
{
throw new Exception("Error en Quaternion.Slerp(), cosOmega > 1.1f");
}
// Compute interpolation fraction, checking for quaternions
// almost exactly the same
float k0, k1;
if (cosOmega > 0.9999)
{
// Very close - just use linear interpolation,
// which will protect againt a divide by zero
k0 = 1.0f - t;
k1 = t;
}
else
{
// Compute the sin of the angle using the
// trig identity sin^2(omega) + cos^2(omega) = 1
var sinOmega = (float)Math.Sqrt(1.0 - (cosOmega * cosOmega));
// Compute the angle from its sin and cosine
var omega = (float)Math.Atan2(sinOmega, cosOmega);
// Compute inverse of denominator, so we only have
// to divide once
var oneOverSinOmega = 1.0f / sinOmega;
// Compute interpolation parameters
k0 = (float)(Math.Sin((1.0 - t) * omega) * oneOverSinOmega);
k1 = (float)(Math.Sin(t * omega) * oneOverSinOmega);
}
// Interpolate and return new quaternion
return(new Quaternion4F(
(k0 * q0._w) + (k1 * q1W),
(k0 * q0._x) + (k1 * q1X),
(k0 * q0._y) + (k1 * q1Y),
(k0 * q0._z) + (k1 * q1Z)
));
}
