fix: linux views when using modified playspace (#2017)

* fix: linux pose history while using rotated playspace * fix crash; reload playspace on hmd connect * auto-recovery on pose desync * only detect desync on linux
alvr-org · Mar 10, 2024 · a4e6355 · a4e6355
1 parent 919988a
commit a4e6355
Show file tree

Hide file tree

Showing 8 changed files with 127 additions and 40 deletions.
diff --git a/alvr/server/cpp/alvr_server/ChaperoneUpdater.cpp b/alvr/server/cpp/alvr_server/ChaperoneUpdater.cpp
@@ -13,28 +13,45 @@ using namespace alvr_chaperone;
 
 std::mutex chaperone_mutex;
 
+bool isOpenvrInit = false;
+
 void InitOpenvrClient() {
 #ifndef __APPLE__
     std::unique_lock<std::mutex> lock(chaperone_mutex);
 
+    if (isOpenvrInit) {
+        return;
+    }
+
     vr::EVRInitError error;
-    vr::VR_Init(&error, vr::VRApplication_Utility);
+    // Background needed for VRCompositor()->GetTrackingSpace()
+    vr::VR_Init(&error, vr::VRApplication_Background);
 
     if (error != vr::VRInitError_None) {
         Warn("Failed to init OpenVR client! Error: %d", error);
         return;
     }
+    isOpenvrInit = true;
 #endif
 }
 
 void ShutdownOpenvrClient() {
 #ifndef __APPLE__
     std::unique_lock<std::mutex> lock(chaperone_mutex);
 
+    if (!isOpenvrInit) {
+        return;
+    }
+
+    isOpenvrInit = false;
     vr::VR_Shutdown();
 #endif
 }
 
+bool IsOpenvrClientReady() {
+    return isOpenvrInit;
+}
+
 void _SetChaperoneArea(float areaWidth, float areaHeight) {
 #ifndef __APPLE__
     std::unique_lock<std::mutex> lock(chaperone_mutex);
@@ -68,7 +85,15 @@ void _SetChaperoneArea(float areaWidth, float areaHeight) {
 }
 
 #ifdef __linux__
-vr::HmdMatrix34_t GetRawZeroPose() {
-    return vr::VRSystem()->GetRawZeroPoseToStandingAbsoluteTrackingPose();
+vr::HmdMatrix34_t GetInvZeroPose() {
+    vr::HmdMatrix34_t mat;
+    // revert pulls live into working copy
+    vr::VRChaperoneSetup()->RevertWorkingCopy();
+    if (vr::VRCompositor()->GetTrackingSpace() == vr::TrackingUniverseStanding) {
+        vr::VRChaperoneSetup()->GetWorkingStandingZeroPoseToRawTrackingPose(&mat);
+    } else {
+        vr::VRChaperoneSetup()->GetWorkingSeatedZeroPoseToRawTrackingPose(&mat);
+    }
+    return mat;
 }
-#endif
+#endif
diff --git a/alvr/server/cpp/alvr_server/PoseHistory.cpp b/alvr/server/cpp/alvr_server/PoseHistory.cpp
@@ -1,6 +1,7 @@
 #include "PoseHistory.h"
 #include "Utils.h"
 #include "Logger.h"
+#include "include/openvr_math.h"
 #include <mutex>
 #include <optional>
 
@@ -18,15 +19,7 @@ void PoseHistory::OnPoseUpdated(uint64_t targetTimestampNs, FfiDeviceMotion moti
 
 	std::unique_lock<std::mutex> lock(m_mutex);
 	if (!m_transformIdentity) {
-		vr::HmdMatrix34_t rotation = {};
-		for (int i = 0; i < 3; ++i) {
-			for (int j = 0; j < 3; ++j) {
-				rotation.m[j][i] = 0;
-				for (int k = 0; k < 3; ++k) {
-					rotation.m[j][i] += history.rotationMatrix.m[k][i] * m_transform.m[j][k];
-				}
-			}
-		}
+		vr::HmdMatrix34_t rotation = vrmath::matMul33(m_transform, history.rotationMatrix);
 		history.rotationMatrix = rotation;
 	}
 
@@ -48,9 +41,6 @@ void PoseHistory::OnPoseUpdated(uint64_t targetTimestampNs, FfiDeviceMotion moti
 std::optional<PoseHistory::TrackingHistoryFrame> PoseHistory::GetBestPoseMatch(const vr::HmdMatrix34_t &pose) const
 {
 	std::unique_lock<std::mutex> lock(m_mutex);
-	if (m_transformUpdating && !m_poseBuffer.empty()) {
-		return m_poseBuffer.back();
-	}
 	float minDiff = 100000;
 	auto minIt = m_poseBuffer.begin();
 	for (auto it = m_poseBuffer.begin(); it != m_poseBuffer.end(); ++it) {
@@ -85,18 +75,10 @@ std::optional<PoseHistory::TrackingHistoryFrame> PoseHistory::GetPoseAt(uint64_t
 	return {};
 }
 
-void PoseHistory::SetTransformUpdating()
-{
-	std::unique_lock<std::mutex> lock(m_mutex);
-	m_transformUpdating = true;
-}
-
 void PoseHistory::SetTransform(const vr::HmdMatrix34_t &transform)
 {
 	std::unique_lock<std::mutex> lock(m_mutex);
 	m_transform = transform;
-	m_transformUpdating = false;
-	m_poseBuffer.clear();
 
 	for (int i = 0; i < 3; ++i) {
 		for (int j = 0; j < 3; ++j) {

diff --git a/alvr/server/cpp/alvr_server/PoseHistory.h b/alvr/server/cpp/alvr_server/PoseHistory.h
@@ -21,13 +21,11 @@ class PoseHistory
 	// Return the most recent pose known at the given timestamp
 	std::optional<TrackingHistoryFrame> GetPoseAt(uint64_t timestampNs) const;
 
-	void SetTransformUpdating();
 	void SetTransform(const vr::HmdMatrix34_t &transform);
 
 private:
 	mutable std::mutex m_mutex;
 	std::list<TrackingHistoryFrame> m_poseBuffer;
 	vr::HmdMatrix34_t m_transform = {{{1.0, 0.0, 0.0, 0.0}, {0.0, 1.0, 0.0, 0.0}, {0.0, 0.0, 1.0, 0.0}}};
 	bool m_transformIdentity = true;
-	bool m_transformUpdating = false;
 };
diff --git a/alvr/server/cpp/alvr_server/alvr_server.cpp b/alvr/server/cpp/alvr_server/alvr_server.cpp
@@ -23,10 +23,19 @@
 #include <map>
 #include <optional>
 
-void _SetChaperoneArea(float areaWidth, float areaHeight);
 #ifdef __linux__
-vr::HmdMatrix34_t GetRawZeroPose();
+#include "include/openvr_math.h"
+vr::HmdMatrix34_t GetInvZeroPose();
+
+vr::HmdMatrix34_t GetRawZeroPose() {
+    return vrmath::matInv33(GetInvZeroPose());
+}
+
+bool IsOpenvrClientReady();
 #endif
+void _SetChaperoneArea(float areaWidth, float areaHeight);
+
+vr::EVREventType VendorEvent_ALVRDriverResync = (vr::EVREventType) (vr::VREvent_VendorSpecific_Reserved_Start + ((vr::EVREventType) 0xC0));
 
 static void load_debug_privilege(void) {
 #ifdef _WIN32
@@ -210,12 +219,16 @@ class DriverProvider : public vr::IServerTrackedDeviceProvider {
                 HapticsSend(id, haptics.fDurationSeconds, haptics.fFrequency, haptics.fAmplitude);
             }
 #ifdef __linux__
-            else if (event.eventType == vr::VREvent_ChaperoneUniverseHasChanged) {
-                if (hmd && hmd->m_poseHistory) {
-                    InitOpenvrClient();
-                    hmd->m_poseHistory->SetTransformUpdating();
+            else if (event.eventType == vr::VREvent_ChaperoneUniverseHasChanged
+                || event.eventType == vr::VREvent_ChaperoneRoomSetupFinished
+                || event.eventType == vr::VREvent_ChaperoneFlushCache
+                || event.eventType == vr::VREvent_ChaperoneSettingsHaveChanged
+                || event.eventType == vr::VREvent_SeatedZeroPoseReset
+                || event.eventType == vr::VREvent_StandingZeroPoseReset
+                || event.eventType == vr::VREvent_SceneApplicationChanged
+                || event.eventType == VendorEvent_ALVRDriverResync) {
+                if (hmd && hmd->m_poseHistory && IsOpenvrClientReady()) {
                     hmd->m_poseHistory->SetTransform(GetRawZeroPose());
-                    ShutdownOpenvrClient();
                 }
             }
 #endif
@@ -351,6 +364,13 @@ void VideoErrorReportReceive() {
     }
 }
 
+void RequestDriverResync() {
+    if (g_driver_provider.hmd) {
+        vr::VRServerDriverHost()->VendorSpecificEvent(
+            g_driver_provider.hmd->object_id, VendorEvent_ALVRDriverResync, {}, 0);
+    }
+}
+
 void ShutdownSteamvr() {
     if (g_driver_provider.hmd) {
         vr::VRServerDriverHost()->VendorSpecificEvent(
@@ -410,7 +430,6 @@ void SetChaperoneArea(float areaWidth, float areaHeight) {
 
 #ifdef __linux__
     if (g_driver_provider.hmd && g_driver_provider.hmd->m_poseHistory) {
-        g_driver_provider.hmd->m_poseHistory->SetTransformUpdating();
         g_driver_provider.hmd->m_poseHistory->SetTransform(GetRawZeroPose());
     }
 #endif
@@ -423,3 +442,4 @@ void CaptureFrame() {
     }
 #endif
 }
+
diff --git a/alvr/server/cpp/alvr_server/bindings.h b/alvr/server/cpp/alvr_server/bindings.h
@@ -147,6 +147,7 @@ extern "C" void SetTracking(unsigned long long targetTimestampNs,
                             const FfiBodyTracker *bodyTrackers,
                             int bodyTrackersCount);
 extern "C" void VideoErrorReportReceive();
+extern "C" void RequestDriverResync();
 extern "C" void ShutdownSteamvr();
 
 extern "C" void SetOpenvrProperty(unsigned long long deviceID, FfiOpenvrProperty prop);

diff --git a/alvr/server/cpp/alvr_server/include/openvr_math.h b/alvr/server/cpp/alvr_server/include/openvr_math.h
@@ -297,5 +297,45 @@ namespace vrmath {
 		result.m[2][3] = a.m[2][3];
 		return result;
 	}
+
+  inline vr::HmdMatrix34_t matInv33(const vr::HmdMatrix34_t &matrix) {
+    vr::HmdMatrix34_t result;
+    float cofac00 = matrix.m[1][1] * matrix.m[2][2] - matrix.m[1][2] * matrix.m[2][1];
+    float cofac10 = matrix.m[1][2] * matrix.m[2][0] - matrix.m[1][0] * matrix.m[2][2];
+    float cofac20 = matrix.m[1][0] * matrix.m[2][1] - matrix.m[1][1] * matrix.m[2][0];
+
+    float det = matrix.m[0][0] * cofac00 + matrix.m[0][1] * cofac10 + matrix.m[0][2] * cofac20;
+
+    if (det == 0) {
+        vr::HmdMatrix34_t result = { { { 1.0, 0.0, 0.0, 0.0 }, { 0.0, 1.0, 0.0, 0.0 }, { 0.0, 0.0, 1.0, 0.0 } } };
+        return result;
+    }
+
+    float invDet = 1.0f / det;
+
+    float cofac01 = matrix.m[0][2] * matrix.m[2][1] - matrix.m[0][1] * matrix.m[2][2];
+    float cofac02 = matrix.m[0][1] * matrix.m[1][2] - matrix.m[0][2] * matrix.m[1][1];
+    float cofac11 = matrix.m[0][0] * matrix.m[2][2] - matrix.m[0][2] * matrix.m[2][0];
+    float cofac12 = matrix.m[0][2] * matrix.m[1][0] - matrix.m[0][0] * matrix.m[1][2];
+    float cofac21 = matrix.m[0][1] * matrix.m[2][0] - matrix.m[0][0] * matrix.m[2][1];
+    float cofac22 = matrix.m[0][0] * matrix.m[1][1] - matrix.m[0][1] * matrix.m[1][0];
+
+    result.m[0][0] = invDet * cofac00;
+    result.m[0][1] = invDet * cofac01;
+    result.m[0][2] = invDet * cofac02;
+    result.m[0][3] = 0.0f;
+
+    result.m[1][0] = invDet * cofac10;
+    result.m[1][1] = invDet * cofac11;
+    result.m[1][2] = invDet * cofac12;
+    result.m[1][3] = 0.0f;
+
+    result.m[2][0] = invDet * cofac20;
+    result.m[2][1] = invDet * cofac21;
+    result.m[2][2] = invDet * cofac22;
+    result.m[2][3] = 0.0f;
+
+    return result;
+  }
 }
 
diff --git a/alvr/server/src/connection.rs b/alvr/server/src/connection.rs
@@ -1002,6 +1002,7 @@ fn connection_pipeline(
     let statistics_thread = thread::spawn({
         let client_hostname = client_hostname.clone();
         move || {
+            let mut _last_resync = Instant::now();
             while is_streaming(&client_hostname) {
                 let data = match statics_receiver.recv(STREAMING_RECV_TIMEOUT) {
                     Ok(stats) => stats,
@@ -1015,7 +1016,10 @@ fn connection_pipeline(
                 if let Some(stats) = &mut *STATISTICS_MANAGER.lock() {
                     let timestamp = client_stats.target_timestamp;
                     let decoder_latency = client_stats.video_decode;
-                    let network_latency = stats.report_statistics(client_stats);
+                    let (network_latency, _game_latency) = stats.report_statistics(client_stats);
+
+                    #[cfg(target_os = "linux")]
+                    detect_desync(&_game_latency, &mut _last_resync);
 
                     let server_data_lock = SERVER_DATA_MANAGER.read();
                     BITRATE_MANAGER.lock().report_frame_latencies(
@@ -1074,7 +1078,10 @@ fn connection_pipeline(
         let control_sender = Arc::clone(&control_sender);
         let client_hostname = client_hostname.clone();
         move || {
-            unsafe { crate::InitOpenvrClient() };
+            unsafe {
+                crate::InitOpenvrClient();
+                crate::RequestDriverResync();
+            }
 
             let mut disconnection_deadline = Instant::now() + KEEPALIVE_TIMEOUT;
             while is_streaming(&client_hostname) {
@@ -1476,3 +1483,17 @@ pub extern "C" fn send_haptics(device_id: u64, duration_s: f32, frequency: f32,
             .ok();
     }
 }
+
+#[cfg(target_os = "linux")]
+fn detect_desync(game_latency: &Duration, last_resync: &mut Instant) {
+    if game_latency.as_secs_f32() > 0.25 {
+        let now = Instant::now();
+        if now.saturating_duration_since(*last_resync).as_secs_f32() > 0.1 {
+            *last_resync = now;
+            warn!("Desync detected. Attempting recovery.");
+            unsafe {
+                crate::RequestDriverResync();
+            }
+        }
+    }
+}
diff --git a/alvr/server/src/statistics.rs b/alvr/server/src/statistics.rs
@@ -181,8 +181,8 @@ impl StatisticsManager {
     }
 
     // Called every frame. Some statistics are reported once every frame
-    // Returns network latency
-    pub fn report_statistics(&mut self, client_stats: ClientStatistics) -> Duration {
+    // Returns (network latency, game time latency)
+    pub fn report_statistics(&mut self, client_stats: ClientStatistics) -> (Duration, Duration) {
         if let Some(frame) = self
             .history_buffer
             .iter_mut()
@@ -297,9 +297,9 @@ impl StatisticsManager {
                 actual_bitrate_bps: bitrate_bps,
             }));
 
-            network_latency
+            (network_latency, game_time_latency)
         } else {
-            Duration::ZERO
+            (Duration::ZERO, Duration::ZERO)
         }
     }