From 8e8740f2d6d20174d6425b023027963d94669443 Mon Sep 17 00:00:00 2001 From: OneUp Date: Tue, 16 Jul 2024 19:20:12 -0500 Subject: [PATCH] Add velocity and acceleration to position/attitude --- vrto3d/src/hmd_device_driver.cpp | 53 ++++++++++++++++++++++++++------ 1 file changed, 44 insertions(+), 9 deletions(-) diff --git a/vrto3d/src/hmd_device_driver.cpp b/vrto3d/src/hmd_device_driver.cpp index c7551cd..28a9dc4 100644 --- a/vrto3d/src/hmd_device_driver.cpp +++ b/vrto3d/src/hmd_device_driver.cpp @@ -313,18 +313,21 @@ void MockControllerDeviceDriver::DebugRequest( const char *pchRequest, char *pch //----------------------------------------------------------------------------- vr::DriverPose_t MockControllerDeviceDriver::GetPose() { + static const float updateInterval = 1.0f / stereo_display_component_->GetConfig().display_frequency; // Update interval in seconds static float currentPitch = 0.0f; // Keep track of the current pitch static float currentYaw = 0.0f; // Keep track of the current yaw + static float lastPitch = 0.0f; + static float lastYaw = 0.0f; + static float lastPos[3] = { 0.0f, 0.0f, 0.0f }; // Last position vector + static float lastVel[3] = { 0.0f, 0.0f, 0.0f }; // Last velocity vector + static float lastAngVel[3] = { 0.0f, 0.0f, 0.0f }; // Last angular velocity vector vr::DriverPose_t pose = { 0 }; pose.qWorldFromDriverRotation = HmdQuaternion_Identity; pose.qDriverFromHeadRotation = HmdQuaternion_Identity; - pose.qRotation = HmdQuaternion_Identity; - float radius = stereo_display_component_->GetConfig().pitch_radius; // Configurable radius for pitch - // Adjust pitch based on controller input if (stereo_display_component_->GetConfig().pitch_enable) { @@ -337,17 +340,49 @@ vr::DriverPose_t MockControllerDeviceDriver::GetPose() stereo_display_component_->AdjustYaw(currentYaw); } - // Calculate the vertical position based on pitch and radius float pitchRadians = DEG_TO_RAD(currentPitch); float yawRadians = DEG_TO_RAD(currentYaw); - pose.vecPosition[0] = radius * sin(pitchRadians) * sin(yawRadians); // Adjust for yaw in the x direction - pose.vecPosition[1] = stereo_display_component_->GetConfig().hmd_height - radius * (1 - cos(pitchRadians)); // Adjust the height based on pitch - pose.vecPosition[2] = radius * sin(pitchRadians) * cos(yawRadians); // Adjust for yaw in the z direction + float radius = stereo_display_component_->GetConfig().pitch_radius; // Configurable radius for pitch // Recompose the rotation quaternion from pitch and yaw - vr::HmdQuaternion_t pitchQuaternion = HmdQuaternion_FromEulerAngles(0, DEG_TO_RAD(currentPitch), 0); - vr::HmdQuaternion_t yawQuaternion = HmdQuaternion_FromEulerAngles(0, 0, DEG_TO_RAD(currentYaw)); + vr::HmdQuaternion_t pitchQuaternion = HmdQuaternion_FromEulerAngles(0, pitchRadians, 0); + vr::HmdQuaternion_t yawQuaternion = HmdQuaternion_FromEulerAngles(0, 0, yawRadians); pose.qRotation = HmdQuaternion_Normalize(yawQuaternion * pitchQuaternion); + + // Calculate the new position relative to the current pitch & yaw + pose.vecPosition[0] = radius * sin(pitchRadians) * sin(yawRadians); + pose.vecPosition[1] = stereo_display_component_->GetConfig().hmd_height - radius * (1 - cos(pitchRadians)); + pose.vecPosition[2] = radius * sin(pitchRadians) * cos(yawRadians); + + // Calculate velocity components based on change in position + pose.vecVelocity[0] = (pose.vecPosition[0] - lastPos[0]) / updateInterval; + pose.vecVelocity[1] = (pose.vecPosition[1] - lastPos[1]) / updateInterval; + pose.vecVelocity[2] = (pose.vecPosition[2] - lastPos[2]) / updateInterval; + + // Calculate velocity using known update interval + pose.vecAngularVelocity[0] = (pitchRadians - lastPitch) / updateInterval; // Pitch angular velocity + pose.vecAngularVelocity[1] = (yawRadians - lastYaw) / updateInterval; // Yaw angular velocity + pose.vecAngularVelocity[2] = 0.0f; + + // Calculate acceleration based on change in velocity + pose.vecAcceleration[0] = (pose.vecVelocity[0] - lastVel[0]) / updateInterval; + pose.vecAcceleration[1] = (pose.vecVelocity[1] - lastVel[1]) / updateInterval; + pose.vecAcceleration[2] = (pose.vecVelocity[2] - lastVel[2]) / updateInterval; + pose.vecAngularAcceleration[0] = (pose.vecAngularVelocity[0] - lastAngVel[0]) / updateInterval; + pose.vecAngularAcceleration[1] = (pose.vecAngularVelocity[1] - lastAngVel[1]) / updateInterval; + pose.vecAngularAcceleration[2] = 0.0f; + + // Update for next iteration + lastPitch = pitchRadians; + lastYaw = yawRadians; + lastPos[0] = pose.vecPosition[0]; + lastPos[1] = pose.vecPosition[1]; + lastPos[2] = pose.vecPosition[2]; + lastVel[0] = pose.vecVelocity[0]; + lastVel[1] = pose.vecVelocity[1]; + lastVel[2] = pose.vecVelocity[2]; + lastAngVel[0] = pose.vecAngularVelocity[0]; + lastAngVel[1] = pose.vecAngularVelocity[1]; pose.poseIsValid = true; pose.deviceIsConnected = true;