public Quaternion Slerp(Quaternion right, float blend)
{
if (blend < 0 || blend > 1.0f)
throw new QuaternionException("Attempting sphereical interpolation with invalid blend (blend < 0.0f || blend > 1.0f).");
if (LengthSquared == 0.0f)
{ if (right.LengthSquared == 0.0f)
return IdentityFactory;
else
return new Quaternion(right.X, right.Y, right.Z, right.W); }
else if (right.LengthSquared == 0.0f)
return new Quaternion(_x, _y, _z, _w);
float cosHalfAngle = _x * right.X + _y * right.Y + _z * right.Z + _w * right.W;
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
return new Quaternion(_x, _y, _z, _w);
else if (cosHalfAngle < 0.0f)
{
right = new Quaternion(-_x, -_y, -_z, -_w);
cosHalfAngle = -cosHalfAngle;
}
float halfAngle = (float)Math.Acos(cosHalfAngle);
float sinHalfAngle = Trigonometry.Sin(halfAngle);
float blendA = Trigonometry.Sin(halfAngle * (1.0f - blend)) / sinHalfAngle;
float blendB = Trigonometry.Sin(halfAngle * blend) / sinHalfAngle;
Quaternion result = new Quaternion(
blendA * _x + blendB * right.X,
blendA * _y + blendB * right.Y,
blendA * _z + blendB * right.Z,
blendA * _w + blendB * right.W);
if (result.LengthSquared > 0.0f)
return result.Normalize();
else
return IdentityFactory;
}
public Quaternion Lerp(Quaternion right, float blend)
{
if (blend < 0 || blend > 1.0f)
throw new QuaternionException("Attempting linear interpolation with invalid blend (blend < 0.0f || blend > 1.0f).");
if (LengthSquared == 0.0f)
{ if (right.LengthSquared == 0.0f)
return IdentityFactory;
else
return new Quaternion(right.X, right.Y, right.Z, right.W); }
else if (right.LengthSquared == 0.0f)
return new Quaternion(_x, _y, _z, _w);
Quaternion result = new Quaternion(
(1.0f - blend) * _x + blend * right.X,
(1.0f - blend) * _y + blend * right.Y,
(1.0f - blend) * _z + blend * right.Z,
(1.0f - blend) * _w + blend * right.W);
if (result.LengthSquared > 0.0f)
return result.Normalize();
else
return IdentityFactory;
}