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));
}
private static Fix64 ClampedMove(Fix64 lhs, Fix64 rhs, Fix64 clampedDelta)
{
Fix64 delta = rhs - lhs;
if (delta > Fix64.Zero)
{
return(lhs + Fix64.Min(delta, clampedDelta));
}
return(lhs - Fix64.Min(-delta, clampedDelta));
}
public static FVec3 Min(FVec3 v, FVec3 v1)
{
return(new FVec3(Fix64.Min(v.x, v1.x), Fix64.Min(v.y, v1.y), Fix64.Min(v.z, v1.z)));
}
public static FVec4 Min(FVec4 v, FVec4 values)
{
return(new FVec4(Fix64.Min(v.x, values.x), Fix64.Min(v.y, values.y), Fix64.Min(v.z, values.z),
Fix64.Min(v.w, values.w)));
}
public static FVec4 Min(FVec4 v, Fix64 value)
{
return(new FVec4(Fix64.Min(v.x, value), Fix64.Min(v.y, value), Fix64.Min(v.z, value),
Fix64.Min(v.w, value)));
}