Merge pull request #49 from BastiaanOlij/fix_projection

Fix stereo projection

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")

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;

extension/T5Integration/T5Math.h

@@ -6,7 +6,7 @@ namespace T5Integration {
 		public:
 		using Ptr = std::shared_ptr<T5Math>;
 
-		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;
 	};
 
 }

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<XRPositionalTracker> positional_tracker;
-    positional_tracker.instantiate();
+	Ref<XRPositionalTracker> 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

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);