/// <summary>
/// Simple velocity driven filter for Quaternion
/// </summary>
/// <param name="prevpos">Previous position</param>
/// <param name="target">Target position</param>
/// <param name="velocity">Maximum angular velocity (1 = INF)</param>
/// <param name="epsilon">Distance to be considered close enough to target to return target</param>
/// <returns>New position</returns>
public static Quaternion Velocity(Quaternion prevpos, Quaternion target, float velocity, float epsilon = 0.00001f)
{
var diffang = prevpos.AngleDiff(target) / (float)(PI * 2);
if (diffang < epsilon)
{
return(target);
}
return(Quaternion.Slerp(prevpos, target, Min(velocity, diffang)));
}
/// <summary>
/// Inertial, force driven filter for Quaternion
/// </summary>
/// <param name="prevpos">Previous position</param>
/// <param name="prevvel">Previous angular velocity</param>
/// <param name="target">Target position</param>
/// <param name="force">Maximum angular force</param>
/// <param name="newpos">New position</param>
/// <param name="newvel">New angular velocity</param>
public static void Inertial(Quaternion prevpos, float prevvel, Quaternion target, float force, out Quaternion newpos, out float newvel)
{
var d = prevpos.AngleDiff(target) / (float)(PI * 2);
if (d < 0.00001)
{
newpos = target;
newvel = 0;
return;
}
var f = Min(force, d);
newvel = prevvel + f;
newpos = Quaternion.Slerp(prevpos, target, newvel);
}
