유니티 VelocityEstimator

using UnityEngine;
using System.Collections;

//-------------------------------------------------------------------------
public class VelocityEstimator : MonoBehaviour
{
   [Tooltip( "How many frames to average over for computing velocity" )]
   public int velocityAverageFrames = 5;
   [Tooltip( "How many frames to average over for computing angular velocity" )]
   public int angularVelocityAverageFrames = 11;

   public bool estimateOnAwake = false;

   private Coroutine routine;
   private int sampleCount;
   private Vector3[] velocitySamples;
   private Vector3[] angularVelocitySamples;


   //-------------------------------------------------
   public void BeginEstimatingVelocity()
   {
      FinishEstimatingVelocity();

      routine = StartCoroutine( EstimateVelocityCoroutine() );
   }


   //-------------------------------------------------
   public void FinishEstimatingVelocity()
   {
      if ( routine != null )
      {
         StopCoroutine( routine );
         routine = null;
      }
   }


   //-------------------------------------------------
   public Vector3 GetVelocityEstimate()
   {
      // Compute average velocity
      Vector3 velocity = Vector3.zero;
      int velocitySampleCount = Mathf.Min( sampleCount, velocitySamples.Length );
      if ( velocitySampleCount != 0 )
      {
         for ( int i = 0; i < velocitySampleCount; i++ )
         {
            velocity += velocitySamples[i];
         }
         velocity *= ( 1.0f / velocitySampleCount );
      }

      return velocity;
   }


   //-------------------------------------------------
   public Vector3 GetAngularVelocityEstimate()
   {
      // Compute average angular velocity
      Vector3 angularVelocity = Vector3.zero;
      int angularVelocitySampleCount = Mathf.Min( sampleCount, angularVelocitySamples.Length );
      if ( angularVelocitySampleCount != 0 )
      {
         for ( int i = 0; i < angularVelocitySampleCount; i++ )
         {
            angularVelocity += angularVelocitySamples[i];
         }
         angularVelocity *= ( 1.0f / angularVelocitySampleCount );
      }

      return angularVelocity;
   }


   //-------------------------------------------------
   public Vector3 GetAccelerationEstimate()
   {
      Vector3 average = Vector3.zero;
      for ( int i = 2 + sampleCount - velocitySamples.Length; i < sampleCount; i++ )
      {
         if ( i < 2 )
            continue;

         int first = i - 2;
         int second = i - 1;

         Vector3 v1 = velocitySamples[first % velocitySamples.Length];
         Vector3 v2 = velocitySamples[second % velocitySamples.Length];
         average += v2 - v1;
      }
      average *= ( 1.0f / Time.deltaTime );
      return average;
   }


   //-------------------------------------------------
   void Awake()
   {
      velocitySamples = new Vector3[velocityAverageFrames];
      angularVelocitySamples = new Vector3[angularVelocityAverageFrames];

      if ( estimateOnAwake )
      {
         BeginEstimatingVelocity();
      }
   }


   //-------------------------------------------------
   private IEnumerator EstimateVelocityCoroutine()
   {
      sampleCount = 0;

      Vector3 previousPosition = transform.position;
      Quaternion previousRotation = transform.rotation;
      while ( true )
      {
         yield return new WaitForEndOfFrame();

         float velocityFactor = 1.0f / Time.deltaTime;

         int v = sampleCount % velocitySamples.Length;
         int w = sampleCount % angularVelocitySamples.Length;
         sampleCount++;

         // Estimate linear velocity
         velocitySamples[v] = velocityFactor * ( transform.position - previousPosition );

         // Estimate angular velocity
         Quaternion deltaRotation = transform.rotation * Quaternion.Inverse( previousRotation );

         float theta = 2.0f * Mathf.Acos( Mathf.Clamp( deltaRotation.w, -1.0f, 1.0f ) );
         if ( theta > Mathf.PI )
         {
            theta -= 2.0f * Mathf.PI;
         }

         Vector3 angularVelocity = new Vector3( deltaRotation.x, deltaRotation.y, deltaRotation.z );
         if ( angularVelocity.sqrMagnitude > 0.0f )
         {
            angularVelocity = theta * velocityFactor * angularVelocity.normalized;
         }

         angularVelocitySamples[w] = angularVelocity;

         previousPosition = transform.position;
         previousRotation = transform.rotation;
      }
   }
}