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