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);
}
public FVec3 RotateAround(Fix64 angle, FVec3 axis)
{
// rotate into world space
FQuat quaternion = FQuat.AngleAxis(Fix64.Zero, axis);
quaternion = quaternion.Conjugate();
FVec3 worldSpaceVector = this.Transform(quaternion);
// rotate back to vector space
quaternion = FQuat.AngleAxis(angle, axis);
worldSpaceVector = worldSpaceVector.Transform(quaternion);
return(worldSpaceVector);
}
public FVec3 Transform(FQuat quaternion)
{
Fix64 x2 = quaternion.x + quaternion.x;
Fix64 y2 = quaternion.y + quaternion.y;
Fix64 z2 = quaternion.z + quaternion.z;
Fix64 xx2 = quaternion.x * x2;
Fix64 xy2 = quaternion.x * y2;
Fix64 xz2 = quaternion.x * z2;
Fix64 yy2 = quaternion.y * y2;
Fix64 yz2 = quaternion.y * z2;
Fix64 zz2 = quaternion.z * z2;
Fix64 wx2 = quaternion.w * x2;
Fix64 wy2 = quaternion.w * y2;
Fix64 wz2 = quaternion.w * z2;
return(new FVec3
(
this.x * (Fix64.One - yy2 - zz2) + this.y * (xy2 - wz2) + this.z * (xz2 + wy2),
this.x * (xy2 + wz2) + this.y * (Fix64.One - xx2 - zz2) + this.z * (yz2 - wx2),
this.x * (xz2 - wy2) + this.y * (yz2 + wx2) + this.z * (Fix64.One - xx2 - yy2)
));
}
