FrontWheelTracker.cs 2.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384
  1. using System;
  2. using System.Collections.Generic;
  3. using System.Linq;
  4. using UnityEditor;
  5. using UnityEngine;
  6. using Valve.VR;
  7. namespace Tracking
  8. {
  9. public class FrontWheelTracker : CalibratableTracker, IViveTracker
  10. {
  11. protected override string KeyPrefix => "fw";
  12. public float trackerDistanceToWheelCenterPoint;
  13. public KineticLegTracker legTracker;
  14. public int framesToConsider = 10;
  15. public bool useTrackerRotationInsteadOfPosition;
  16. public string id;
  17. public string Id => id;
  18. public SteamVR_TrackedObject TrackedObject => GetComponent<SteamVR_TrackedObject>();
  19. public float BatteryLevel { get; set; }
  20. public float SteerRotation => framesToConsider > 1
  21. ? CalculateSteerRotationMultiFrame()
  22. : CalculateSteerRotationSingleFrame();
  23. public Vector3 DebugPosOnBikePlane { private set; get; }
  24. private Queue<float> previousValues;
  25. private void OnEnable()
  26. {
  27. if (framesToConsider > 1)
  28. {
  29. previousValues = new Queue<float>(framesToConsider);
  30. }
  31. }
  32. private float CalculateSteerRotationSingleFrame()
  33. {
  34. if (useTrackerRotationInsteadOfPosition) return RelativeRotation.y;
  35. var posOnBikePlane = GetPosOnBikePlane();
  36. return TrackerPositionToAngle(posOnBikePlane);
  37. }
  38. private float CalculateSteerRotationMultiFrame()
  39. {
  40. var angleThisFrame = CalculateSteerRotationSingleFrame();
  41. if (previousValues.Count > framesToConsider)
  42. {
  43. previousValues.Dequeue();
  44. }
  45. previousValues.Enqueue(angleThisFrame);
  46. //easiest way -> avg; TODO maybe in future, remove outliers
  47. return previousValues.Average();
  48. }
  49. private Vector3 GetPosOnBikePlane()
  50. {
  51. //legTracker position is added to compensate movements of the whole installation
  52. Vector3 legTrackerPos = Vector3.zero;
  53. if (legTracker.isActiveAndEnabled)
  54. {
  55. legTrackerPos = legTracker.RelativePosition;
  56. }
  57. var posOnBikePlane = Vector3.ProjectOnPlane(RelativePosition + legTrackerPos, bicycleTransform.up);
  58. DebugPosOnBikePlane = posOnBikePlane;
  59. /*if (Mathf.Abs(posOnBikePlane.x / trackerDistanceToWheelCenterPoint) >= 1f)
  60. {
  61. Debug.LogError("Strange behaviour!");
  62. //EditorApplication.isPaused = true;
  63. }*/
  64. return posOnBikePlane;
  65. }
  66. private float TrackerPositionToAngle(Vector3 posOnBikePlane)
  67. {
  68. var theta = Mathf.Acos(Mathf.Clamp(posOnBikePlane.x / trackerDistanceToWheelCenterPoint, -1f, 1f));
  69. return 90f - theta * Mathf.Rad2Deg;
  70. }
  71. }
  72. }