123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384 |
- using System;
- using System.Collections.Generic;
- using System.Linq;
- using UnityEditor;
- using UnityEngine;
- using Valve.VR;
- namespace Tracking
- {
- public class FrontWheelTracker : CalibratableTracker, IViveTracker
- {
- protected override string KeyPrefix => "fw";
- public float trackerDistanceToWheelCenterPoint;
- public KineticLegTracker legTracker;
- public int framesToConsider = 10;
- public bool useTrackerRotationInsteadOfPosition;
- public string id;
- public string Id => id;
- public SteamVR_TrackedObject TrackedObject => GetComponent<SteamVR_TrackedObject>();
- public float BatteryLevel { get; set; }
- public float SteerRotation => framesToConsider > 1
- ? CalculateSteerRotationMultiFrame()
- : CalculateSteerRotationSingleFrame();
- public Vector3 DebugPosOnBikePlane { private set; get; }
- private Queue<float> previousValues;
- private void OnEnable()
- {
- if (framesToConsider > 1)
- {
- previousValues = new Queue<float>(framesToConsider);
- }
- }
- private float CalculateSteerRotationSingleFrame()
- {
- if (useTrackerRotationInsteadOfPosition) return RelativeRotation.y;
- var posOnBikePlane = GetPosOnBikePlane();
- return TrackerPositionToAngle(posOnBikePlane);
- }
- private float CalculateSteerRotationMultiFrame()
- {
- var angleThisFrame = CalculateSteerRotationSingleFrame();
- if (previousValues.Count > framesToConsider)
- {
- previousValues.Dequeue();
- }
- previousValues.Enqueue(angleThisFrame);
- //easiest way -> avg; TODO maybe in future, remove outliers
- return previousValues.Average();
- }
- private Vector3 GetPosOnBikePlane()
- {
- //legTracker position is added to compensate movements of the whole installation
- Vector3 legTrackerPos = Vector3.zero;
- if (legTracker.isActiveAndEnabled)
- {
- legTrackerPos = legTracker.RelativePosition;
- }
- var posOnBikePlane = Vector3.ProjectOnPlane(RelativePosition + legTrackerPos, bicycleTransform.up);
- DebugPosOnBikePlane = posOnBikePlane;
- /*if (Mathf.Abs(posOnBikePlane.x / trackerDistanceToWheelCenterPoint) >= 1f)
- {
- Debug.LogError("Strange behaviour!");
- //EditorApplication.isPaused = true;
- }*/
- return posOnBikePlane;
- }
- private float TrackerPositionToAngle(Vector3 posOnBikePlane)
- {
- var theta = Mathf.Acos(Mathf.Clamp(posOnBikePlane.x / trackerDistanceToWheelCenterPoint, -1f, 1f));
- return 90f - theta * Mathf.Rad2Deg;
- }
- }
- }
|