public static Quaternion operator -(Quaternion q1, Quaternion q2)
{
Quaternion q = new Quaternion();
q.v = q1.v - q2.v;
q.w = q1.w - q2.w;
return q;
}
public Quaternion Normalize(Quaternion q)
{
return q / (float)Math.Sqrt(Dot(q, q));
}
public Quaternion Slerp(float t, Quaternion q1, Quaternion q2)
{
float cosTheta = Dot(q1, q2);
if (cosTheta > .9995f)
return Normalize((1.0f - t) * q1 + t * q2);
else
{
float theta = (float)Math.Acos(Utility.Clamp(cosTheta, -1.0f, 1.0f));
float thetap = theta * t;
Quaternion qperp = Normalize(q2 - q1 * cosTheta);
return q1 * (float)Math.Cos(thetap) + qperp * (float)Math.Sin(thetap);
}
}
public float Dot(Quaternion q1, Quaternion q2)
{
return Geometry.Geometry.Dot(q1.v, q2.v) + q1.w * q2.w;
}