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..0c629b8 --- /dev/null +++ b/example.csharp/main.cs @@ -0,0 +1,82 @@ +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) + { + var rigImageCapture = rig.GetNode("Origin/T5ImageCapture"); + if(imageCapture == null) { + imageCapture = rigImageCapture; + } + } + + private void _on_t_5_manager_xr_rig_will_be_removed(SubViewport rig) + { + var rigImageCapture = rig.GetNode("Origin/T5ImageCapture"); + if(imageCapture != null && imageCapture == rigImageCapture) { + if(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..32bf403 100644 --- a/example.gd/main.gd +++ b/example.gd/main.gd @@ -1,7 +1,55 @@ 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") +# Grab the T5ImageCapture out of the xr rig +func _on_t_5_manager_xr_rig_was_added(xr_rig: T5XRRig): + var new_image_capture = xr_rig.get_image_capture() + if not image_capture and new_image_capture: + image_capture = new_image_capture + +func _on_t_5_manager_xr_rig_will_be_removed(xr_rig): + var old_image_capture = xr_rig.get_image_capture() + if image_capture and image_capture == old_image_capture: + if is_capturing: + image_capture.stop_capture() + is_capturing = false + camera_view.visible = false + image_capture = null + +# Toggle the camera on and off +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 + # Test to see if a image buffer is available + if image_capture.acquire_buffer(): + # get the image data + var buffer := image_capture.get_image_data() + # Get the size + var new_size = image_capture.get_image_size() + # If is the first time of the size has changed + 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..dbca591 --- /dev/null +++ b/extension/T5Integration/Camera.h @@ -0,0 +1,67 @@ +#ifndef _CAMERA_H +#define _CAMERA_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 //_CAMERA_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();