1
0

SteamVR_RingBuffer.cs 5.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224
  1. //======= Copyright (c) Valve Corporation, All rights reserved. ===============
  2. using UnityEngine;
  3. using System.Collections;
  4. namespace Valve.VR
  5. {
  6. public class SteamVR_RingBuffer<T>
  7. {
  8. protected T[] buffer;
  9. protected int currentIndex;
  10. protected T lastElement;
  11. public SteamVR_RingBuffer(int size)
  12. {
  13. buffer = new T[size];
  14. currentIndex = 0;
  15. }
  16. public void Add(T newElement)
  17. {
  18. buffer[currentIndex] = newElement;
  19. StepForward();
  20. }
  21. public virtual void StepForward()
  22. {
  23. lastElement = buffer[currentIndex];
  24. currentIndex++;
  25. if (currentIndex >= buffer.Length)
  26. currentIndex = 0;
  27. cleared = false;
  28. }
  29. public virtual T GetAtIndex(int atIndex)
  30. {
  31. if (atIndex < 0)
  32. atIndex += buffer.Length;
  33. return buffer[atIndex];
  34. }
  35. public virtual T GetLast()
  36. {
  37. return lastElement;
  38. }
  39. public virtual int GetLastIndex()
  40. {
  41. int lastIndex = currentIndex - 1;
  42. if (lastIndex < 0)
  43. lastIndex += buffer.Length;
  44. return lastIndex;
  45. }
  46. private bool cleared = false;
  47. public void Clear()
  48. {
  49. if (cleared == true)
  50. return;
  51. if (buffer == null)
  52. return;
  53. for (int index = 0; index < buffer.Length; index++)
  54. {
  55. buffer[index] = default(T);
  56. }
  57. lastElement = default(T);
  58. currentIndex = 0;
  59. cleared = true;
  60. }
  61. }
  62. public class SteamVR_HistoryBuffer : SteamVR_RingBuffer<SteamVR_HistoryStep>
  63. {
  64. public SteamVR_HistoryBuffer(int size) : base(size)
  65. {
  66. }
  67. public void Update(Vector3 position, Quaternion rotation, Vector3 velocity, Vector3 angularVelocity)
  68. {
  69. if (buffer[currentIndex] == null)
  70. buffer[currentIndex] = new SteamVR_HistoryStep();
  71. buffer[currentIndex].position = position;
  72. buffer[currentIndex].rotation = rotation;
  73. buffer[currentIndex].velocity = velocity;
  74. buffer[currentIndex].angularVelocity = angularVelocity;
  75. buffer[currentIndex].timeInTicks = System.DateTime.Now.Ticks;
  76. StepForward();
  77. }
  78. public float GetVelocityMagnitudeTrend(int toIndex = -1, int fromIndex = -1)
  79. {
  80. if (toIndex == -1)
  81. toIndex = currentIndex - 1;
  82. if (toIndex < 0)
  83. toIndex += buffer.Length;
  84. if (fromIndex == -1)
  85. fromIndex = toIndex - 1;
  86. if (fromIndex < 0)
  87. fromIndex += buffer.Length;
  88. SteamVR_HistoryStep toStep = buffer[toIndex];
  89. SteamVR_HistoryStep fromStep = buffer[fromIndex];
  90. if (IsValid(toStep) && IsValid(fromStep))
  91. {
  92. return toStep.velocity.sqrMagnitude - fromStep.velocity.sqrMagnitude;
  93. }
  94. return 0;
  95. }
  96. public bool IsValid(SteamVR_HistoryStep step)
  97. {
  98. return step != null && step.timeInTicks != -1;
  99. }
  100. public int GetTopVelocity(int forFrames, int addFrames = 0)
  101. {
  102. int topFrame = currentIndex;
  103. float topVelocitySqr = 0;
  104. int currentFrame = currentIndex;
  105. while (forFrames > 0)
  106. {
  107. forFrames--;
  108. currentFrame--;
  109. if (currentFrame < 0)
  110. currentFrame = buffer.Length - 1;
  111. SteamVR_HistoryStep currentStep = buffer[currentFrame];
  112. if (IsValid(currentStep) == false)
  113. break;
  114. float currentSqr = buffer[currentFrame].velocity.sqrMagnitude;
  115. if (currentSqr > topVelocitySqr)
  116. {
  117. topFrame = currentFrame;
  118. topVelocitySqr = currentSqr;
  119. }
  120. }
  121. topFrame += addFrames;
  122. if (topFrame >= buffer.Length)
  123. topFrame -= buffer.Length;
  124. return topFrame;
  125. }
  126. public void GetAverageVelocities(out Vector3 velocity, out Vector3 angularVelocity, int forFrames, int startFrame = -1)
  127. {
  128. velocity = Vector3.zero;
  129. angularVelocity = Vector3.zero;
  130. if (startFrame == -1)
  131. startFrame = currentIndex - 1;
  132. if (startFrame < 0)
  133. startFrame = buffer.Length - 1;
  134. int endFrame = startFrame - forFrames;
  135. if (endFrame < 0)
  136. endFrame += buffer.Length;
  137. Vector3 totalVelocity = Vector3.zero;
  138. Vector3 totalAngularVelocity = Vector3.zero;
  139. float totalFrames = 0;
  140. int currentFrame = startFrame;
  141. while (forFrames > 0)
  142. {
  143. forFrames--;
  144. currentFrame--;
  145. if (currentFrame < 0)
  146. currentFrame = buffer.Length - 1;
  147. SteamVR_HistoryStep currentStep = buffer[currentFrame];
  148. if (IsValid(currentStep) == false)
  149. break;
  150. totalFrames++;
  151. totalVelocity += currentStep.velocity;
  152. totalAngularVelocity += currentStep.angularVelocity;
  153. }
  154. velocity = totalVelocity / totalFrames;
  155. angularVelocity = totalAngularVelocity / totalFrames;
  156. }
  157. }
  158. public class SteamVR_HistoryStep
  159. {
  160. public Vector3 position;
  161. public Quaternion rotation;
  162. public Vector3 velocity;
  163. public Vector3 angularVelocity;
  164. public long timeInTicks = -1;
  165. }
  166. }