From 78dd3f28393cefa33a1aa696b2fe1ab992eb7c5c Mon Sep 17 00:00:00 2001
From: Trevor Jones <jones1876@gmail.com>
Date: Wed, 19 Mar 2025 23:46:49 -0400
Subject: [PATCH] Experimental 6-Dof Tracking

---
 Uuvr/ModConfiguration.cs | 22 ++++++++++++++++++++-
 Uuvr/UuvrCore.cs         | 12 ++++++++++--
 Uuvr/UuvrPoseDriver.cs   | 42 ++++++++++++++++++++++++++++++++++++++++
 3 files changed, 73 insertions(+), 3 deletions(-)

diff --git a/Uuvr/ModConfiguration.cs b/Uuvr/ModConfiguration.cs
index 1abc2bf..953ffda 100644
--- a/Uuvr/ModConfiguration.cs
+++ b/Uuvr/ModConfiguration.cs
@@ -31,8 +31,17 @@ public enum VrApi
         [Description("OpenXR")]
         OpenXr,
     }
+
 #endif
-    
+
+    public enum DegreesOfFreedom
+    {
+        [Description("3-DOF")]
+        Three,
+        [Description("6-DOF")]
+        Six,
+    }
+
     public enum ScreenSpaceCanvasType
     {
         [Description("None")]
@@ -61,8 +70,10 @@ public enum UiPatchMode
         CanvasRedirect,
     }
 
+    
     public readonly ConfigFile Config;
     public readonly ConfigEntry<CameraTrackingMode> CameraTracking;
+    public readonly ConfigEntry<DegreesOfFreedom> HeadsetDOF;
     public readonly ConfigEntry<bool> RelativeCameraSetStereoView;
     public readonly ConfigEntry<int> VrCameraDepth;
     public readonly ConfigEntry<int> VrUiLayerOverride;
@@ -103,7 +114,9 @@ public ModConfiguration(ConfigFile config)
             CameraTrackingMode.RelativeTransform,
 #endif
             "Defines how camera tracking is done. Relative is usually preferred, but not all games support it. Changing this might require restarting the level.");
+
         
+
         RelativeCameraSetStereoView = config.Bind(
             "Relative Camera",
             "Use SetStereoView for Relative Camera",
@@ -116,6 +129,13 @@ public ModConfiguration(ConfigFile config)
             false,
             "Prevents pitch and roll changes on the camera, allowing only yaw changes.");
 
+
+        HeadsetDOF = config.Bind(
+            "Camera",
+            "Headset Degrees of Freedom",
+            DegreesOfFreedom.Three,
+            "Some games are better with 3-DOF, some are better with 6-DOF. Changing this might require restarting the level.");
+
         CameraPositionOffsetX = config.Bind(
             "Camera",
             "Camera Position Offset X",
diff --git a/Uuvr/UuvrCore.cs b/Uuvr/UuvrCore.cs
index c6d9ad8..0257d07 100644
--- a/Uuvr/UuvrCore.cs
+++ b/Uuvr/UuvrCore.cs
@@ -15,6 +15,7 @@ public UuvrCore(IntPtr pointer) : base(pointer)
     }
 #endif
 
+    private readonly KeyboardKey _recenterKey = new (KeyboardKey.KeyCode.F2);
     private readonly KeyboardKey _toggleVrKey = new (KeyboardKey.KeyCode.F3);
     private float _originalFixedDeltaTime;
     
@@ -56,12 +57,19 @@ private void Start()
 
         _vrTogglerManager = new VrTogglerManager();
 
-        SetPositionTrackingEnabled(false);
+        SetPositionTrackingEnabled(true);
     }
 
     private void Update()
     {
-        if (_toggleVrKey.UpdateIsDown()) _vrTogglerManager?.ToggleVr();
+        if (_toggleVrKey.UpdateIsDown()) 
+            _vrTogglerManager?.ToggleVr();
+
+        if (_recenterKey.UpdateIsDown())
+        {
+            UuvrPoseDriver.RecenterView();
+        }
+
         UpdatePhysicsRate();
     }
 
diff --git a/Uuvr/UuvrPoseDriver.cs b/Uuvr/UuvrPoseDriver.cs
index 44d393f..b53040e 100644
--- a/Uuvr/UuvrPoseDriver.cs
+++ b/Uuvr/UuvrPoseDriver.cs
@@ -3,6 +3,8 @@
 using UnityEngine;
 using Uuvr.UnityTypesHelper;
 
+using static Uuvr.ModConfiguration;
+
 namespace Uuvr;
 
 public class UuvrPoseDriver: UuvrBehaviour
@@ -14,6 +16,9 @@ public UuvrPoseDriver(IntPtr pointer) : base(pointer)
 #endif
 
     private MethodInfo? _trackingRotationMethod;
+
+    private MethodInfo? _trackingPositionMethod;
+
     private readonly object[] _trackingRotationMethodArgs = {
         2 // Enum value for XRNode.CenterEye
     };
@@ -27,6 +32,7 @@ protected override void Awake()
                                 Type.GetType("UnityEngine.VR.InputTracking, UnityEngine");
 
         _trackingRotationMethod = inputTrackingType?.GetMethod("GetLocalRotation");
+        
 
         if (_trackingRotationMethod == null)
         {
@@ -35,7 +41,14 @@ protected override void Awake()
             return;
         }
 
+        _trackingPositionMethod = inputTrackingType?.GetMethod("GetLocalPosition");
+        if (_trackingPositionMethod == null)
+        {
+            Debug.LogWarning("Failed to find InputTracking.GetLocalPosition. Position tracking will be disabled.");
+        }
+
         DisableCameraAutoTracking();
+        RecenterView();
     }
 
     protected override void OnBeforeRender()
@@ -54,12 +67,41 @@ private void LateUpdate()
         UpdateTransform();
     }
 
+    /// <summary>
+    /// This vector is used to recenter the view position.  (its static so that all VRCamera's share the same offset.
+    /// </summary>
+    private static Vector3? _positionOffset;
+    
+
+    public static void RecenterView()
+    {
+        _positionOffset = null;
+    }
+
+    
     private void UpdateTransform()
     {
         if (_trackingRotationMethod != null)
         {
             transform.localRotation = (Quaternion)_trackingRotationMethod.Invoke(null, _trackingRotationMethodArgs);
         }
+
+        if (_trackingPositionMethod != null && ModConfiguration.Instance.HeadsetDOF.Value == DegreesOfFreedom.Six)
+        {            
+            var pos = (Vector3)_trackingPositionMethod.Invoke(null, _trackingRotationMethodArgs);
+            if (_positionOffset == null)
+            {
+                _positionOffset = -pos;  // First time we get the position, we store the reverse as the offset.
+            }
+            // add the offset to the position.
+            pos += _positionOffset.Value;
+            
+            transform.localPosition = pos;
+        }
+        else
+        {
+            transform.localPosition = Vector3.zero;
+        }
     }
 
     private void DisableCameraAutoTracking()