diff --git a/alvr/server_openvr/cpp/alvr_server/Controller.cpp b/alvr/server_openvr/cpp/alvr_server/Controller.cpp index 52a6a0fc42..25237c6170 100644 --- 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 index fbc4794ef4..975b51a0fc 100644 --- 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 index dbbf55de1b..05f794b994 100644 --- 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 index 38d6ccf1d8..ee85748464 100644 --- 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) {