/// <summary>
/// Do Spherical linear interpolation between two quaternions
/// </summary>
/// <param name="q1">The first quaternion</param>
/// <param name="q2">The second quaternion</param>
/// <param name="blend">The blend factor</param>
/// <returns>A smooth blend between the given quaternions</returns>
public static Quaternion4F OtkSlerp(Quaternion4F q1, Quaternion4F q2, float blend)
{
// if either input is zero, return the other.
if (q1.LengthSquared == 0.0f)
{
if (q2.LengthSquared == 0.0f)
{
return(Identity);
}
return(q2);
}
if (q2.LengthSquared == 0.0f)
{
return(q1);
}
var cosHalfAngle = q1.W * q2.W + q1.Xyz.Dot(q2.Xyz);
if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
{
// angle = 0.0f, so just return one input.
return(q1);
}
if (cosHalfAngle < 0.0f)
{
q2.Xyz.Negate();
q2.W = -q2.W;
cosHalfAngle = -cosHalfAngle;
}
float blendA;
float blendB;
if (cosHalfAngle < 0.99f)
{
// do proper slerp for big angles
var halfAngle = (float)Math.Acos(cosHalfAngle);
var sinHalfAngle = (float)Math.Sin(halfAngle);
var oneOverSinHalfAngle = 1.0f / sinHalfAngle;
blendA = (float)Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle;
blendB = (float)Math.Sin(halfAngle * blend) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = 1.0f - blend;
blendB = blend;
}
var result = new Quaternion4F(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W);
if (result.LengthSquared > 0.0f)
{
result.Normalize();
return(result);
}
return(Identity);
}
public static Quaternion4F FromRadiansAxis(float radianes, float x, float y, float z)
{
var quaternion = new Quaternion4F();
var num2 = radianes * 0.5f;
var num = (float)Math.Sin(num2);
var num3 = (float)Math.Cos(num2);
quaternion._x = x * num;
quaternion._y = y * num;
quaternion._z = z * num;
quaternion._w = num3;
quaternion.Normalize();
return(quaternion);
}
