/// <summary>
/// Spherical linear interpolation.
///
/// Assumes q1 and q2 are normalized and that t in [0,1].
///
/// This method does interpolate along the shortest arc
/// between q1 and q2.
/// </summary>
/// <param name="q1"></param>
/// <param name="q2"></param>
/// <param name="t">[0,1]</param>
/// <returns>Interpolant</returns>
public static Rot3f SlerpShortest(
Rot3f q1, Rot3f q2, double t
)
{
Rot3f q3 = q2;
float cosomega = Fun.Clamp(Rot3f.Dot(q1, q3), -1, 1);
if (cosomega < 0.0)
{
cosomega = -cosomega;
q3 *= -1; //q3 = -q3;
}
if (cosomega >= 1.0)
{
// Special case: q1 and q2 are the same, so just return one of them.
// This also catches the case where cosomega is very slightly > 1.0
return(q1);
}
double sinomega = System.Math.Sqrt(1 - cosomega * cosomega);
Rot3f result = new Rot3f();
if (sinomega * double.MaxValue > 1)
{
double omega = System.Math.Acos(cosomega);
float s1 = (float)(System.Math.Sin((1.0 - t) * omega) / sinomega);
float s2 = (float)(System.Math.Sin(t * omega) / sinomega);
result = s1 * q1 + s2 * q3;
}
else if (cosomega > 0)
{
// omega == 0
float s1 = 1.0f - (float)t;
float s2 = (float)t;
result = s1 * q1 + s2 * q3;
}
else
{
// omega == -pi
result.X = -q1.Y;
result.Y = q1.X;
result.Z = -q1.W;
result.W = q1.Z;
float s1 = (float)System.Math.Sin((0.5 - t) * System.Math.PI);
float s2 = (float)System.Math.Sin(t * System.Math.PI);
result = s1 * q1 + s2 * result;
}
return(result);
}
