From a839513b881d031db2adc7dd3dc7c8dffb839f83 Mon Sep 17 00:00:00 2001 From: patrickdown Date: Sun, 19 May 2024 10:19:17 -0400 Subject: [PATCH] Add Tangible Tracking Camera support Added enabling the tangible camera and reading frames and pose Added the T5ImageCapture node to interface with the camera Update example.gd and example.csharp to demo new features --- example.csharp/ExampleRig.tscn | 6 +- .../tiltfive/scenes/T5ImageCaptureCS.cs | 50 ++++++ example.csharp/example.csharp.csproj.old | 8 + example.csharp/main.cs | 76 ++++++++++ example.csharp/main.tscn | 35 ++++- example.csharp/project.godot | 8 + example.gd/addons/tiltfive/scenes/T5XRRig.gd | 5 + example.gd/main.gd | 46 +++++- example.gd/main.tscn | 36 ++++- example.gd/project.godot | 8 +- example.gd/scenes/ExampleXRRig.tscn | 2 + extension/T5Integration/Camera.cpp | 143 ++++++++++++++++++ extension/T5Integration/Camera.h | 67 ++++++++ extension/T5Integration/Glasses.h | 2 + extension/T5Integration/Logging.h | 18 ++- extension/T5Integration/Wand.h | 1 + extension/src/T5ImageCapture.cpp | 95 ++++++++++++ extension/src/T5ImageCapture.h | 54 +++++++ extension/src/TiltFiveXRInterface.cpp | 12 ++ extension/src/TiltFiveXRInterface.h | 3 + extension/src/register_types.cpp | 2 + 21 files changed, 667 insertions(+), 10 deletions(-) create mode 100644 example.csharp/addons/tiltfive/scenes/T5ImageCaptureCS.cs create mode 100644 example.csharp/example.csharp.csproj.old create mode 100644 example.csharp/main.cs create mode 100644 extension/T5Integration/Camera.cpp create mode 100644 extension/T5Integration/Camera.h create mode 100644 extension/src/T5ImageCapture.cpp create mode 100644 extension/src/T5ImageCapture.h diff --git a/example.csharp/ExampleRig.tscn b/example.csharp/ExampleRig.tscn index 25ff8a4..67da1bc 100644 --- a/example.csharp/ExampleRig.tscn +++ b/example.csharp/ExampleRig.tscn @@ -1,10 +1,11 @@ -[gd_scene load_steps=8 format=3 uid="uid://ba8h6c1mtb3h0"] +[gd_scene load_steps=9 format=3 uid="uid://ba8h6c1mtb3h0"] [ext_resource type="PackedScene" uid="uid://dpbt52d0p5wjw" path="res://addons/tiltfive/scenes/T5XRRig.tscn" id="1_x7gas"] [ext_resource type="PackedScene" uid="uid://b1cd3jc00rhal" path="res://addons/tiltfive/assets/T5GlassesModel.tscn" id="2_dp1ep"] [ext_resource type="Script" path="res://WandControl.cs" id="2_epf7w"] [ext_resource type="PackedScene" uid="uid://dnx42xctfl3mx" path="res://Controls.tscn" id="2_ge6xw"] [ext_resource type="PackedScene" uid="uid://fipea8dbocg4" path="res://addons/tiltfive/assets/T5WandModel.tscn" id="5_j53ao"] +[ext_resource type="Script" path="res://addons/tiltfive/scenes/T5ImageCaptureCS.cs" id="6_mxd08"] [sub_resource type="StandardMaterial3D" id="StandardMaterial3D_tnkdi"] albedo_color = Color(0.580392, 0.396078, 0.278431, 1) @@ -28,3 +29,6 @@ selected = SubResource("StandardMaterial3D_kgxv6") transform = Transform3D(10, 0, 0, 0, 10, 0, 0, 0, 10, 0.585525, -0.00207818, 0.223126) [node name="T5-wand" parent="Origin/Wand_1" index="1" instance=ExtResource("5_j53ao")] + +[node name="T5ImageCapture" type="T5ImageCapture" parent="Origin" index="2"] +script = ExtResource("6_mxd08") diff --git a/example.csharp/addons/tiltfive/scenes/T5ImageCaptureCS.cs b/example.csharp/addons/tiltfive/scenes/T5ImageCaptureCS.cs new file mode 100644 index 0000000..ad26622 --- /dev/null +++ b/example.csharp/addons/tiltfive/scenes/T5ImageCaptureCS.cs @@ -0,0 +1,50 @@ +using Godot; +using System; + +public partial class T5ImageCaptureCS : Node3D +{ + public bool startCapture() + { + return Call("start_capture").AsBool(); + } + + public void stopCapture() + { + Call("stop_capture"); + } + + public bool acquireBuffer() + { + return Call("acquire_buffer").AsBool(); + } + + public void releaseBuffer() + { + Call("release_buffer"); + } + + public byte[] getImageData() + { + return Call("get_image_data").As(); + } + + public Transform3D getCameraTransform() + { + return Call("get_camera_transform").As(); + } + + public Vector2I getImageSize() + { + return Call("get_image_size").As(); + } + + public int getImageStride() + { + return Call("get_image_stride").As(); + } + + public int getFrameIlluminationMode() + { + return Call("get_frame_illumination_mode").As(); + } +} diff --git a/example.csharp/example.csharp.csproj.old b/example.csharp/example.csharp.csproj.old new file mode 100644 index 0000000..198256e --- /dev/null +++ b/example.csharp/example.csharp.csproj.old @@ -0,0 +1,8 @@ + + + net6.0 + net7.0 + net8.0 + true + + \ No newline at end of file diff --git a/example.csharp/main.cs b/example.csharp/main.cs new file mode 100644 index 0000000..077fdc1 --- /dev/null +++ b/example.csharp/main.cs @@ -0,0 +1,76 @@ +using Godot; +using System; + +public partial class main : Node3D +{ + TextureRect cameraView; + Image cameraImage; + ImageTexture cameraTexture; + T5ImageCaptureCS imageCapture; + bool isCapturing = false; + Vector2I currentImageSize = Vector2I.Zero; + + public override void _Ready() { + cameraView = GetNode("ScreenUI/CameraView"); + } + + public override void _Process(double delta) + { + if (imageCapture != null && isCapturing) + { + if (imageCapture.acquireBuffer()) + { + byte[] imageData = imageCapture.getImageData(); + Vector2I imageSize = imageCapture.getImageSize(); + + if(cameraImage == null || imageSize != currentImageSize) + { + cameraImage = Image.CreateFromData(imageSize.X, imageSize.Y, false, Image.Format.R8, imageData); + cameraTexture = ImageTexture.CreateFromImage(cameraImage); + cameraView.Texture = cameraTexture; + currentImageSize = imageSize; + } + else + { + cameraImage.SetData(imageSize.X, imageSize.Y, false, Image.Format.R8, imageData); + cameraTexture.Update(cameraImage); + } + imageCapture.releaseBuffer(); + } + } + } + + public override void _Input(InputEvent evt) + { + if (evt.IsActionPressed("toggle_camera") && imageCapture != null) + { + if (!isCapturing && imageCapture.startCapture()) + { + isCapturing = true; + cameraView.Visible = true; + } + else if (isCapturing) + { + imageCapture.stopCapture(); + isCapturing = false; + cameraView.Visible = false; + } + } + } + + private void _on_t_5_manager_xr_rig_was_added(SubViewport rig) + { + imageCapture = rig.GetNode("Origin/T5ImageCapture"); + } + + private void _on_t_5_manager_xr_rig_will_be_removed(SubViewport rig) + { + if(imageCapture != null && isCapturing) + { + imageCapture.stopCapture(); + isCapturing = false; + cameraView.Visible = false; + } + imageCapture = null; + } +} diff --git a/example.csharp/main.tscn b/example.csharp/main.tscn index f83e535..1f1a3b5 100644 --- a/example.csharp/main.tscn +++ b/example.csharp/main.tscn @@ -1,5 +1,6 @@ -[gd_scene load_steps=10 format=3 uid="uid://cc7yui6nxllyl"] +[gd_scene load_steps=11 format=3 uid="uid://cc7yui6nxllyl"] +[ext_resource type="Script" path="res://main.cs" id="1_55n0b"] [ext_resource type="Script" path="res://addons/tiltfive/T5Manager.cs" id="1_e8x2j"] [ext_resource type="PackedScene" uid="uid://ba8h6c1mtb3h0" path="res://ExampleRig.tscn" id="2_vyjmk"] @@ -24,6 +25,7 @@ albedo_color = Color(0.0313726, 0, 1, 1) albedo_color = Color(0.45098, 0, 1, 1) [node name="Main" type="Node3D"] +script = ExtResource("1_55n0b") [node name="T5Manager" type="Node3D" parent="." node_paths=PackedStringArray("startLocation")] script = ExtResource("1_e8x2j") @@ -72,3 +74,34 @@ transform = Transform3D(0.99099, 0.062492, 0.118462, -0.133934, 0.462383, 0.8765 [node name="SpectatorCamera" type="Camera3D" parent="."] transform = Transform3D(0.518176, -0.550674, 0.654409, -7.45058e-09, 0.765146, 0.643857, -0.855274, -0.333631, 0.396481, 8.04684, 5.20446, 5.82711) cull_mask = 3 + +[node name="ScreenUI" type="Control" parent="."] +visibility_layer = 4 +layout_mode = 3 +anchors_preset = 15 +anchor_right = 1.0 +anchor_bottom = 1.0 +grow_horizontal = 2 +grow_vertical = 2 + +[node name="CameraView" type="TextureRect" parent="ScreenUI"] +visible = false +visibility_layer = 4 +layout_mode = 1 +anchors_preset = 15 +anchor_right = 1.0 +anchor_bottom = 1.0 +grow_horizontal = 2 +grow_vertical = 2 + +[node name="Label" type="Label" parent="ScreenUI"] +visibility_layer = 4 +layout_mode = 1 +offset_left = 16.0 +offset_top = 18.0 +offset_right = 196.0 +offset_bottom = 41.0 +text = "C - Toggle Camera View" + +[connection signal="XRRigWasAdded" from="T5Manager" to="." method="_on_t_5_manager_xr_rig_was_added"] +[connection signal="XRRigWillBeRemoved" from="T5Manager" to="." method="_on_t_5_manager_xr_rig_will_be_removed"] diff --git a/example.csharp/project.godot b/example.csharp/project.godot index 6fb615f..944b5c8 100644 --- a/example.csharp/project.godot +++ b/example.csharp/project.godot @@ -27,6 +27,14 @@ project/assembly_name="example.csharp" enabled=PackedStringArray("res://addons/tiltfive/plugin.cfg") +[input] + +toggle_camera={ +"deadzone": 0.5, +"events": [Object(InputEventKey,"resource_local_to_scene":false,"resource_name":"","device":-1,"window_id":0,"alt_pressed":false,"shift_pressed":false,"ctrl_pressed":false,"meta_pressed":false,"pressed":false,"keycode":0,"physical_keycode":67,"key_label":0,"unicode":99,"echo":false,"script":null) +] +} + [xr] shaders/enabled=true diff --git a/example.gd/addons/tiltfive/scenes/T5XRRig.gd b/example.gd/addons/tiltfive/scenes/T5XRRig.gd index b1a4c9e..8ea80f0 100644 --- a/example.gd/addons/tiltfive/scenes/T5XRRig.gd +++ b/example.gd/addons/tiltfive/scenes/T5XRRig.gd @@ -7,6 +7,7 @@ var _gameboard_size := AABB() var _origin : T5Origin3D var _camera : T5Camera3D var _wand : T5Controller3D +var _image_capture : T5ImageCapture ## Get the ID attached to a pair of Tilt Five glasses func get_glasses_id() -> StringName: @@ -32,10 +33,14 @@ func get_camera() -> T5Camera3D: func get_wand() -> T5Controller3D: return _wand +func get_image_capture() -> T5ImageCapture: + return _image_capture + func _enter_tree(): _origin = $Origin _camera = $Origin/Camera _wand = $Origin/Wand_1 + _image_capture = $Origin.get_node("T5ImageCapture") func _process(_delta): if _wand: _wand.visible = _wand.get_has_tracking_data() diff --git a/example.gd/main.gd b/example.gd/main.gd index 880e6ec..ad2a8ce 100644 --- a/example.gd/main.gd +++ b/example.gd/main.gd @@ -1,7 +1,45 @@ extends Node3D -func _on_t5_manager_glasses_scene_was_added(glasses): - print("Scene ", glasses.name, " added") +var image_capture: T5ImageCapture +var is_capturing := false +var image_size: Vector2i +var camera_image: Image +@onready var camera_view: TextureRect = $ScreenUI/CameraView -func _on_t5_manager_glasses_scene_will_be_removed(glasses): - print("Scene ", glasses.name, " removed") +func _on_t_5_manager_xr_rig_was_added(xr_rig: T5XRRig): + image_capture = xr_rig.get_image_capture() + +func _on_t_5_manager_xr_rig_will_be_removed(_xr_rig): + image_capture.stop_capture() + is_capturing = false + camera_view.visible = false + image_capture = null + +func _input(event): + if image_capture == null: + return + if not event.is_action_pressed("toggle_camera"): + return + if not is_capturing and image_capture.start_capture(): + is_capturing = true + camera_view.visible = true + else: + image_capture.stop_capture() + is_capturing = false + camera_view.visible = false + +func _process(_delta): + if not is_capturing: + return + if image_capture.acquire_buffer(): + var buffer := image_capture.get_image_data() + var new_size = image_capture.get_image_size() + var new_illumination_mode = image_capture.get_frame_illumination_mode() + if camera_image == null or image_size != new_size: + image_size = new_size + camera_image = Image.create_from_data(image_size.x, image_size.y, false, Image.FORMAT_R8, buffer) + camera_view.texture = ImageTexture.create_from_image(camera_image) + else: + camera_image.set_data(image_size.x, image_size.y, false, Image.FORMAT_R8, buffer) + camera_view.texture.update(camera_image) + image_capture.release_buffer() diff --git a/example.gd/main.tscn b/example.gd/main.tscn index 50512fe..91bc4ef 100644 --- a/example.gd/main.tscn +++ b/example.gd/main.tscn @@ -1,4 +1,4 @@ -[gd_scene load_steps=17 format=3 uid="uid://ckbe6draoen0x"] +[gd_scene load_steps=18 format=3 uid="uid://ckbe6draoen0x"] [ext_resource type="Script" path="res://main.gd" id="1_xvgge"] [ext_resource type="Script" path="res://addons/tiltfive/T5Manager.gd" id="2_dibvp"] @@ -34,6 +34,8 @@ albedo_color = Color(0.862745, 0, 0.0235294, 1) [sub_resource type="StandardMaterial3D" id="StandardMaterial3D_qrhlq"] albedo_color = Color(0.741176, 0, 0.686275, 1) +[sub_resource type="ImageTexture" id="ImageTexture_0jitc"] + [node name="Main" type="Node3D"] script = ExtResource("1_xvgge") @@ -119,3 +121,35 @@ surface_material_override/0 = SubResource("StandardMaterial3D_qrhlq") [node name="SpectatorCam" type="Camera3D" parent="."] transform = Transform3D(0.670983, -0.138786, 0.728368, 0, 0.982326, 0.187176, -0.741472, -0.125592, 0.659125, 14.0459, 4.9572, 12.9908) cull_mask = 3 + +[node name="ScreenUI" type="Control" parent="."] +visibility_layer = 2 +layout_mode = 3 +anchors_preset = 15 +anchor_right = 1.0 +anchor_bottom = 1.0 +grow_horizontal = 2 +grow_vertical = 2 + +[node name="CameraView" type="TextureRect" parent="ScreenUI"] +visible = false +visibility_layer = 2 +layout_mode = 1 +anchors_preset = 15 +anchor_right = 1.0 +anchor_bottom = 1.0 +grow_horizontal = 2 +grow_vertical = 2 +texture = SubResource("ImageTexture_0jitc") + +[node name="Label" type="Label" parent="ScreenUI"] +visibility_layer = 4 +layout_mode = 1 +offset_left = 17.0 +offset_top = 14.0 +offset_right = 197.0 +offset_bottom = 37.0 +text = "C - Toggle Camera View" + +[connection signal="xr_rig_was_added" from="T5Manager" to="." method="_on_t_5_manager_xr_rig_was_added"] +[connection signal="xr_rig_will_be_removed" from="T5Manager" to="." method="_on_t_5_manager_xr_rig_will_be_removed"] diff --git a/example.gd/project.godot b/example.gd/project.godot index 81e86cc..884f932 100644 --- a/example.gd/project.godot +++ b/example.gd/project.godot @@ -12,7 +12,7 @@ config_version=5 config/name="T5Example.gd" run/main_scene="res://main.tscn" -config/features=PackedStringArray("4.1") +config/features=PackedStringArray("4.2") run/max_fps=60 config/icon="res://icon.png" @@ -31,6 +31,11 @@ trigger={ "events": [Object(InputEventJoypadMotion,"resource_local_to_scene":false,"resource_name":"","device":-1,"axis":5,"axis_value":1.0,"script":null) ] } +toggle_camera={ +"deadzone": 0.5, +"events": [Object(InputEventKey,"resource_local_to_scene":false,"resource_name":"","device":-1,"window_id":0,"alt_pressed":false,"shift_pressed":false,"ctrl_pressed":false,"meta_pressed":false,"pressed":false,"keycode":0,"physical_keycode":67,"key_label":0,"unicode":99,"echo":false,"script":null) +] +} [layer_names] @@ -41,3 +46,4 @@ trigger={ [xr] shaders/enabled=true +tilt_five/debug_logging=true diff --git a/example.gd/scenes/ExampleXRRig.tscn b/example.gd/scenes/ExampleXRRig.tscn index b55cb6a..d3ccca1 100644 --- a/example.gd/scenes/ExampleXRRig.tscn +++ b/example.gd/scenes/ExampleXRRig.tscn @@ -80,5 +80,7 @@ transform = Transform3D(1, 0, 0, 0, 1, 0, 0, 0, 1, 0, -2.77558, 0) mesh = SubResource("BoxMesh_aaxuw") surface_material_override/0 = SubResource("StandardMaterial3D_iuako") +[node name="T5ImageCapture" type="T5ImageCapture" parent="Origin" index="3"] + [connection signal="button_pressed" from="Origin/Wand_1" to="Origin/Wand_1" method="_on_button_pressed"] [connection signal="button_released" from="Origin/Wand_1" to="Origin/Wand_1" method="_on_button_released"] diff --git a/extension/T5Integration/Camera.cpp b/extension/T5Integration/Camera.cpp new file mode 100644 index 0000000..a1955d3 --- /dev/null +++ b/extension/T5Integration/Camera.cpp @@ -0,0 +1,143 @@ +#include +#include +#include + +namespace T5Integration { + +extern std::mutex g_t5_exclusivity_group_1; + +bool Camera::start_capture() { + LOG_FAIL_COND_V(_glasses.expired(), false); + _is_captured = configure_camera(true); + + // Put the buffers into the queue for the first time.a + if (_is_captured) { + for (int i = 0; i < camera_buffer_count; ++i) { + release_filled_buffer(i); + } + } + return _is_captured; +} + +void Camera::stop_capture() { + LOG_FAIL_COND(_glasses.expired()); + configure_camera(false); +} + +void Camera::release_filled_buffer(int buffer_index) { + LOG_FAIL_COND(_glasses.expired()); + LOG_FAIL_COND(!_is_captured); + LOG_FAIL_COND(buffer_index < 0 || buffer_index >= camera_buffer_count); + + auto& buffer = _camera_buffers[buffer_index]; + auto& camImage = _camera_buffer_info[buffer_index]; + + camImage.cameraIndex = 0; + camImage.imageWidth = 0; + camImage.imageHeight = 0; + camImage.cameraIndex = 0; + camImage.imageStride = 0; + camImage.illuminationMode = 0; + camImage.bufferSize = buffer.size(); + camImage.pixelData = buffer.data(); + + T5_Result result; + { + std::lock_guard lock(g_t5_exclusivity_group_1); + result = t5SubmitEmptyCamImageBuffer(_glasses.lock()->_glasses_handle, &camImage); + } + if (result != T5_SUCCESS) { + LOG_T5_ERROR(result); + } +} + +int Camera::acquire_filled_buffer() { + LOG_FAIL_COND_V(_glasses.expired(), -1); + T5_CamImage camImage; + T5_Result result; + { + std::lock_guard lock(g_t5_exclusivity_group_1); + result = t5GetFilledCamImageBuffer(_glasses.lock()->_glasses_handle, &camImage); + } + if (result == T5_ERROR_TRY_AGAIN) { + return -1; + } else if (result != T5_SUCCESS) { + LOG_T5_ERROR(result); + return -1; + } + for (int i = 0; i < _camera_buffers.size(); ++i) { + if (_camera_buffers[i].data() == camImage.pixelData) { + _camera_buffer_info[i] = camImage; + return i; + } + } + LOG_ERROR("Failed to find buffer."); + return -1; +} + +std::span Camera::get_buffer(int buffer_index) { + LOG_FAIL_COND_V(buffer_index < 0 || buffer_index >= _camera_buffers.size(), std::span()); + + return std::span(_camera_buffers[buffer_index]); +} + +int Camera::get_image_width(int buffer_index) const { + LOG_FAIL_COND_V(buffer_index < 0 || buffer_index >= _camera_buffer_info.size(), 0); + + return _camera_buffer_info[buffer_index].imageWidth; +} + +int Camera::get_image_height(int buffer_index) const { + LOG_FAIL_COND_V(buffer_index < 0 || buffer_index >= _camera_buffer_info.size(), 0); + + return _camera_buffer_info[buffer_index].imageHeight; +} + +int Camera::get_image_stride(int buffer_index) const { + LOG_FAIL_COND_V(buffer_index < 0 || buffer_index >= _camera_buffer_info.size(), 0); + + return _camera_buffer_info[buffer_index].imageStride; +} + +uint8_t Camera::get_illumination_mode(int buffer_index) const { + LOG_FAIL_COND_V(buffer_index < 0 || buffer_index >= _camera_buffer_info.size(), false); + + return _camera_buffer_info[buffer_index].illuminationMode; +} + +void Camera::get_camera_position(int buffer_index, float& out_pos_x, float& out_pos_y, float& out_pos_z) const { + LOG_FAIL_COND(buffer_index < 0 || buffer_index >= _camera_buffer_info.size()); + + out_pos_x = _camera_buffer_info[buffer_index].posCAM_GBD.x; + out_pos_y = _camera_buffer_info[buffer_index].posCAM_GBD.y; + out_pos_z = _camera_buffer_info[buffer_index].posCAM_GBD.z; +} + +void Camera::get_camera_orientation(int buffer_index, float& out_quat_x, float& out_quat_y, float& out_quat_z, float& out_quat_w) const { + LOG_FAIL_COND(buffer_index < 0 || buffer_index >= _camera_buffer_info.size()); + + out_quat_x = _camera_buffer_info[buffer_index].rotToCAM_GBD.x; + out_quat_y = _camera_buffer_info[buffer_index].rotToCAM_GBD.y; + out_quat_z = _camera_buffer_info[buffer_index].rotToCAM_GBD.z; + out_quat_w = _camera_buffer_info[buffer_index].rotToCAM_GBD.w; +} + +bool Camera::configure_camera(bool enable) { + LOG_FAIL_COND_V(_glasses.expired(), false); + LOG_FAIL_COND_V(_camera_buffers.size() <= 0, false); + + T5_CameraStreamConfig config{ _camera_idx, enable }; + T5_Result result = T5_SUCCESS; + { + std::lock_guard lock(g_t5_exclusivity_group_1); + result = t5ConfigureCameraStreamForGlasses(_glasses.lock()->_glasses_handle, config); + } + + if (result != T5_SUCCESS) { + LOG_T5_ERROR(result); + return false; + } + return true; +} + +} //namespace T5Integration diff --git a/extension/T5Integration/Camera.h b/extension/T5Integration/Camera.h new file mode 100644 index 0000000..8d8c7e0 --- /dev/null +++ b/extension/T5Integration/Camera.h @@ -0,0 +1,67 @@ +#ifndef _WANDSERVICE_H +#define _WANDSERVICE_H + +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std::chrono_literals; + +namespace T5Integration { + +const int camera_buffer_count = 3; +const size_t camera_buffer_size = T5_MIN_CAM_IMAGE_BUFFER_WIDTH * T5_MIN_CAM_IMAGE_BUFFER_HEIGHT; + +using CameraBuffer = std::array; + +class Camera { +public: + void set_camera_idx(uint8_t idx); + void set_glasses(Glasses::Ptr glasses); + + bool start_capture(); + void stop_capture(); + bool is_capturing() const; + + void release_filled_buffer(int buffer_index); + int acquire_filled_buffer(); + std::span get_buffer(int buffer_index); + int get_image_width(int buffer_index) const; + int get_image_height(int buffer_index) const; + int get_image_stride(int buffer_index) const; + uint8_t get_illumination_mode(int buffer_index) const; + void get_camera_position(int buffer_index, float& out_pos_x, float& out_pos_y, float& out_pos_z) const; + void get_camera_orientation(int buffer_index, float& out_quat_x, float& out_quat_y, float& out_quat_z, float& out_quat_w) const; + +private: + bool configure_camera(bool enable); + + bool _is_captured = false; + uint8_t _camera_idx = 0; + std::weak_ptr _glasses; + + std::array _camera_buffers; + std::array _camera_buffer_info; +}; + +inline void Camera::set_camera_idx(uint8_t idx) { + _camera_idx = idx; +} + +inline bool Camera::is_capturing() const { + return _is_captured; +} + +inline void Camera::set_glasses(Glasses::Ptr glasses) { + _glasses = glasses; +} + +} //namespace T5Integration + +#endif //_WANDSERVICE_H \ No newline at end of file diff --git a/extension/T5Integration/Glasses.h b/extension/T5Integration/Glasses.h index 90968d9..6cbe9f0 100644 --- a/extension/T5Integration/Glasses.h +++ b/extension/T5Integration/Glasses.h @@ -11,6 +11,7 @@ namespace T5Integration { using namespace std::chrono_literals; using GlassesFlags = StateFlags; class T5Service; +class Camera; using TaskSystem::CotaskPtr; using TaskSystem::Scheduler; @@ -58,6 +59,7 @@ struct GlassesEvent { class Glasses { friend T5Service; + friend Camera; protected: struct SwapChainFrame { diff --git a/extension/T5Integration/Logging.h b/extension/T5Integration/Logging.h index 3e22072..643d0d5 100644 --- a/extension/T5Integration/Logging.h +++ b/extension/T5Integration/Logging.h @@ -61,7 +61,7 @@ void log_message(T var1, Types... var2) { #define LOG_CHECK_POINT_ONCE \ { \ static bool once##__LINE__ = false; \ - if (!once##__LINE__) { \ + if (!once##__LINE__) [[unlikely]] { \ LOG_CHECK_POINT once##__LINE__ = true; \ } \ } @@ -95,9 +95,23 @@ void log_message(T var1, Types... var2) { #define LOG_ERROR_ONCE(MSG) \ { \ static bool once##__LINE__ = false; \ - if (!once##__LINE__) { \ + if (!once##__LINE__) [[unlikely]] { \ LOG_ERROR(MSG); \ once##__LINE__ = true; \ } \ } #endif + +#define LOG_FAIL_COND(m_cond) \ + if (m_cond) [[unlikely]] { \ + T5Integration::log_error("Condition \"" #m_cond "\" is true.", __func__, __FILE__, __LINE__); \ + return; \ + } else \ + ((void)0) + +#define LOG_FAIL_COND_V(m_cond, m_retval) \ + if (m_cond) [[unlikely]] { \ + T5Integration::log_error("Condition \"" #m_cond "\" is true. Returning: " #m_retval, __func__, __FILE__, __LINE__); \ + return m_retval; \ + } else \ + ((void)0) diff --git a/extension/T5Integration/Wand.h b/extension/T5Integration/Wand.h index dc59afd..6a4b716 100644 --- a/extension/T5Integration/Wand.h +++ b/extension/T5Integration/Wand.h @@ -1,6 +1,7 @@ #pragma once #include #include +#include #include #include diff --git a/extension/src/T5ImageCapture.cpp b/extension/src/T5ImageCapture.cpp new file mode 100644 index 0000000..87766b4 --- /dev/null +++ b/extension/src/T5ImageCapture.cpp @@ -0,0 +1,95 @@ +#include +#include +#include + +using godot::ClassDB; +using godot::D_METHOD; +using godot::MethodInfo; +using godot::Quaternion; + +void T5ImageCapture::_bind_methods() { + ClassDB::bind_method(D_METHOD("start_capture"), &T5ImageCapture::start_capture); + ClassDB::bind_method(D_METHOD("stop_capture"), &T5ImageCapture::stop_capture); + ClassDB::bind_method(D_METHOD("acquire_buffer"), &T5ImageCapture::acquire_buffer); + ClassDB::bind_method(D_METHOD("release_buffer"), &T5ImageCapture::release_buffer); + ClassDB::bind_method(D_METHOD("get_image_data"), &T5ImageCapture::get_image_data); + ClassDB::bind_method(D_METHOD("get_camera_transform"), &T5ImageCapture::get_camera_transform); + ClassDB::bind_method(D_METHOD("get_image_size"), &T5ImageCapture::get_image_size); + ClassDB::bind_method(D_METHOD("get_image_stride"), &T5ImageCapture::get_image_stride); + ClassDB::bind_method(D_METHOD("get_frame_illumination_mode"), &T5ImageCapture::get_frame_illumination_mode); +}; + +bool T5ImageCapture::start_capture() { + _camera.set_camera_idx(0); + return _camera.start_capture(); +} + +void T5ImageCapture::stop_capture() { + _camera.stop_capture(); +} + +bool T5ImageCapture::acquire_buffer() { + ERR_FAIL_COND_V(!_camera.is_capturing(), false); + + _acquired_buffer_idx = _camera.acquire_filled_buffer(); + if (_acquired_buffer_idx >= 0) { + set_transform(get_camera_transform()); + } + + return _acquired_buffer_idx >= 0 && _acquired_buffer_idx < 3; +} + +void T5ImageCapture::release_buffer() { + if (_acquired_buffer_idx >= 0) { + _camera.release_filled_buffer(_acquired_buffer_idx); + _acquired_buffer_idx = -1; + } +} + +PackedByteArray T5ImageCapture::get_image_data() { + ERR_FAIL_INDEX_V(_acquired_buffer_idx, 3, PackedByteArray()); + + auto buffer = _camera.get_buffer(_acquired_buffer_idx); + PackedByteArray packed_data; + packed_data.resize(buffer.size()); + memcpy(packed_data.ptrw(), buffer.data(), buffer.size()); + + return packed_data; +} + +Transform3D T5ImageCapture::get_camera_transform() { + ERR_FAIL_INDEX_V(_acquired_buffer_idx, 3, Transform3D()); + + Quaternion orientation; + Vector3 position; + _camera.get_camera_position(_acquired_buffer_idx, position.x, position.y, position.z); + _camera.get_camera_orientation(_acquired_buffer_idx, orientation.x, orientation.y, orientation.z, orientation.w); + + // 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 local_transform; + local_transform.set_origin(position); + local_transform.set_basis(orientation); + + return local_transform; +} + +Vector2i T5ImageCapture::get_image_size() const { + ERR_FAIL_INDEX_V(_acquired_buffer_idx, 3, Vector2i()); + auto width = _camera.get_image_width(_acquired_buffer_idx); + auto height = _camera.get_image_height(_acquired_buffer_idx); + return Vector2i(width, height); +} + +int T5ImageCapture::get_image_stride() const { + ERR_FAIL_INDEX_V(_acquired_buffer_idx, 3, 0); + return _camera.get_image_stride(_acquired_buffer_idx); +} + +int T5ImageCapture::get_frame_illumination_mode() const { + ERR_FAIL_INDEX_V(_acquired_buffer_idx, 3, false); + return _camera.get_illumination_mode(_acquired_buffer_idx); +} diff --git a/extension/src/T5ImageCapture.h b/extension/src/T5ImageCapture.h new file mode 100644 index 0000000..79d8765 --- /dev/null +++ b/extension/src/T5ImageCapture.h @@ -0,0 +1,54 @@ +#ifndef T5_IMAGE_CAPTURE_H +#define T5_IMAGE_CAPTURE_H + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using godot::Image; +using godot::Node3D; +using godot::PackedByteArray; +using godot::Ref; +using godot::Texture2D; +using godot::Vector2i; +using godot::XRPose; +using GodotT5Integration::GodotT5Glasses; +using T5Integration::Camera; + +class T5ImageCapture : public Node3D { + GDCLASS(T5ImageCapture, Node3D); + +public: + void set_glasses(GodotT5Glasses::Ptr glasses); + + bool start_capture(); + void stop_capture(); + + bool acquire_buffer(); + void release_buffer(); + + PackedByteArray get_image_data(); + Transform3D get_camera_transform(); + Vector2i get_image_size() const; + int get_image_stride() const; + int get_frame_illumination_mode() const; + +protected: + static void _bind_methods(); + + int _acquired_buffer_idx = -1; + Camera _camera; +}; + +inline void T5ImageCapture::set_glasses(GodotT5Glasses::Ptr glasses) { + _camera.set_glasses(glasses); +} + +#endif // T5_IMAGE_CAPTURE_H \ No newline at end of file diff --git a/extension/src/TiltFiveXRInterface.cpp b/extension/src/TiltFiveXRInterface.cpp index f40d36b..0950ecf 100644 --- a/extension/src/TiltFiveXRInterface.cpp +++ b/extension/src/TiltFiveXRInterface.cpp @@ -1,4 +1,5 @@ #include "TiltFiveXRInterface.h" +#include #include #include #include @@ -226,10 +227,21 @@ void TiltFiveXRInterface::_start_display(TiltFiveXRInterface::GlassesIndexEntry& entry.viewport_id = viewport->get_instance_id(); entry.gameboard_id = gameboard->get_instance_id(); + _setup_scene_nodes(glasses, gameboard); + viewport->set_use_xr(true); viewport->set_update_mode(godot::SubViewport::UpdateMode::UPDATE_ALWAYS); } +void TiltFiveXRInterface::_setup_scene_nodes(GodotT5Glasses::Ptr glasses, Node* node) { + for (int i = 0; i < node->get_child_count(); i++) { + auto image_capture = Object::cast_to(node->get_child(i)); + if (image_capture) { + image_capture->set_glasses(glasses); + } + } +} + void TiltFiveXRInterface::stop_display(const StringName glasses_id) { auto entry = lookup_glasses_entry(glasses_id); ERR_FAIL_COND_MSG(!entry, "Glasses id was not found"); diff --git a/extension/src/TiltFiveXRInterface.h b/extension/src/TiltFiveXRInterface.h index 98b9ef8..526d379 100644 --- a/extension/src/TiltFiveXRInterface.h +++ b/extension/src/TiltFiveXRInterface.h @@ -13,6 +13,7 @@ #include using godot::AABB; +using godot::Node; using godot::ObjectID; using godot::PackedFloat64Array; using godot::PackedStringArray; @@ -151,6 +152,8 @@ class TiltFiveXRInterface : public XRInterfaceExtension { void _start_display(GlassesIndexEntry &entry, SubViewport *viewport, T5Origin3D *xr_origin); void _stop_display(GlassesIndexEntry &entry); + void _setup_scene_nodes(GodotT5Glasses::Ptr glasses, Node *node); + GlassesIndexEntry *lookup_glasses_entry(StringName glasses_id); GlassesIndexEntry *lookup_glasses_by_render_target(RID render_target); GlassesIndexEntry *lookup_glasses_by_viewport(RID render_target); diff --git a/extension/src/register_types.cpp b/extension/src/register_types.cpp index 28bc9d8..e25e758 100644 --- a/extension/src/register_types.cpp +++ b/extension/src/register_types.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include #include @@ -19,6 +20,7 @@ void initialize_tiltfive_types(ModuleInitializationLevel p_level) { ClassDB::register_class(); ClassDB::register_class(); ClassDB::register_class(); + ClassDB::register_class(); ClassDB::register_class(); ClassDB::register_class(); ClassDB::register_class();