/// <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(Rot.Dot(q1, q3), -1, 1);
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(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 = new Rot3f(s1 * q1 + s2 * q3);
}
else if (cosomega > 0)
{
// omega == 0
float s1 = 1.0f - (float)t;
float s2 = (float)t;
result = new Rot3f(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 = new Rot3f(s1 * q1 + s2 * result);
}
return(result);
}
/// <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 = Rot.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 = new Rot3d(s1 * a + s2 * q3);
}
else if (cosomega > 0)
{
// omega == 0
double s1 = 1.0 - t;
double s2 = t;
result = new Rot3d(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 = new Rot3d(s1 * a + s2 * result);
}
return(result);
}