fix(server_openvr): Calculate velocities for skeleton-based controlle…

…rs (#2416) * Calculate velocities for skeleton-based controllers * format * no chrono, no verbs * hopefully last nits
alvr-org · Sep 20, 2024 · 1cbb133 · 1cbb133
1 parent 365b884
commit 1cbb133
Show file tree

Hide file tree

Showing 4 changed files with 47 additions and 12 deletions.
diff --git a/alvr/server_openvr/cpp/alvr_server/Controller.cpp b/alvr/server_openvr/cpp/alvr_server/Controller.cpp
@@ -220,7 +220,7 @@ void Controller::SetButton(uint64_t id, FfiButtonValue value) {
     }
 }
 
-bool Controller::onPoseUpdate(float predictionS, FfiHandData handData) {
+bool Controller::onPoseUpdate(uint64_t targetTimestampNs, float predictionS, FfiHandData handData) {
     if (this->object_id == vr::k_unTrackedDeviceIndexInvalid) {
         return false;
     }
@@ -282,18 +282,35 @@ bool Controller::onPoseUpdate(float predictionS, FfiHandData handData) {
         pose.vecPosition[1] = p[1];
         pose.vecPosition[2] = p[2];
 
-        pose.vecVelocity[0] = 0;
-        pose.vecVelocity[1] = 0;
-        pose.vecVelocity[2] = 0;
+        // If possible, use the last stored m_pose and timestamp
+        // to calculate the velocities of the current pose.
+        double linearVelocity[3] = { 0.0, 0.0, 0.0 };
+        vr::HmdVector3d_t angularVelocity = { 0.0, 0.0, 0.0 };
 
-        pose.vecAngularVelocity[0] = 0;
-        pose.vecAngularVelocity[1] = 0;
-        pose.vecAngularVelocity[2] = 0;
+        if (m_pose.poseIsValid) {
+            double dt = ((double)targetTimestampNs - (double)m_poseTargetTimestampNs) / NS_PER_S;
+
+            if (dt > 0.0) {
+                linearVelocity[0] = (pose.vecPosition[0] - m_pose.vecPosition[0]) / dt;
+                linearVelocity[1] = (pose.vecPosition[1] - m_pose.vecPosition[1]) / dt;
+                linearVelocity[2] = (pose.vecPosition[2] - m_pose.vecPosition[2]) / dt;
+                angularVelocity = AngularVelocityBetweenQuats(m_pose.qRotation, pose.qRotation, dt);
+            }
+        }
+
+        pose.vecVelocity[0] = linearVelocity[0];
+        pose.vecVelocity[1] = linearVelocity[1];
+        pose.vecVelocity[2] = linearVelocity[2];
+
+        pose.vecAngularVelocity[0] = angularVelocity.v[0];
+        pose.vecAngularVelocity[1] = angularVelocity.v[1];
+        pose.vecAngularVelocity[2] = angularVelocity.v[2];
     }
 
     pose.poseTimeOffset = predictionS;
 
     m_pose = pose;
+    m_poseTargetTimestampNs = targetTimestampNs;
 
     vr::VRServerDriverHost()->TrackedDevicePoseUpdated(
         this->object_id, pose, sizeof(vr::DriverPose_t)

diff --git a/alvr/server_openvr/cpp/alvr_server/Controller.h b/alvr/server_openvr/cpp/alvr_server/Controller.h
@@ -37,7 +37,7 @@ class Controller : public TrackedDevice, public vr::ITrackedDeviceServerDriver {
 
     void SetButton(uint64_t id, FfiButtonValue value);
 
-    bool onPoseUpdate(float predictionS, FfiHandData handData);
+    bool onPoseUpdate(uint64_t targetTimestampNs, float predictionS, FfiHandData handData);
 
     void GetBoneTransform(bool withController, vr::VRBoneTransform_t outBoneTransform[]);
 
@@ -54,6 +54,7 @@ class Controller : public TrackedDevice, public vr::ITrackedDeviceServerDriver {
     vr::EVRSkeletalTrackingLevel m_skeletonLevel;
 
     vr::DriverPose_t m_pose;
+    uint64_t m_poseTargetTimestampNs;
 
     // These variables are used for controller hand animation
     // todo: move to rust

diff --git a/alvr/server_openvr/cpp/alvr_server/Utils.h b/alvr/server_openvr/cpp/alvr_server/Utils.h
@@ -27,6 +27,7 @@
 #include "openvr_driver.h"
 
 const float DEG_TO_RAD = (float)(M_PI / 180.);
+const double NS_PER_S = 1000000000.0;
 
 // Get elapsed time in us from Unix Epoch
 inline uint64_t GetTimestampUs() {
@@ -163,6 +164,16 @@ Slerp(vr::HmdQuaternionf_t& q1, vr::HmdQuaternionf_t& q2, double lambda) {
     }
 }
 
+// Sourced from https://mariogc.com/post/angular-velocity-quaternions/
+inline vr::HmdVector3d_t AngularVelocityBetweenQuats(
+    const vr::HmdQuaternion_t& q1, const vr::HmdQuaternion_t& q2, double dt
+) {
+    double r = (2.0f / dt);
+    return { (q1.w * q2.x - q1.x * q2.w - q1.y * q2.z + q1.z * q2.y) * r,
+             (q1.w * q2.y + q1.x * q2.z - q1.y * q2.w - q1.z * q2.x) * r,
+             (q1.w * q2.z - q1.x * q2.y + q1.y * q2.x - q1.z * q2.w) * r };
+}
+
 #ifdef _WIN32
 typedef void(WINAPI* RtlGetVersion_FUNC)(OSVERSIONINFOEXW*);
 

diff --git a/alvr/server_openvr/cpp/alvr_server/alvr_server.cpp b/alvr/server_openvr/cpp/alvr_server/alvr_server.cpp
@@ -417,21 +417,27 @@ void SetTracking(
     }
 
     if (g_driver_provider.left_hand_tracker) {
-        g_driver_provider.left_hand_tracker->onPoseUpdate(controllerPoseTimeOffsetS, leftHandData);
+        g_driver_provider.left_hand_tracker->onPoseUpdate(
+            targetTimestampNs, controllerPoseTimeOffsetS, leftHandData
+        );
     }
 
     if (g_driver_provider.left_controller) {
-        g_driver_provider.left_controller->onPoseUpdate(controllerPoseTimeOffsetS, leftHandData);
+        g_driver_provider.left_controller->onPoseUpdate(
+            targetTimestampNs, controllerPoseTimeOffsetS, leftHandData
+        );
     }
 
     if (g_driver_provider.right_hand_tracker) {
         g_driver_provider.right_hand_tracker->onPoseUpdate(
-            controllerPoseTimeOffsetS, rightHandData
+            targetTimestampNs, controllerPoseTimeOffsetS, rightHandData
         );
     }
 
     if (g_driver_provider.right_controller) {
-        g_driver_provider.right_controller->onPoseUpdate(controllerPoseTimeOffsetS, rightHandData);
+        g_driver_provider.right_controller->onPoseUpdate(
+            targetTimestampNs, controllerPoseTimeOffsetS, rightHandData
+        );
     }
 
     if (Settings::Instance().m_enableBodyTrackingFakeVive) {