PageRenderTime 48ms CodeModel.GetById 16ms RepoModel.GetById 1ms app.codeStats 0ms

/Code/InteractivePaint-AR/Assets/HoloToolkit/Input/Scripts/GazeStabilizer.cs

https://gitlab.com/openhid/InteractivePaint
C# | 223 lines | 150 code | 44 blank | 29 comment | 10 complexity | fb31ccb2a06bbff4814893eb647d3968 MD5 | raw file
  1. // Copyright (c) Microsoft Corporation. All rights reserved.
  2. // Licensed under the MIT License. See LICENSE in the project root for license information.
  3. using System.Collections.Generic;
  4. using System.Linq;
  5. using UnityEngine;
  6. namespace HoloToolkit.Unity
  7. {
  8. /// <summary>
  9. /// GazeStabilizer iterates over samples of Raycast data and
  10. /// helps stabilize the user's gaze for precision targeting.
  11. /// </summary>
  12. public class GazeStabilizer : MonoBehaviour
  13. {
  14. [Tooltip("Number of samples that you want to iterate on.")]
  15. [Range(1, 120)]
  16. public int StoredStabilitySamples = 60;
  17. [Tooltip("Position based distance away from gravity well.")]
  18. public float PositionDropOffRadius = 0.02f;
  19. [Tooltip("Direction based distance away from gravity well.")]
  20. public float DirectionDropOffRadius = 0.1f;
  21. [Tooltip("Position lerp interpolation factor.")]
  22. [Range(0.25f, 0.85f)]
  23. public float PositionStrength = 0.66f;
  24. [Tooltip("Direction lerp interpolation factor.")]
  25. [Range(0.25f, 0.85f)]
  26. public float DirectionStrength = 0.83f;
  27. [Tooltip("Stability average weight multiplier factor.")]
  28. public float StabilityAverageDistanceWeight = 2.0f;
  29. [Tooltip("Stability variance weight multiplier factor.")]
  30. public float StabilityVarianceWeight = 1.0f;
  31. // Access the below public properties from the client class to consume stable values.
  32. public Vector3 StableHeadPosition { get; private set; }
  33. public Quaternion StableHeadRotation { get; private set; }
  34. public Ray StableHeadRay { get; private set; }
  35. public struct GazeSample
  36. {
  37. public Vector3 position;
  38. public Vector3 direction;
  39. public float timestamp;
  40. };
  41. private LinkedList<GazeSample> stabilitySamples = new LinkedList<GazeSample>();
  42. private Vector3 gazePosition;
  43. private Vector3 gazeDirection;
  44. // Most recent calculated instability values.
  45. private float gazePositionInstability;
  46. private float gazeDirectionInstability;
  47. private bool gravityPointExists = false;
  48. private Vector3 gravityWellPosition;
  49. private Vector3 gravityWellDirection;
  50. // Transforms instability value into a modified drop off distance, modify with caution.
  51. private const float positionDestabilizationFactor = 0.02f;
  52. private const float directionDestabilizationFactor = 0.3f;
  53. /// <summary>
  54. /// Updates the StableHeadPosition and StableHeadRotation based on GazeSample values.
  55. /// Call this method with Raycasthit parameters to get stable values.
  56. /// </summary>
  57. /// <param name="position">Position value from a RaycastHit point.</param>
  58. /// <param name="rotation">Rotation value from a RaycastHit rotation.</param>
  59. public void UpdateHeadStability(Vector3 position, Quaternion rotation)
  60. {
  61. gazePosition = position;
  62. gazeDirection = rotation * Vector3.forward;
  63. AddGazeSample(gazePosition, gazeDirection);
  64. UpdateInstability(out gazePositionInstability, out gazeDirectionInstability);
  65. // If we don't have a gravity point, just use the gaze position.
  66. if (!gravityPointExists)
  67. {
  68. gravityWellPosition = gazePosition;
  69. gravityWellDirection = gazeDirection;
  70. gravityPointExists = true;
  71. }
  72. UpdateGravityWellPositionDirection();
  73. }
  74. private void AddGazeSample(Vector3 positionSample, Vector3 directionSample)
  75. {
  76. // Record and save sample data.
  77. GazeSample newStabilitySample;
  78. newStabilitySample.position = positionSample;
  79. newStabilitySample.direction = directionSample;
  80. newStabilitySample.timestamp = Time.time;
  81. if (stabilitySamples != null)
  82. {
  83. // Remove from front items if we exceed stored samples.
  84. while (stabilitySamples.Count >= StoredStabilitySamples)
  85. {
  86. stabilitySamples.RemoveFirst();
  87. }
  88. stabilitySamples.AddLast(newStabilitySample);
  89. }
  90. }
  91. private void UpdateInstability(out float positionInstability, out float directionInstability)
  92. {
  93. GazeSample mostRecentSample;
  94. float positionDeltaMin = 0.0f;
  95. float positionDeltaMax = 0.0f;
  96. float positionDeltaMean = 0.0f;
  97. float directionDeltaMin = 0.0f;
  98. float directionDeltaMax = 0.0f;
  99. float directionDeltaMean = 0.0f;
  100. float positionDelta = 0.0f;
  101. float directionDelta = 0.0f;
  102. positionInstability = 0.0f;
  103. directionInstability = 0.0f;
  104. // If we have zero or one sample, there is no instability to report.
  105. if (stabilitySamples.Count < 2)
  106. {
  107. return;
  108. }
  109. mostRecentSample = stabilitySamples.Last.Value;
  110. bool first = true;
  111. foreach(GazeSample sample in stabilitySamples)
  112. {
  113. // Calculate difference between current sample and most recent sample.
  114. positionDelta = Vector3.Magnitude(sample.position - mostRecentSample.position);
  115. directionDelta = Vector3.Angle(sample.direction, mostRecentSample.direction) * Mathf.Deg2Rad;
  116. // Initialize max and min on first sample.
  117. if (first)
  118. {
  119. positionDeltaMin = positionDelta;
  120. positionDeltaMax = positionDelta;
  121. directionDeltaMin = directionDelta;
  122. directionDeltaMax = directionDelta;
  123. first = false;
  124. }
  125. else
  126. {
  127. // Update maximum, minimum and mean differences from most recent sample.
  128. positionDeltaMin = Mathf.Min(positionDelta, positionDeltaMin);
  129. positionDeltaMax = Mathf.Max(positionDelta, positionDeltaMax);
  130. directionDeltaMin = Mathf.Min(directionDelta, directionDeltaMin);
  131. directionDeltaMax = Mathf.Max(directionDelta, directionDeltaMax);
  132. }
  133. positionDeltaMean += positionDelta;
  134. directionDeltaMean += directionDelta;
  135. }
  136. positionDeltaMean = positionDeltaMean / (stabilitySamples.Count - 1);
  137. directionDeltaMean = directionDeltaMean / (stabilitySamples.Count - 1);
  138. // Calculate stability value for Gaze position and direction. Note that stability values will be significantly different for position and
  139. // direction since the position value is based on values in meters while the direction stability is based on data in radians.
  140. positionInstability = StabilityVarianceWeight * (positionDeltaMax - positionDeltaMin) + StabilityAverageDistanceWeight * positionDeltaMean;
  141. directionInstability = StabilityVarianceWeight * (directionDeltaMax - directionDeltaMin) + StabilityAverageDistanceWeight * directionDeltaMean;
  142. }
  143. private void UpdateGravityWellPositionDirection()
  144. {
  145. float stabilityModifiedPositionDropOffDistance;
  146. float stabilityModifiedDirectionDropOffDistance;
  147. float normalizedGazeToGravityWellPosition;
  148. float normalizedGazeToGravityWellDirection;
  149. // Modify effective size of well based on gaze stability.
  150. stabilityModifiedPositionDropOffDistance = Mathf.Max(0.0f, PositionDropOffRadius - (gazePositionInstability * positionDestabilizationFactor));
  151. stabilityModifiedDirectionDropOffDistance = Mathf.Max(0.0f, DirectionDropOffRadius - (gazeDirectionInstability * directionDestabilizationFactor));
  152. // Determine how far away from the well the gaze is, if that distance is zero push the normalized value above 1.0 to
  153. // force a gravity well position update.
  154. normalizedGazeToGravityWellPosition = 2.0f;
  155. if (stabilityModifiedPositionDropOffDistance > 0.0f)
  156. {
  157. normalizedGazeToGravityWellPosition = Vector3.Magnitude(gravityWellPosition - gazePosition) / stabilityModifiedPositionDropOffDistance;
  158. }
  159. normalizedGazeToGravityWellDirection = 2.0f;
  160. if (stabilityModifiedDirectionDropOffDistance > 0.0f)
  161. {
  162. normalizedGazeToGravityWellDirection = Mathf.Acos(Vector3.Dot(gravityWellDirection, gazeDirection)) / stabilityModifiedDirectionDropOffDistance;
  163. }
  164. // Move gravity well with Gaze if necessary.
  165. if (normalizedGazeToGravityWellPosition > 1.0f)
  166. {
  167. gravityWellPosition = gazePosition - Vector3.Normalize(gazePosition - gravityWellPosition) * stabilityModifiedPositionDropOffDistance;
  168. }
  169. if (normalizedGazeToGravityWellDirection > 1.0f)
  170. {
  171. gravityWellDirection = Vector3.Normalize(gazeDirection - Vector3.Normalize(gazeDirection - gravityWellDirection) * stabilityModifiedDirectionDropOffDistance);
  172. }
  173. // Adjust direction and position towards gravity well based on configurable strengths.
  174. StableHeadPosition = Vector3.Lerp(gazePosition, gravityWellPosition, PositionStrength);
  175. StableHeadRotation = Quaternion.LookRotation(Vector3.Lerp(gazeDirection, gravityWellDirection, DirectionStrength));
  176. StableHeadRay = new Ray(StableHeadPosition, StableHeadRotation * Vector3.forward);
  177. }
  178. }
  179. }