Add OpenTrack 3DoF support

oneup03 · Feb 22, 2025 · adb7c29 · adb7c29
1 parent 2e25625
commit adb7c29
Show file tree

Hide file tree

Showing 5 changed files with 99 additions and 1 deletion.
diff --git a/README.md b/README.md
@@ -67,6 +67,7 @@ Checkout the [Compatibility List](https://github.com/oneup03/VRto3D/wiki/Compati
 | `display_frequency` | `float` | The display refresh rate, in Hz.                                                            | `60.0`         |
 | `pitch_enable` +    | `bool`  | Enables or disables Controller right stick y-axis mapped to HMD Pitch                       | `false`        |
 | `yaw_enable` +      | `bool`  | Enables or disables Controller right stick x-axis mapped to HMD Yaw                         | `false`        |
+| `use_open_track`    | `bool`  | Enables or disables OpenTrack 3DoF HMD Control                                              | `false`        |
 | `pose_reset_key` +  | `string`| The Virtual-Key Code to reset the HMD position and orientation                              | `"VK_NUMPAD7"` |
 | `ctrl_toggle_key` + | `string`| The Virtual-Key Code to toggle Pitch and Yaw emulation on/off when they are enabled         | `"XINPUT_GAMEPAD_RIGHT_THUMB"` |
 | `ctrl_toggle_type` +| `string`| The ctrl_toggle_key's behavior ("toggle" "hold")                                            | `"toggle"`     |
@@ -205,6 +206,7 @@ Checkout the [Compatibility List](https://github.com/oneup03/VRto3D/wiki/Compati
     - The `pose_reset_key` can be set to allow resetting the view to the original position and orientation
     - Both of these keys can be set to XInput buttons & combinations or single keyboard/mouse keys as outlined in User Settings - Load Keys
     - The `pitch_radius` can be set to make the pitch emulation move along a semicircle instead of just tilting up/down in place
+- OpenTrack 3DoF support is available over UDP loopback at the default 4242 port when `use_open_track` is true. It can be used in combination with Pitch/Yaw emulation
 
 #### User Presets
 - If you swap between different convergence settings in-game, sometimes you will end up with black bars on the sides of the screen or you may not see a change immediately. If you reload/restart/reinitialize the VR mod, you should see the change

diff --git a/vrto3d/src/hmd_device_driver.cpp b/vrto3d/src/hmd_device_driver.cpp
@@ -14,6 +14,7 @@
  * You should have received a copy of the GNU Lesser General Public License
  * along with VRto3D. If not, see <http://www.gnu.org/licenses/>.
  */
+#define WIN32_LEAN_AND_MEAN
 #include "hmd_device_driver.h"
 #include "key_mappings.h"
 #include "driverlog.h"
@@ -23,6 +24,9 @@
 #include <sstream>
 #include <ctime>
 
+#include <winsock2.h>
+#include <ws2tcpip.h>
+#pragma comment (lib, "WSock32.Lib")
 #include <windows.h>
 #include <xinput.h>
 #include <nlohmann/json.hpp>
@@ -268,6 +272,10 @@ vr::EVRInitError MockControllerDeviceDriver::Activate( uint32_t unObjectId )
     hotkey_thread_ = std::thread(&MockControllerDeviceDriver::PollHotkeysThread, this);
     focus_thread_ = std::thread(&MockControllerDeviceDriver::FocusUpdateThread, this);
     depth_thread_ = std::thread(&MockControllerDeviceDriver::AutoDepthThread, this);
+    if (stereo_display_component_->GetConfig().use_open_track) {
+        open_track_att_ = HmdQuaternion_Identity;
+        track_thread_ = std::thread(&MockControllerDeviceDriver::OpenTrackThread, this);
+    }
 
     HANDLE thread_handle = pose_thread_.native_handle();
 
@@ -306,6 +314,73 @@ void MockControllerDeviceDriver::DebugRequest( const char *pchRequest, char *pch
 }
 
 
+//-----------------------------------------------------------------------------
+// Purpose: Receive OpenTrack 3DoF updates
+//-----------------------------------------------------------------------------
+void MockControllerDeviceDriver::OpenTrackThread()
+{
+    SOCKET socket_s;
+    struct sockaddr_in from = {};
+    int from_len = sizeof(from);
+    struct TOpenTrack {
+        double X;
+        double Y;
+        double Z;
+        double Yaw;
+        double Pitch;
+        double Roll;
+    };
+    TOpenTrack open_track;
+
+    WSADATA wsaData;
+    int iResult = WSAStartup(MAKEWORD(2, 2), &wsaData);
+    if (iResult != 0) {
+        DriverLog("WSAStartup failed: %d", iResult);
+    }
+    else {
+        struct sockaddr_in local = {};
+        local.sin_family = AF_INET;
+        local.sin_port = htons(4242);
+        local.sin_addr.s_addr = INADDR_ANY;
+
+        socket_s = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
+        if (socket_s == INVALID_SOCKET) {
+            DriverLog("Socket creation failed: %d", WSAGetLastError());
+            WSACleanup();  // Cleanup only if socket creation fails
+        }
+        else {
+            // Set non-blocking mode
+            u_long nonblocking_enabled = 1;
+            if (ioctlsocket(socket_s, FIONBIO, &nonblocking_enabled) == SOCKET_ERROR) {
+                DriverLog("Failed to set non-blocking mode: %d", WSAGetLastError());
+                closesocket(socket_s);
+                WSACleanup();
+            }
+            else if (bind(socket_s, (struct sockaddr*)&local, sizeof(local)) == SOCKET_ERROR) {
+                DriverLog("Bind failed: %d", WSAGetLastError());
+                closesocket(socket_s);
+                WSACleanup();
+            }
+        }
+    }
+
+    while (is_active_) {
+        //Read UDP socket with OpenTrack data
+        memset(&open_track, 0, sizeof(open_track));
+        auto bytes_read = recvfrom(socket_s, (char*)(&open_track), sizeof(open_track), 0, (sockaddr*)&from, &from_len);
+
+        if (bytes_read > 0) {
+            std::unique_lock<std::shared_mutex> lock(trk_mutex_);
+            open_track_att_ = HmdQuaternion_FromEulerAngles(DEG_TO_RAD(open_track.Roll), DEG_TO_RAD(open_track.Pitch), DEG_TO_RAD(open_track.Yaw));
+            lock.unlock();
+        }
+        else Sleep(1);
+    }
+    closesocket(socket_s);
+    WSACleanup();
+}
+
+
 //-----------------------------------------------------------------------------
 // Purpose: Static Pose with pitch & yaw adjustment
 //-----------------------------------------------------------------------------
@@ -402,7 +477,17 @@ void MockControllerDeviceDriver::PoseUpdateThread()
 
         // Recompose the rotation quaternion from pitch and yaw
         vr::HmdQuaternion_t pitchQuaternion = QuaternionFromAxisAngle(1.0f, 0.0f, 0.0f, pitchRadians);
-        pose.qRotation = HmdQuaternion_Normalize(currentYawQuat * pitchQuaternion);
+
+        if (config.use_open_track)
+        {
+            std::unique_lock<std::shared_mutex> lock(trk_mutex_);
+            pose.qRotation = HmdQuaternion_Normalize(currentYawQuat * pitchQuaternion * open_track_att_);
+            lock.unlock();
+        }
+        else
+        {
+            pose.qRotation = HmdQuaternion_Normalize(currentYawQuat * pitchQuaternion);
+        }
 
         // Calculate the new position relative to the current pitch & yaw
         pose.vecPosition[0] = config.pitch_radius * cos(pitchRadians) * sin(yawRadians) - config.pitch_radius * sin(yawRadians);
@@ -732,6 +817,9 @@ void MockControllerDeviceDriver::Deactivate()
         hotkey_thread_.join();
         focus_thread_.join();
         depth_thread_.join();
+        if (track_thread_.joinable()) {
+            track_thread_.join();
+        }
     }
 
     // unassign our controller index (we don't want to be calling vrserver anymore after Deactivate() has been called

diff --git a/vrto3d/src/hmd_device_driver.h b/vrto3d/src/hmd_device_driver.h
@@ -77,6 +77,7 @@ class MockControllerDeviceDriver : public vr::ITrackedDeviceServerDriver
     vr::DriverPose_t GetPose() override;
     void Deactivate() override;
 
+    void OpenTrackThread();
     void PoseUpdateThread();
     void PollHotkeysThread();
     void FocusUpdateThread();
@@ -104,4 +105,8 @@ class MockControllerDeviceDriver : public vr::ITrackedDeviceServerDriver
     std::thread hotkey_thread_;
     std::thread focus_thread_;
     std::thread depth_thread_;
+    std::thread track_thread_;
+
+    vr::HmdQuaternion_t open_track_att_;
+    std::shared_mutex  trk_mutex_;
 };
