From 13660e271446bb3b91b33798775e25ad99597358 Mon Sep 17 00:00:00 2001 From: The-Cyber-Captain Date: Sat, 12 Oct 2024 23:23:43 +0100 Subject: [PATCH 1/3] Reworked geometric algebra. Accesses Basis via get_column. --- ...nxr_fb_body_tracking_extension_wrapper.cpp | 21 +++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/plugin/src/main/cpp/extensions/openxr_fb_body_tracking_extension_wrapper.cpp b/plugin/src/main/cpp/extensions/openxr_fb_body_tracking_extension_wrapper.cpp index 3f4d5863..53773d64 100644 --- a/plugin/src/main/cpp/extensions/openxr_fb_body_tracking_extension_wrapper.cpp +++ b/plugin/src/main/cpp/extensions/openxr_fb_body_tracking_extension_wrapper.cpp @@ -314,17 +314,22 @@ void OpenXRFbBodyTrackingExtensionWrapper::_on_process() { // with how models are designed, rather than the Meta positions of // the tips of the clavicles. if (locations.isActive) { + // Construct a root under the hips pointing forwards + // IE, +z aligns with hips & user's real-world forward. + // (Remaining, however, on the XRorigin / Global XZ plane; root's basis rotated around Y to fit) + // Get the hips transform Transform3D hips = xr_body_tracker->get_joint_transform(XRBodyTracker::JOINT_HIPS); - - // Construct the root under the hips pointing forwards Vector3 root_y = Vector3(0.0, 1.0, 0.0); - Vector3 root_z = -hips.basis[Vector3::AXIS_X].cross(root_y); - Vector3 root_x = root_y.cross(root_z); + Vector3 hips_left = hips.basis.get_column(Vector3::AXIS_X); + // flatten hips' left (x basis) on to real-world / global ground plane. Normalize. + Vector3 root_x = (hips_left.slide(Vector3(0.0, 1.0, 0.0))).normalized(); + Vector3 root_z = root_x.cross(root_y); + // flatten hips' origin on to ground plane. Vector3 root_o = hips.origin.slide(Vector3(0.0, 1.0, 0.0)); + // set the actual root transform Transform3D root = Transform3D(root_x, root_y, root_z, root_o).orthonormalized(); xr_body_tracker->set_joint_transform(XRBodyTracker::JOINT_ROOT, root); - xr_body_tracker->set_pose("default", root, Vector3(), Vector3(), XRPose::XR_TRACKING_CONFIDENCE_HIGH); // Distance in meters to push the shoulder joints back from the // clavicle-position to be in-line with the upper arm joints as @@ -333,7 +338,7 @@ void OpenXRFbBodyTrackingExtensionWrapper::_on_process() { // Deduce the shoulder offset from the upper chest transform Transform3D upper_chest = xr_body_tracker->get_joint_transform(XRBodyTracker::JOINT_UPPER_CHEST); - Vector3 shoulder_offset = upper_chest.basis[2] * shoulder_z_offset; + Vector3 shoulder_offset = upper_chest.basis.get_column(Vector3::AXIS_Z) * shoulder_z_offset; // Correct the left shoulder Transform3D left_shoulder = xr_body_tracker->get_joint_transform(XRBodyTracker::JOINT_LEFT_SHOULDER); @@ -344,6 +349,10 @@ void OpenXRFbBodyTrackingExtensionWrapper::_on_process() { Transform3D right_shoulder = xr_body_tracker->get_joint_transform(XRBodyTracker::JOINT_RIGHT_SHOULDER); right_shoulder.origin += shoulder_offset; xr_body_tracker->set_joint_transform(XRBodyTracker::JOINT_RIGHT_SHOULDER, right_shoulder); + + // Set tracker pose, velocities, confidence. + // Good to go. + xr_body_tracker->set_pose("default", root, Vector3(), Vector3(), XRPose::XR_TRACKING_CONFIDENCE_HIGH); } // Register the XRBodyTracker if necessary From 1c56797affed47541037394bdf1fbe57c97e966e Mon Sep 17 00:00:00 2001 From: The-Cyber-Captain Date: Mon, 4 Nov 2024 08:26:13 +0000 Subject: [PATCH 2/3] Tidied comments. Reinstated root tracker's position. --- ...enxr_fb_body_tracking_extension_wrapper.cpp | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/plugin/src/main/cpp/extensions/openxr_fb_body_tracking_extension_wrapper.cpp b/plugin/src/main/cpp/extensions/openxr_fb_body_tracking_extension_wrapper.cpp index 53773d64..a691e03e 100644 --- a/plugin/src/main/cpp/extensions/openxr_fb_body_tracking_extension_wrapper.cpp +++ b/plugin/src/main/cpp/extensions/openxr_fb_body_tracking_extension_wrapper.cpp @@ -307,29 +307,28 @@ void OpenXRFbBodyTrackingExtensionWrapper::_on_process() { // If the location data is good then we need to apply some corrections // before handing the data back to Godot. These include: // - // - The Root joint carries no data, so instead place it on the ground + // - The Root joint carries no useful orientation data, so instead align it // under the hips pointing forwards. // // - Adjusting the left and right shoulder back so they are aligned // with how models are designed, rather than the Meta positions of // the tips of the clavicles. if (locations.isActive) { - // Construct a root under the hips pointing forwards - // IE, +z aligns with hips & user's real-world forward. - // (Remaining, however, on the XRorigin / Global XZ plane; root's basis rotated around Y to fit) + // Align root under the hips pointing 'forwards' + // IE, +z aligns with hips & user's real-world [upper-body] forward. + // (Remaining, however, parallel to the XRorigin / Global XZ plane; root's basis rotated around Y to fit) // Get the hips transform Transform3D hips = xr_body_tracker->get_joint_transform(XRBodyTracker::JOINT_HIPS); Vector3 root_y = Vector3(0.0, 1.0, 0.0); Vector3 hips_left = hips.basis.get_column(Vector3::AXIS_X); - // flatten hips' left (x basis) on to real-world / global ground plane. Normalize. Vector3 root_x = (hips_left.slide(Vector3(0.0, 1.0, 0.0))).normalized(); Vector3 root_z = root_x.cross(root_y); - // flatten hips' origin on to ground plane. - Vector3 root_o = hips.origin.slide(Vector3(0.0, 1.0, 0.0)); - // set the actual root transform + Vector3 root_o = xr_body_tracker->get_joint_transform(XRBodyTracker::JOINT_ROOT).origin; Transform3D root = Transform3D(root_x, root_y, root_z, root_o).orthonormalized(); xr_body_tracker->set_joint_transform(XRBodyTracker::JOINT_ROOT, root); + // Set tracker pose, velocities, confidence. + xr_body_tracker->set_pose("default", root, Vector3(), Vector3(), XRPose::XR_TRACKING_CONFIDENCE_HIGH); // Distance in meters to push the shoulder joints back from the // clavicle-position to be in-line with the upper arm joints as @@ -350,9 +349,6 @@ void OpenXRFbBodyTrackingExtensionWrapper::_on_process() { right_shoulder.origin += shoulder_offset; xr_body_tracker->set_joint_transform(XRBodyTracker::JOINT_RIGHT_SHOULDER, right_shoulder); - // Set tracker pose, velocities, confidence. - // Good to go. - xr_body_tracker->set_pose("default", root, Vector3(), Vector3(), XRPose::XR_TRACKING_CONFIDENCE_HIGH); } // Register the XRBodyTracker if necessary From 8141970815fd51fd5412dae8660875f0206f2bed Mon Sep 17 00:00:00 2001 From: The-Cyber-Captain Date: Mon, 4 Nov 2024 08:43:18 +0000 Subject: [PATCH 3/3] Adjusting for clang --- .../cpp/extensions/openxr_fb_body_tracking_extension_wrapper.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/plugin/src/main/cpp/extensions/openxr_fb_body_tracking_extension_wrapper.cpp b/plugin/src/main/cpp/extensions/openxr_fb_body_tracking_extension_wrapper.cpp index a691e03e..d97d99c6 100644 --- a/plugin/src/main/cpp/extensions/openxr_fb_body_tracking_extension_wrapper.cpp +++ b/plugin/src/main/cpp/extensions/openxr_fb_body_tracking_extension_wrapper.cpp @@ -348,7 +348,6 @@ void OpenXRFbBodyTrackingExtensionWrapper::_on_process() { Transform3D right_shoulder = xr_body_tracker->get_joint_transform(XRBodyTracker::JOINT_RIGHT_SHOULDER); right_shoulder.origin += shoulder_offset; xr_body_tracker->set_joint_transform(XRBodyTracker::JOINT_RIGHT_SHOULDER, right_shoulder); - } // Register the XRBodyTracker if necessary