/// <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); }