diff --git a/vrto3d/src/json_manager.cpp b/vrto3d/src/json_manager.cpp
@@ -182,6 +182,7 @@ void JsonManager::LoadParamsFromJson(StereoDisplayDriverConfiguration& config)
         config.reverse_enable = getValue<bool>(jsonConfig, "reverse_enable");
         config.depth_gauge = getValue<bool>(jsonConfig, "depth_gauge");
         config.dash_enable = getValue<bool>(jsonConfig, "dash_enable");
+        config.use_open_track = getValue<bool>(jsonConfig, "use_open_track");
 
         config.display_latency = getValue<float>(jsonConfig, "display_latency");
         config.display_frequency = getValue<float>(jsonConfig, "display_frequency");

diff --git a/vrto3d/src/json_manager.h b/vrto3d/src/json_manager.h
@@ -57,6 +57,7 @@ struct StereoDisplayDriverConfiguration
     bool yaw_enable;
     bool pitch_set;
     bool yaw_set;
+    bool use_open_track;
     int32_t pose_reset_key;
     std::string pose_reset_str;
     bool reset_xinput;
@@ -120,6 +121,7 @@ class JsonManager {
         {"display_frequency", 60.0},
         {"pitch_enable", false},
         {"yaw_enable", false},
+        {"use_open_track", false},
         {"pose_reset_key", "VK_NUMPAD7"},
         {"ctrl_toggle_key", "XINPUT_GAMEPAD_RIGHT_THUMB"},
         {"ctrl_toggle_type", "toggle"},