public static Quaternion Slerp(Quaternion a, Quaternion b, float c)
{
float theta = Angle(a, b);
if (theta != 0)
{
float d = 1f / (float)Math.Sin(theta);
float s0 = (float)Math.Sin((1f - c) * theta);
float s1 = (float)Math.Sin(c * theta);
return new Quaternion(
(a.X * s0 + b.X * s1) * d,
(a.Y * s0 + b.Y * s1) * d,
(a.Z * s0 + b.Z * s1) * d,
(a.W * s0 + b.W * s1) * d);
}
else
{
return a;
}
}
public static Quaternion Normalize(Quaternion a)
{
return a / a.Length();
}
public static Quaternion Inverse(Quaternion a)
{
return new Quaternion(a.X, a.Y, a.Z, -a.W);
}
public static Quaternion Farthest(Quaternion a, Quaternion b)
{
Quaternion diff, sum;
diff = a - b;
sum = a + b;
if (Dot(diff, diff) > Dot(sum, sum))
return b;
return -b;
}
public static float Dot(Quaternion a, Quaternion b)
{
return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W;
}
public static float Angle(Quaternion a, Quaternion b)
{
float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared());
if (s == 0) throw new DivideByZeroException();
return (float)Math.Acos(Dot(a, b) / s);
}