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 / Math.Sqrt(Dot(q, q));
}
public Quaternion Slerp(double t, Quaternion q1, Quaternion q2)
{
double cosTheta = Dot(q1, q2);
if (cosTheta > .9995f)
return Normalize((1.0d - t) * q1 + t * q2);
else
{
double theta = Math.Acos(Utility.Clamp(cosTheta, -1.0d, 1.0d));
double thetap = theta * t;
Quaternion qperp = Normalize(q2 - q1 * cosTheta);
return q1 * Math.Cos(thetap) + qperp * Math.Sin(thetap);
}
}
public double Dot(Quaternion q1, Quaternion q2)
{
return Geometry.Dot(q1.v, q2.v) + q1.w * q2.w;
}