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(); public float BatteryLevel { get; set; } public float SteerRotation => framesToConsider > 1 ? CalculateSteerRotationMultiFrame() : CalculateSteerRotationSingleFrame(); public Vector3 DebugPosOnBikePlane { private set; get; } private Queue previousValues; private void OnEnable() { if (framesToConsider > 1) { previousValues = new Queue(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; } } }