VelocityEstimator.cs 4.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162
  1. //======= Copyright (c) Valve Corporation, All rights reserved. ===============
  2. //
  3. // Purpose: Estimates the velocity of an object based on change in position
  4. //
  5. //=============================================================================
  6. using UnityEngine;
  7. using System.Collections;
  8. namespace Valve.VR.InteractionSystem
  9. {
  10. //-------------------------------------------------------------------------
  11. public class VelocityEstimator : MonoBehaviour
  12. {
  13. [Tooltip( "How many frames to average over for computing velocity" )]
  14. public int velocityAverageFrames = 5;
  15. [Tooltip( "How many frames to average over for computing angular velocity" )]
  16. public int angularVelocityAverageFrames = 11;
  17. public bool estimateOnAwake = false;
  18. private Coroutine routine;
  19. private int sampleCount;
  20. private Vector3[] velocitySamples;
  21. private Vector3[] angularVelocitySamples;
  22. //-------------------------------------------------
  23. public void BeginEstimatingVelocity()
  24. {
  25. FinishEstimatingVelocity();
  26. routine = StartCoroutine( EstimateVelocityCoroutine() );
  27. }
  28. //-------------------------------------------------
  29. public void FinishEstimatingVelocity()
  30. {
  31. if ( routine != null )
  32. {
  33. StopCoroutine( routine );
  34. routine = null;
  35. }
  36. }
  37. //-------------------------------------------------
  38. public Vector3 GetVelocityEstimate()
  39. {
  40. // Compute average velocity
  41. Vector3 velocity = Vector3.zero;
  42. int velocitySampleCount = Mathf.Min( sampleCount, velocitySamples.Length );
  43. if ( velocitySampleCount != 0 )
  44. {
  45. for ( int i = 0; i < velocitySampleCount; i++ )
  46. {
  47. velocity += velocitySamples[i];
  48. }
  49. velocity *= ( 1.0f / velocitySampleCount );
  50. }
  51. return velocity;
  52. }
  53. //-------------------------------------------------
  54. public Vector3 GetAngularVelocityEstimate()
  55. {
  56. // Compute average angular velocity
  57. Vector3 angularVelocity = Vector3.zero;
  58. int angularVelocitySampleCount = Mathf.Min( sampleCount, angularVelocitySamples.Length );
  59. if ( angularVelocitySampleCount != 0 )
  60. {
  61. for ( int i = 0; i < angularVelocitySampleCount; i++ )
  62. {
  63. angularVelocity += angularVelocitySamples[i];
  64. }
  65. angularVelocity *= ( 1.0f / angularVelocitySampleCount );
  66. }
  67. return angularVelocity;
  68. }
  69. //-------------------------------------------------
  70. public Vector3 GetAccelerationEstimate()
  71. {
  72. Vector3 average = Vector3.zero;
  73. for ( int i = 2 + sampleCount - velocitySamples.Length; i < sampleCount; i++ )
  74. {
  75. if ( i < 2 )
  76. continue;
  77. int first = i - 2;
  78. int second = i - 1;
  79. Vector3 v1 = velocitySamples[first % velocitySamples.Length];
  80. Vector3 v2 = velocitySamples[second % velocitySamples.Length];
  81. average += v2 - v1;
  82. }
  83. average *= ( 1.0f / Time.deltaTime );
  84. return average;
  85. }
  86. //-------------------------------------------------
  87. void Awake()
  88. {
  89. velocitySamples = new Vector3[velocityAverageFrames];
  90. angularVelocitySamples = new Vector3[angularVelocityAverageFrames];
  91. if ( estimateOnAwake )
  92. {
  93. BeginEstimatingVelocity();
  94. }
  95. }
  96. //-------------------------------------------------
  97. private IEnumerator EstimateVelocityCoroutine()
  98. {
  99. sampleCount = 0;
  100. Vector3 previousPosition = transform.position;
  101. Quaternion previousRotation = transform.rotation;
  102. while ( true )
  103. {
  104. yield return new WaitForEndOfFrame();
  105. float velocityFactor = 1.0f / Time.deltaTime;
  106. int v = sampleCount % velocitySamples.Length;
  107. int w = sampleCount % angularVelocitySamples.Length;
  108. sampleCount++;
  109. // Estimate linear velocity
  110. velocitySamples[v] = velocityFactor * ( transform.position - previousPosition );
  111. // Estimate angular velocity
  112. Quaternion deltaRotation = transform.rotation * Quaternion.Inverse( previousRotation );
  113. float theta = 2.0f * Mathf.Acos( Mathf.Clamp( deltaRotation.w, -1.0f, 1.0f ) );
  114. if ( theta > Mathf.PI )
  115. {
  116. theta -= 2.0f * Mathf.PI;
  117. }
  118. Vector3 angularVelocity = new Vector3( deltaRotation.x, deltaRotation.y, deltaRotation.z );
  119. if ( angularVelocity.sqrMagnitude > 0.0f )
  120. {
  121. angularVelocity = theta * velocityFactor * angularVelocity.normalized;
  122. }
  123. angularVelocitySamples[w] = angularVelocity;
  124. previousPosition = transform.position;
  125. previousRotation = transform.rotation;
  126. }
  127. }
  128. }
  129. }