VelocityTracker.cs 4.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215
  1. using System;
  2. using UnityEngine;
  3. public class VelocityTracker : MonoBehaviour
  4. {
  5. public Vector3 FrameAngularVelocity
  6. {
  7. get
  8. {
  9. return this.m_frameAngularVelocity;
  10. }
  11. }
  12. public Vector3 FrameLinearVelocity
  13. {
  14. get
  15. {
  16. return this.m_frameLinearVelocity;
  17. }
  18. }
  19. public Vector3 TrackedAngularVelocity
  20. {
  21. get
  22. {
  23. return this.m_trackedAngularVelocity;
  24. }
  25. }
  26. public Vector3 TrackedLinearVelocity
  27. {
  28. get
  29. {
  30. return this.m_trackedLinearVelocity;
  31. }
  32. }
  33. private void Awake()
  34. {
  35. this.m_position = base.transform.position;
  36. this.m_rotation = base.transform.rotation;
  37. }
  38. private void FixedUpdate()
  39. {
  40. Vector3 position = base.transform.position;
  41. Vector3 deltaPosition = position - this.m_position;
  42. this.m_position = position;
  43. Quaternion rotation = base.transform.rotation;
  44. Vector3 deltaRotation = this.DeltaRotation(rotation, this.m_rotation) * 0.0174532924f;
  45. this.m_rotation = rotation;
  46. this.AddSample(deltaPosition, deltaRotation);
  47. this.m_frameLinearVelocity = this.m_samples[this.m_index].LinearVelocity;
  48. this.m_frameAngularVelocity = this.m_samples[this.m_index].AngularVelocity;
  49. this.m_trackedLinearVelocity = this.ComputeAverageLinearVelocity().normalized * this.ComputeMaxLinearSpeed();
  50. this.m_trackedAngularVelocity = this.ComputeAverageAngularVelocity();
  51. }
  52. private void OnDrawGizmos()
  53. {
  54. if (!this.m_showGizmos)
  55. {
  56. return;
  57. }
  58. Gizmos.color = Color.red;
  59. Gizmos.DrawRay(base.transform.position, this.TrackedLinearVelocity);
  60. }
  61. private Vector3 DeltaRotation(Quaternion final, Quaternion initial)
  62. {
  63. Vector3 eulerAngles = final.eulerAngles;
  64. Vector3 eulerAngles2 = initial.eulerAngles;
  65. Vector3 result = new Vector3(Mathf.DeltaAngle(eulerAngles2.x, eulerAngles.x), Mathf.DeltaAngle(eulerAngles2.y, eulerAngles.y), Mathf.DeltaAngle(eulerAngles2.z, eulerAngles.z));
  66. return result;
  67. }
  68. private void AddSample(Vector3 deltaPosition, Vector3 deltaRotation)
  69. {
  70. this.m_index = (this.m_index + 1) % this.m_samples.Length;
  71. this.m_count = Mathf.Min(this.m_count + 1, this.m_samples.Length);
  72. float time = Time.time;
  73. Vector3 linearVelocity = deltaPosition / Time.deltaTime;
  74. Vector3 angularVelocity = deltaRotation / Time.deltaTime;
  75. this.m_samples[this.m_index] = new VelocityTracker.Sample
  76. {
  77. Time = time,
  78. LinearVelocity = linearVelocity,
  79. AngularVelocity = angularVelocity
  80. };
  81. this.m_samples[this.m_index].SquaredLinearSpeed = this.ComputeAverageLinearVelocity().sqrMagnitude;
  82. }
  83. private int Count()
  84. {
  85. return Mathf.Min(this.m_count, this.m_samples.Length);
  86. }
  87. private int IndexPrev(int index)
  88. {
  89. return (index != 0) ? (index - 1) : (this.m_count - 1);
  90. }
  91. private bool IsSampleValid(int index, float windowSize)
  92. {
  93. float num = Time.time - this.m_samples[index].Time;
  94. return windowSize - num >= 0.0001f || index == this.m_index;
  95. }
  96. private Vector3 ComputeAverageAngularVelocity()
  97. {
  98. int num = this.m_index;
  99. int num2 = this.Count();
  100. int num3 = 0;
  101. Vector3 vector = Vector3.zero;
  102. for (int i = 0; i < num2; i++)
  103. {
  104. if (!this.IsSampleValid(num, 0.0222222228f))
  105. {
  106. break;
  107. }
  108. num3++;
  109. vector += this.m_samples[num].AngularVelocity;
  110. num = this.IndexPrev(num);
  111. }
  112. if (num3 > 1)
  113. {
  114. vector /= (float)num3;
  115. }
  116. return vector;
  117. }
  118. private Vector3 ComputeAverageLinearVelocity()
  119. {
  120. int num = this.m_index;
  121. int num2 = this.Count();
  122. int num3 = 0;
  123. Vector3 vector = Vector3.zero;
  124. for (int i = 0; i < num2; i++)
  125. {
  126. if (!this.IsSampleValid(num, 0.0444444455f))
  127. {
  128. break;
  129. }
  130. num3++;
  131. vector += this.m_samples[num].LinearVelocity;
  132. num = this.IndexPrev(num);
  133. }
  134. if (num3 > 1)
  135. {
  136. vector /= (float)num3;
  137. }
  138. return vector;
  139. }
  140. private float ComputeMaxLinearSpeed()
  141. {
  142. int num = this.m_index;
  143. int num2 = this.Count();
  144. float num3 = 0f;
  145. for (int i = 0; i < num2; i++)
  146. {
  147. if (!this.IsSampleValid(num, 0.08888889f))
  148. {
  149. break;
  150. }
  151. num3 = Mathf.Max(num3, this.m_samples[num].SquaredLinearSpeed);
  152. num = this.IndexPrev(num);
  153. }
  154. return (num3 <= Mathf.Epsilon) ? 0f : Mathf.Sqrt(num3);
  155. }
  156. public const float WINDOW_TIME = 0.0111111114f;
  157. public const float WINDOW_EPSILON = 0.0001f;
  158. public const float LINEAR_SPEED_WINDOW = 0.08888889f;
  159. public const float LINEAR_VELOCITY_WINDOW = 0.0444444455f;
  160. public const float ANGULAR_VELOCITY_WINDOW = 0.0222222228f;
  161. public const int MAX_SAMPLES = 45;
  162. [SerializeField]
  163. public bool m_showGizmos = true;
  164. private int m_index = -1;
  165. private int m_count;
  166. private VelocityTracker.Sample[] m_samples = new VelocityTracker.Sample[45];
  167. private Vector3 m_position = Vector3.zero;
  168. private Quaternion m_rotation = Quaternion.identity;
  169. private Vector3 m_frameLinearVelocity = Vector3.zero;
  170. private Vector3 m_frameAngularVelocity = Vector3.zero;
  171. private Vector3 m_trackedLinearVelocity = Vector3.zero;
  172. private Vector3 m_trackedAngularVelocity = Vector3.zero;
  173. private struct Sample
  174. {
  175. public float Time;
  176. public float SquaredLinearSpeed;
  177. public Vector3 LinearVelocity;
  178. public Vector3 AngularVelocity;
  179. }
  180. }