From 5c1b829555511699e63115b28bc275fdcdf7be52 Mon Sep 17 00:00:00 2001 From: Bastiaan Olij Date: Tue, 21 Nov 2023 11:47:45 +1100 Subject: [PATCH] Fix stereo projection --- example.gd/main.tscn | 15 ++ extension/T5Integration/Glasses.cpp | 3 +- extension/T5Integration/T5Math.h | 2 +- extension/src/GodotT5Glasses.cpp | 233 +++++++++++++------------- extension/src/GodotT5Glasses.h | 5 +- extension/src/GodotT5Service.cpp | 6 +- extension/src/GodotT5Service.h | 2 +- extension/src/TiltFiveXRInterface.cpp | 17 +- 8 files changed, 160 insertions(+), 123 deletions(-) diff --git a/example.gd/main.tscn b/example.gd/main.tscn index 857d1d6..501cb36 100644 --- a/example.gd/main.tscn +++ b/example.gd/main.tscn @@ -73,6 +73,11 @@ mesh = SubResource("BoxMesh_gbwc2") skeleton = NodePath("../..") surface_material_override/0 = SubResource("StandardMaterial3D_70r0u") +[node name="Label3D" type="Label3D" parent="Boxes/PositiveYBody/Positive Y"] +transform = Transform3D(1, 0, 0, 0, 0.707107, 0.707107, 0, -0.707107, 0.707107, 0, 0.922618, 0) +pixel_size = 0.01 +text = "Up (Y+)" + [node name="CollisionShape3D" type="CollisionShape3D" parent="Boxes/PositiveYBody"] shape = SubResource("BoxShape3D_1q0d0") @@ -86,6 +91,11 @@ transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 1.5) mesh = SubResource("BoxMesh_gbwc2") surface_material_override/0 = SubResource("StandardMaterial3D_dji1h") +[node name="Label3D" type="Label3D" parent="Boxes/Positive Z"] +transform = Transform3D(1, 0, 0, 0, 0.707107, 0.707107, 0, -0.707107, 0.707107, 0, 0, 1.08084) +pixel_size = 0.01 +text = "Forward (Z+)" + [node name="Negative Z" type="MeshInstance3D" parent="Boxes"] transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, -1.5) mesh = SubResource("BoxMesh_gbwc2") @@ -96,6 +106,11 @@ transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 1.5, 0, 0) mesh = SubResource("BoxMesh_qm7rn") surface_material_override/0 = SubResource("StandardMaterial3D_0arwu") +[node name="Label3D" type="Label3D" parent="Boxes/Positive X"] +transform = Transform3D(1, 0, 0, 0, 0.707107, 0.707107, 0, -0.707107, 0.707107, 1.3681, 0, 0) +pixel_size = 0.01 +text = "Right (X+)" + [node name="Negative X" type="MeshInstance3D" parent="Boxes"] transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, -1.5, 0, 0) mesh = SubResource("BoxMesh_qm7rn") diff --git a/extension/T5Integration/Glasses.cpp b/extension/T5Integration/Glasses.cpp index 2f685bb..5165d6b 100644 --- a/extension/T5Integration/Glasses.cpp +++ b/extension/T5Integration/Glasses.cpp @@ -501,7 +501,8 @@ namespace T5Integration { pose.rotToGLS_GBD.w, pos.x, pos.y, - pos.z); + pos.z, + true); pos.x += pose.posGLS_GBD.x; pos.y += pose.posGLS_GBD.y; diff --git a/extension/T5Integration/T5Math.h b/extension/T5Integration/T5Math.h index 5910452..69d7639 100644 --- a/extension/T5Integration/T5Math.h +++ b/extension/T5Integration/T5Math.h @@ -6,7 +6,7 @@ namespace T5Integration { public: using Ptr = std::shared_ptr; - virtual void rotate_vector(float quat_x, float quat_y, float quat_z, float quat_w, float& vec_x, float& vec_y, float& vec_z) = 0; + virtual void rotate_vector(float quat_x, float quat_y, float quat_z, float quat_w, float& vec_x, float& vec_y, float& vec_z, bool inverse = false) = 0; }; } \ No newline at end of file diff --git a/extension/src/GodotT5Glasses.cpp b/extension/src/GodotT5Glasses.cpp index dc5fe08..c226cac 100644 --- a/extension/src/GodotT5Glasses.cpp +++ b/extension/src/GodotT5Glasses.cpp @@ -20,35 +20,43 @@ namespace GodotT5Integration { GodotT5Glasses::GodotT5Glasses(std::string_view id) : Glasses(id) { - set_swap_chain_size(g_swap_chain_length); + set_swap_chain_size(g_swap_chain_length); } -Transform3D GodotT5Glasses::get_head_transform() { +Transform3D GodotT5Glasses::get_head_transform(Vector3 eye_offset) { Quaternion orientation; Vector3 position; + get_glasses_orientation(orientation.x, orientation.y, orientation.z, orientation.w); get_glasses_position(position.x, position.y, position.z); + // Tiltfive -> Godot axis + position = Vector3(position.x, position.z, -position.y); + orientation = Quaternion(orientation.x, orientation.z, -orientation.y, orientation.w); + orientation = orientation.inverse(); + Transform3D headPose; - headPose.set_origin(position); - headPose.set_basis(orientation.inverse()); - headPose.rotate(Vector3(1,0,0), -Math_PI / 2.0f); + headPose.set_origin(position); + headPose.set_basis(orientation); + + Transform3D axisAdjust; + axisAdjust.rotate(Vector3(1.0, 0.0, 0.0), -Math_PI / 2.0f); + + Transform3D eye_pose; + eye_pose.set_origin(eye_offset); - return headPose; + return headPose * axisAdjust * eye_pose; } -Transform3D GodotT5Glasses::get_eye_offset(Glasses::Eye eye) { +Vector3 GodotT5Glasses::get_eye_offset(Glasses::Eye eye) { float dir = (eye == Glasses::Left ? -1.0f : 1.0f); auto ipd = get_ipd(); - Transform3D eye_pose; - eye_pose.set_origin(Vector3(dir * ipd / 2.0f, 0, 0)); - - return eye_pose; + return Vector3(dir * ipd / 2.0f, 0, 0); } Transform3D GodotT5Glasses::get_eye_transform(Glasses::Eye eye) { - return get_eye_offset(eye) * get_head_transform(); + return get_head_transform(get_eye_offset(eye)); } Transform3D GodotT5Glasses::get_wand_transform(size_t wand_num) { @@ -57,12 +65,13 @@ Transform3D GodotT5Glasses::get_wand_transform(size_t wand_num) { get_wand_position(wand_num, position.x, position.y, position.z); get_wand_orientation(wand_num, orientation.x, orientation.y, orientation.z, orientation.w); - position = Vector3(position.x, position.z, -position.y); - orientation = Quaternion(orientation.x, orientation.z, -orientation.y, orientation.w); - orientation = orientation.inverse(); + // Tiltfive -> Godot axis + position = Vector3(position.x, position.z, -position.y); + orientation = Quaternion(orientation.x, orientation.z, -orientation.y, orientation.w); + orientation = orientation.inverse(); Transform3D wandPose; - wandPose.set_origin(position); + wandPose.set_origin(position); wandPose.set_basis(orientation * Quaternion(Vector3(1,0,0), Math_PI / 2.0f)); return wandPose; @@ -71,24 +80,24 @@ Transform3D GodotT5Glasses::get_wand_transform(size_t wand_num) { PackedFloat64Array GodotT5Glasses::get_projection_for_eye(Glasses::Eye view, double aspect, double z_near, double z_far) { PackedFloat64Array arr; arr.resize(16); // 4x4 matrix - arr.fill(0); + arr.fill(0); - Projection cm; - cm.set_perspective(get_fov(), aspect, z_near, z_far); + Projection cm; + cm.set_perspective(get_fov(), aspect, z_near, z_far); - real_t *m = (real_t *)cm.columns; + real_t *m = (real_t *)cm.columns; for (int i = 0; i < 16; i++) { arr[i] = m[i]; } - return arr; + return arr; } void GodotT5Glasses::on_glasses_reserved() { XRServer *xr_server = XRServer::get_singleton(); ERR_FAIL_NULL(xr_server); - auto tracker_name = std::format("/user/{}/head", get_id()); + auto tracker_name = std::format("/user/{}/head", get_id()); _head.instantiate(); _head->set_tracker_type(XRServer::TRACKER_HEAD); @@ -98,121 +107,121 @@ void GodotT5Glasses::on_glasses_reserved() { } void GodotT5Glasses::on_glasses_released() { - if(_head.is_valid()) { - XRServer *xr_server = XRServer::get_singleton(); - ERR_FAIL_NULL(xr_server); + if(_head.is_valid()) { + XRServer *xr_server = XRServer::get_singleton(); + ERR_FAIL_NULL(xr_server); - xr_server->remove_tracker(_head); - } + xr_server->remove_tracker(_head); + } } void GodotT5Glasses::on_glasses_dropped() { - if(_head.is_valid()) { - XRServer *xr_server = XRServer::get_singleton(); - ERR_FAIL_NULL(xr_server); + if(_head.is_valid()) { + XRServer *xr_server = XRServer::get_singleton(); + ERR_FAIL_NULL(xr_server); - xr_server->remove_tracker(_head); - } + xr_server->remove_tracker(_head); + } } void GodotT5Glasses::on_tracking_updated() { - if(_head.is_valid()) { - if (is_tracking()) { - _head->set_pose( - "default", - get_head_transform(), - Vector3(), - Vector3(), - godot::XRPose::XR_TRACKING_CONFIDENCE_HIGH); - } else { - _head->invalidate_pose("default"); - } - } - - auto num_wands = get_num_wands(); - for(int wand_idx = 0; wand_idx < num_wands; ++wand_idx) { - if(wand_idx == _wand_trackers.size()) - add_tracker(); - update_wand(wand_idx); - } + if(_head.is_valid()) { + if (is_tracking()) { + _head->set_pose( + "default", + get_head_transform(), + Vector3(), + Vector3(), + godot::XRPose::XR_TRACKING_CONFIDENCE_HIGH); + } else { + _head->invalidate_pose("default"); + } + } + + auto num_wands = get_num_wands(); + for(int wand_idx = 0; wand_idx < num_wands; ++wand_idx) { + if(wand_idx == _wand_trackers.size()) + add_tracker(); + update_wand(wand_idx); + } } bool GodotT5Glasses::is_in_use() { - auto current_state = get_current_state(); - return (current_state & GlassesState::SUSTAIN_CONNECTION) || (current_state & GlassesState::UNAVAILABLE); + auto current_state = get_current_state(); + return (current_state & GlassesState::SUSTAIN_CONNECTION) || (current_state & GlassesState::UNAVAILABLE); } Vector2 GodotT5Glasses::get_render_size() { - int width; - int height; - Glasses::get_display_size(width, height); + int width; + int height; + Glasses::get_display_size(width, height); - return Vector2(width, height); + return Vector2(width, height); } void GodotT5Glasses::add_tracker() { - int new_idx = _wand_trackers.size(); - int new_id = new_idx + 1; + int new_idx = _wand_trackers.size(); + int new_id = new_idx + 1; - Ref positional_tracker; - positional_tracker.instantiate(); + Ref positional_tracker; + positional_tracker.instantiate(); - auto tracker_name = std::format("/user/{}/wand_{}", get_id(), new_id); + auto tracker_name = std::format("/user/{}/wand_{}", get_id(), new_id); - positional_tracker->set_tracker_type(XRServer::TRACKER_CONTROLLER); - positional_tracker->set_tracker_name(tracker_name.c_str()); - positional_tracker->set_tracker_desc(tracker_name.c_str()); + positional_tracker->set_tracker_type(XRServer::TRACKER_CONTROLLER); + positional_tracker->set_tracker_name(tracker_name.c_str()); + positional_tracker->set_tracker_desc(tracker_name.c_str()); - _wand_trackers.push_back(positional_tracker); + _wand_trackers.push_back(positional_tracker); } void GodotT5Glasses::update_wand(size_t wand_idx) { - auto xr_server = XRServer::get_singleton(); - - auto tracker = _wand_trackers[wand_idx]; - - if(is_wand_state_changed(wand_idx, WandState::CONNECTED)) { - if(is_wand_state_set(wand_idx, WandState::CONNECTED)) { - xr_server->add_tracker(tracker); - } else { - xr_server->remove_tracker(tracker); - return; - } - } - if(is_wand_state_set(wand_idx, WandState::POSE_VALID)) { - auto wand_transform = get_wand_transform(wand_idx); - tracker->set_pose("default", wand_transform, Vector3(), Vector3(), godot::XRPose::XR_TRACKING_CONFIDENCE_HIGH); - } else { - tracker->invalidate_pose("default"); - } - if(is_wand_state_set(wand_idx, WandState::ANALOG_VALID)) { - float trigger_value; - get_wand_trigger(wand_idx, trigger_value); - tracker->set_input("trigger", Variant(trigger_value)); - Vector2 stick; - get_wand_stick(wand_idx, stick.x, stick.y); - tracker->set_input("stick", Variant(stick)); - - if(trigger_value > _trigger_click_threshold + g_trigger_hysteresis_range) { - tracker->set_input("trigger_click", Variant(true)); - } - else if(trigger_value < (_trigger_click_threshold - g_trigger_hysteresis_range)) { - tracker->set_input("trigger_click", Variant(false)); - } - } - if(is_wand_state_set(wand_idx, WandState::BUTTONS_VALID)) { - WandButtons buttons; - get_wand_buttons(wand_idx, buttons); - - tracker->set_input("button_a", Variant(buttons.a)); - tracker->set_input("button_b", Variant(buttons.b)); - tracker->set_input("button_x", Variant(buttons.x)); - tracker->set_input("button_y", Variant(buttons.y)); - tracker->set_input("button_1", Variant(buttons.one)); - tracker->set_input("button_2", Variant(buttons.two)); - tracker->set_input("button_3", Variant(buttons.three)); - tracker->set_input("button_t5", Variant(buttons.t5)); - } + auto xr_server = XRServer::get_singleton(); + + auto tracker = _wand_trackers[wand_idx]; + + if(is_wand_state_changed(wand_idx, WandState::CONNECTED)) { + if(is_wand_state_set(wand_idx, WandState::CONNECTED)) { + xr_server->add_tracker(tracker); + } else { + xr_server->remove_tracker(tracker); + return; + } + } + if(is_wand_state_set(wand_idx, WandState::POSE_VALID)) { + auto wand_transform = get_wand_transform(wand_idx); + tracker->set_pose("default", wand_transform, Vector3(), Vector3(), godot::XRPose::XR_TRACKING_CONFIDENCE_HIGH); + } else { + tracker->invalidate_pose("default"); + } + if(is_wand_state_set(wand_idx, WandState::ANALOG_VALID)) { + float trigger_value; + get_wand_trigger(wand_idx, trigger_value); + tracker->set_input("trigger", Variant(trigger_value)); + Vector2 stick; + get_wand_stick(wand_idx, stick.x, stick.y); + tracker->set_input("stick", Variant(stick)); + + if(trigger_value > _trigger_click_threshold + g_trigger_hysteresis_range) { + tracker->set_input("trigger_click", Variant(true)); + } + else if(trigger_value < (_trigger_click_threshold - g_trigger_hysteresis_range)) { + tracker->set_input("trigger_click", Variant(false)); + } + } + if(is_wand_state_set(wand_idx, WandState::BUTTONS_VALID)) { + WandButtons buttons; + get_wand_buttons(wand_idx, buttons); + + tracker->set_input("button_a", Variant(buttons.a)); + tracker->set_input("button_b", Variant(buttons.b)); + tracker->set_input("button_x", Variant(buttons.x)); + tracker->set_input("button_y", Variant(buttons.y)); + tracker->set_input("button_1", Variant(buttons.one)); + tracker->set_input("button_2", Variant(buttons.two)); + tracker->set_input("button_3", Variant(buttons.three)); + tracker->set_input("button_t5", Variant(buttons.t5)); + } } } // GodotT5Integration \ No newline at end of file diff --git a/extension/src/GodotT5Glasses.h b/extension/src/GodotT5Glasses.h index ec0b0f2..e8a8381 100644 --- a/extension/src/GodotT5Glasses.h +++ b/extension/src/GodotT5Glasses.h @@ -11,6 +11,7 @@ using godot::RID; using godot::Ref; using godot::XRPositionalTracker; using godot::Vector2; +using godot::Vector3; using godot::Transform3D; using godot::StringName; using godot::PackedFloat64Array; @@ -37,8 +38,8 @@ namespace GodotT5Integration { bool is_reserved(); Vector2 get_render_size(); - virtual Transform3D get_head_transform(); - virtual Transform3D get_eye_offset(Glasses::Eye eye); + virtual Transform3D get_head_transform(Vector3 eye_offset = Vector3()); + virtual Vector3 get_eye_offset(Glasses::Eye eye); virtual Transform3D get_eye_transform(Glasses::Eye eye); virtual PackedFloat64Array get_projection_for_eye(Glasses::Eye view, double aspect, double z_near, double z_far); diff --git a/extension/src/GodotT5Service.cpp b/extension/src/GodotT5Service.cpp index 26b2334..851e615 100644 --- a/extension/src/GodotT5Service.cpp +++ b/extension/src/GodotT5Service.cpp @@ -70,11 +70,15 @@ void GodotT5Service::use_vulkan_api() { set_graphics_context(graphics_context); } -void GodotT5Math::rotate_vector(float quat_x, float quat_y, float quat_z, float quat_w, float& vec_x, float& vec_y, float& vec_z) { +void GodotT5Math::rotate_vector(float quat_x, float quat_y, float quat_z, float quat_w, float& vec_x, float& vec_y, float& vec_z, bool inverse) { godot::Quaternion orient(quat_x, quat_y, quat_z, quat_w); godot::Vector3 vec(vec_x, vec_y, vec_z); + if (inverse) { + orient = orient.inverse(); + } + vec = orient.xform(vec); vec_x = vec.x; diff --git a/extension/src/GodotT5Service.h b/extension/src/GodotT5Service.h index 4dfe558..3eeb40b 100644 --- a/extension/src/GodotT5Service.h +++ b/extension/src/GodotT5Service.h @@ -49,7 +49,7 @@ class GodotT5Service : public T5Integration::T5Service { class GodotT5Math : public T5Integration::T5Math { public: using Ptr = std::shared_ptr; - void rotate_vector(float quat_x, float quat_y, float quat_z, float quat_w, float& vec_x, float& vec_y, float& vec_z) override; + void rotate_vector(float quat_x, float quat_y, float quat_z, float quat_w, float& vec_x, float& vec_y, float& vec_z, bool inverse = false) override; }; class GodotT5Logger : public T5Integration::Logger { diff --git a/extension/src/TiltFiveXRInterface.cpp b/extension/src/TiltFiveXRInterface.cpp index 626b5b2..8ac0981 100644 --- a/extension/src/TiltFiveXRInterface.cpp +++ b/extension/src/TiltFiveXRInterface.cpp @@ -334,7 +334,12 @@ Transform3D TiltFiveXRInterface::_get_camera_transform() { auto hmd_transform = _render_glasses->get_head_transform(); - return xr_server->get_reference_frame() * hmd_transform; + // Should be the gameboard scale set in _pre_draw_viewport. + auto world_scale = xr_server->get_world_scale(); + hmd_transform.origin *= world_scale; + + // We want the transform not adjusted by the reference frame. + return hmd_transform; } Transform3D TiltFiveXRInterface::_get_transform_for_view(uint32_t view, const Transform3D &origin_transform) { @@ -347,13 +352,15 @@ Transform3D TiltFiveXRInterface::_get_transform_for_view(uint32_t view, const Tr WARN_PRINT_ONCE("Glasses not set"); return Transform3D(); } - // Should be the gameboard scale set in _pre_draw_viewport - auto world_scale = xr_server->get_world_scale(); auto eye_transform = _render_glasses->get_eye_transform(view == 0 ? Eye::Left : Eye::Right); - eye_transform.scale(Vector3(world_scale, world_scale, world_scale)); - return origin_transform * eye_transform; + // Should be the gameboard scale set in _pre_draw_viewport. + auto world_scale = xr_server->get_world_scale(); + eye_transform.origin *= world_scale; + + // Apply origin and reference frame. + return origin_transform * xr_server->get_reference_frame() * eye_transform; } PackedFloat64Array TiltFiveXRInterface::_get_projection_for_view(uint32_t p_view, double aspect, double z_near, double z_far) {