// Spherical linear interpolation
public static FVec3 RotateTowards(FVec3 from, FVec3 to, float maxThetaRad)
{
// Normalize the vectors
FVec3 unitfrom = from.GetNormal(),
unitto = to.GetNormal();
// Calculate the included angle
//if (FVec3.IsNan(unitfrom)) //UnityEngine.Debug.LogError("From is NULL");
//if (FVec3.IsNan(to)) //UnityEngine.Debug.LogError("To is NULL");
double theta =
Math.Acos(unitfrom.DotProduct(unitto));
if (theta < 0.05 || Double.IsNaN(theta))
{
return(to);
}
// Avoid the repeated sine calculation
double st =
Math.Sin(theta);
if (st == 0 || st < 0.01)
{
return(to);
}
// Return the geometric spherical linear interpolation
float alpha = (float)Math.Min(theta, maxThetaRad);
return
(from * (float)(Math.Sin(theta - alpha) / st) +
to * (float)Math.Sin(alpha) / (float)st);
}
public static double AngleBetween(FVec3 A, FVec3 B)
{
FVec3 tempA = A.Clone();
FVec3 tempB = B.Clone();
tempA.Normalise();
tempB.Normalise();
return(Math.Acos(FVec3.DotProduct(tempA, tempB)));
}
// Spherical linear interpolation
public static FVec3 Slerp(FVec3 from, FVec3 to, float step)
{
if (step == 0)
{
return(from);
}
if (from == to || step == 1)
{
return(to);
}
// Normalize the vectors
FVec3 unitfrom = from.GetNormal(),
unitto = to.GetNormal();
// Calculate the included angle
double theta =
Math.Acos(unitfrom.DotProduct(unitto));
if (theta < 0.1 || Double.IsNaN(theta))
{
return(to);
}
// Avoid the repeated sine calculation
double st =
Math.Sin(theta);
if (st == 0 || st < 0.01)
{
return(to);
}
// Return the geometric spherical linear interpolation
return
(from * (float)(Math.Sin((1 - step) * theta) / st) +
to * (float)Math.Sin(step * theta) / (float)st);
}
