public static fp3 RotateTowards(fp3 lhs, fp3 rhs, fp angleMove, fp magnitudeMove)
{
var lhsMag = fpmath.length(lhs);
var rhsMag = fpmath.length(rhs);
// both vectors are non-zero
if (lhsMag > VecMath.VECTOR3_EPSILON && rhsMag > VecMath.VECTOR3_EPSILON)
{
fp3 lhsNorm = lhs / lhsMag;
fp3 rhsNorm = rhs / rhsMag;
var dot = fpmath.dot(lhsNorm, rhsNorm);
// direction is almost the same
if (dot > fp.one - VecMath.VECTOR3_EPSILON)
{
return(VecMath.MoveTowards(lhs, rhs, magnitudeMove));
}
// directions are almost opposite
else if (dot < -fp.one + VecMath.VECTOR3_EPSILON)
{
var axis = VecMath.OrthoNormalVectorFast(lhsNorm);
var m = fpmatrix3x3.zero;
m.SetAxisAngle(axis, angleMove);
var rotated = m.MultiplyVector3(lhsNorm);
rotated *= VecMath.ClampedMove(lhsMag, rhsMag, magnitudeMove);
return(rotated);
}
// normal case
else
{
var angle = fpmath.acos(dot);
var axis = fpmath.normalize(fpmath.cross(lhsNorm, rhsNorm));
var m = fpmatrix3x3.zero;
m.SetAxisAngle(axis, fpmath.min(angleMove, angle));
var rotated = m.MultiplyVector3(lhsNorm);
rotated *= VecMath.ClampedMove(lhsMag, rhsMag, magnitudeMove);
return(rotated);
}
}
// at least one of the vectors is almost zero
else
{
return(VecMath.MoveTowards(lhs, rhs, magnitudeMove));
}
}
public static fp3 Slerp(fp3 lhs, fp3 rhs, fp t)
{
var lhsMag = fpmath.length(lhs);
var rhsMag = fpmath.length(rhs);
if (lhsMag < VecMath.VECTOR3_EPSILON || rhsMag < VecMath.VECTOR3_EPSILON)
{
return(VecMath.Lerp(lhs, rhs, t));
}
var lerpedMagnitude = fpmath.lerp(lhsMag, rhsMag, t);
var dot = fpmath.dot(lhs, rhs) / (lhsMag * rhsMag);
// direction is almost the same
if (dot > 1.0F - VecMath.VECTOR3_EPSILON)
{
return(VecMath.Lerp(lhs, rhs, t));
}
// directions are almost opposite
else if (dot < -1.0F + VecMath.VECTOR3_EPSILON)
{
var lhsNorm = lhs / lhsMag;
var axis = VecMath.OrthoNormalVectorFast(lhsNorm);
var m = fpmatrix3x3.zero;
m.SetAxisAngle(axis, fpmath.PI * t);
var slerped = m.MultiplyVector3(lhsNorm);
slerped *= lerpedMagnitude;
return(slerped);
}
// normal case
else
{
var axis = fpmath.cross(lhs, rhs);
var lhsNorm = lhs / lhsMag;
axis = fpmath.normalizesafe(axis);
var angle = fpmath.acos(dot) * t;
var m = fpmatrix3x3.zero;
m.SetAxisAngle(axis, angle);
var slerped = m.MultiplyVector3(lhsNorm);
slerped *= lerpedMagnitude;
return(slerped);
}
}
