/// <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="a"></param>
/// <param name="b"></param>
/// <param name="t">[0,1]</param>
/// <returns>Interpolant</returns>
public static Rot3d SlerpShortest(
Rot3d a, Rot3d b, double t
)
{
Rot3d q3 = b;
double cosomega = Rot3d.Dot(a, q3);
if (cosomega < 0.0)
{
cosomega = -cosomega;
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(a);
}
double sinomega = (1 - cosomega * cosomega).Sqrt();
Rot3d result;
if (sinomega * double.MaxValue > 1)
{
double omega = cosomega.Acos();
double s1 = ((1.0 - t) * omega).Sin() / sinomega;
double s2 = (t * omega).Sin() / sinomega;
result = s1 * a + s2 * q3;
}
else if (cosomega > 0)
{
// omega == 0
double s1 = 1.0 - t;
double s2 = t;
result = s1 * a + s2 * q3;
}
else
{
// omega == -pi
result = new Rot3d(a.Z, -a.Y, a.X, -a.W);
double s1 = ((0.5 - t) * Constant.Pi).Sin();
double s2 = (t * Constant.Pi).Sin();
result = s1 * a + s2 * result;
}
return(result);
}