public static FVec3 RotateTowards(FVec3 current, FVec3 target, Fix64 maxRadiansDelta, Fix64 maxMagnitudeDelta)
{
Fix64 len1 = current.Magnitude();
Fix64 len2 = target.Magnitude();
if (len1 > Fix64.Epsilon && len2 > Fix64.Epsilon)
{
FVec3 from = current / len1;
FVec3 to = target / len2;
Fix64 cosom = Dot(from, to);
if (cosom > Fix64.One - Fix64.Epsilon)
{
return(MoveTowards(current, target, maxMagnitudeDelta));
}
if (cosom < -Fix64.One + Fix64.Epsilon)
{
FQuat q = FQuat.AngleAxis(maxRadiansDelta * Fix64.RAD_TO_DEG, OrthoNormalVector(from));
return(q * from * ClampedMove(len1, len2, maxMagnitudeDelta));
}
else
{
Fix64 angle = Fix64.Acos(cosom);
FQuat q = FQuat.AngleAxis(Fix64.Min(maxRadiansDelta, angle) * Fix64.RAD_TO_DEG,
Normalize(Cross(from, to)));
return(q * from * ClampedMove(len1, len2, maxMagnitudeDelta));
}
}
return(MoveTowards(current, target, maxMagnitudeDelta));
}
public static FVec3 SlerpUnclamped(FVec3 from, FVec3 to, Fix64 t)
{
Fix64 scale0, scale1;
Fix64 len2 = to.Magnitude();
Fix64 len1 = from.Magnitude();
FVec3 v2 = to / len2;
FVec3 v1 = from / len1;
Fix64 len = (len2 - len1) * t + len1;
Fix64 cosom = Dot(v1, v2);
if (cosom > ( Fix64 )(1 - 1e-6))
{
scale0 = Fix64.One - t;
scale1 = t;
}
else if (cosom < ( Fix64 )(-1 + 1e-6))
{
FVec3 axis = OrthoNormalVector(from);
FQuat q = FQuat.AngleAxis(( Fix64 )180 * t, axis);
FVec3 v = q * from * len;
return(v);
}
else
{
Fix64 omega = Fix64.Acos(cosom);
Fix64 sinom = Fix64.Sin(omega);
scale0 = Fix64.Sin((Fix64.One - t) * omega) / sinom;
scale1 = Fix64.Sin(t * omega) / sinom;
}
v2 = (v2 * scale1 + v1 * scale0) * len;
return(v2);
}