FrontWheelTracker.cs 1.3 KB

123456789101112131415161718192021222324252627282930313233343536373839
  1. using System;
  2. using UnityEditor;
  3. using UnityEngine;
  4. namespace Tracking
  5. {
  6. public class FrontWheelTracker : CalibratableTracker
  7. {
  8. protected override string KeyPrefix => "fw";
  9. public float trackerDistanceToWheelCenterPoint;
  10. public KineticLegTracker legTracker;
  11. public Vector3 DebugPosOnBikePlane { private set; get; }
  12. public float SteerRotation
  13. {
  14. get
  15. {
  16. //legTracker position is added to compensate movements of the whole installation
  17. Vector3 legTrackerPos = Vector3.zero;
  18. if (legTracker.isActiveAndEnabled)
  19. {
  20. legTrackerPos = legTracker.RelativePosition;
  21. }
  22. var posOnBikePlane = Vector3.ProjectOnPlane(RelativePosition + legTrackerPos, bicycleTransform.up);
  23. DebugPosOnBikePlane = posOnBikePlane;
  24. if (Mathf.Abs(posOnBikePlane.x / trackerDistanceToWheelCenterPoint) >= 1f)
  25. {
  26. Debug.LogError("Strange behaviour!");
  27. //EditorApplication.isPaused = true;
  28. }
  29. var theta = Mathf.Acos(Mathf.Clamp(posOnBikePlane.x / trackerDistanceToWheelCenterPoint, -1f, 1f));
  30. return 90f - theta * Mathf.Rad2Deg;
  31. }
  32. }
  33. }
  34. }