public Quaternion Slerp(Quaternion q1, Quaternion q2, float time)
{
float angle = q1.GetDotProduct(q2);
if (angle < 0.0f)
{
q1 *= -1.0f;
angle *= -1.0f;
}
float scale;
float invscale;
if ((angle + 1.0f) > 0.05f)
{
if ((1.0f - angle) >= 0.05f) // spherical interpolation
{
float theta = (float)Math.Acos(angle);
float invsintheta = 1.0f / (float)Math.Sin(theta);
scale = (float)Math.Sin(theta * (1.0f - time)) * invsintheta;
invscale = (float)Math.Sin(theta * time) * invsintheta;
}
else // linear interploation
{
scale = 1.0f - time;
invscale = time;
}
}
else
{
q2 = new Quaternion(-q1.Y, q1.X, -q1.W, q1.Z);
scale = (float)Math.Sin(Math.PI * (0.5f - time));
invscale = (float)Math.Sin(Math.PI * time);
}
Quaternion t_tmp= (q1 * scale) + (q2 * invscale);
this.m_x = t_tmp.X;
this.m_y = t_tmp.Y;
this.m_z = t_tmp.Z;
this.m_w = t_tmp.W;
return this;
}