123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162 |
- //======= Copyright (c) Valve Corporation, All rights reserved. ===============
- //
- // Purpose: Estimates the velocity of an object based on change in position
- //
- //=============================================================================
- using UnityEngine;
- using System.Collections;
- namespace Valve.VR.InteractionSystem
- {
- //-------------------------------------------------------------------------
- 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;
- }
- }
- }
- }
|