public static Fix Angle(FixVec2 from, FixVec2 to)
{
Fix denominator = FixMath.Sqrt(from.GetMagnitudeSquared() * to.GetMagnitudeSquared());
if (denominator < Fix.Epsilon)
{
return(Fix.zero);
}
Fix dot = FixMath.Clamp(FixVec2.Dot(from, to) / denominator, -Fix.one, Fix.one);
return(FixMath.Acos(dot) * FixMath.Rad2Deg);
}
/// <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 FixQuaternion Slerp(FixQuaternion q1, FixQuaternion q2, Fix blend)
{
// if either input is zero, return the other.
if (q1.LengthSquared == Fix.Zero)
{
if (q2.LengthSquared == Fix.Zero)
{
return(Identity);
}
return(q2);
}
else if (q2.LengthSquared == Fix.Zero)
{
return(q1);
}
Fix cosHalfAngle = q1.W * q2.W + q1.Xyz.Dot(q2.Xyz);
if (cosHalfAngle >= Fix.One || cosHalfAngle <= -Fix.One)
{
// angle = 0.0f, so just return one input.
return(q1);
}
else if (cosHalfAngle < Fix.Zero)
{
q2.Xyz = -q2.Xyz;
q2.W = -q2.W;
cosHalfAngle = -cosHalfAngle;
}
Fix blendA;
Fix blendB;
if (cosHalfAngle < (Fix.One / 100) * 99)
{
// do proper slerp for big angles
Fix halfAngle = FixMath.Acos(cosHalfAngle);
Fix sinHalfAngle = FixMath.Sin(halfAngle);
Fix oneOverSinHalfAngle = Fix.One / sinHalfAngle;
blendA = FixMath.Sin(halfAngle * (Fix.One - blend)) * oneOverSinHalfAngle;
blendB = FixMath.Sin(halfAngle * blend) * oneOverSinHalfAngle;
}
else
{
// do lerp if angle is really small.
blendA = Fix.One - blend;
blendB = blend;
}
FixQuaternion result = new FixQuaternion(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W);
if (result.LengthSquared > Fix.Zero)
{
return(Normalize(result));
}
else
{
return(Identity);
}
}