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); } }