Add undistortion to viewer

demo/main.tscn

@@ -1,13 +1,15 @@
-[gd_scene load_steps=98 format=3 uid="uid://2vmsr5t51l5c"]
+[gd_scene load_steps=100 format=3 uid="uid://2vmsr5t51l5c"]
 
 [ext_resource type="Script" path="res://scripts/Visualiser.gd" id="1_i4o6k"]
 [ext_resource type="Script" path="res://scripts/Starter.gd" id="1_pwqvd"]
 [ext_resource type="PackedScene" uid="uid://uaaq7yd773go" path="res://scenes/HUD.tscn" id="3_2i584"]
 [ext_resource type="Texture2D" uid="uid://64qjvjymo7oj" path="res://textures/LeftImage.tres" id="3_48g1p"]
-[ext_resource type="Sky" uid="uid://drk5ocwpf0ggn" path="res://materials/SkyImages.tres" id="3_il4lh"]
+[ext_resource type="Sky" uid="uid://dlont1iyrrl3g" path="res://materials/SkyImages.tres" id="3_il4lh"]
 [ext_resource type="Texture2D" uid="uid://t5jb84gwhfx4" path="res://textures/RightImage.tres" id="4_antfi"]
 [ext_resource type="Script" path="res://scripts/ImagePuller.gd" id="4_sgil3"]
+[ext_resource type="Texture2D" uid="uid://7h3wom5btmhp" path="res://textures/LeftDistortion.tres" id="7_vascd"]
 [ext_resource type="Script" path="res://scripts/DebugHands.gd" id="8_uah46"]
+[ext_resource type="Texture2D" uid="uid://dwx41gu1dkwgt" path="res://textures/RightDistortion.tres" id="9_douml"]
 
 [sub_resource type="Environment" id="Environment_3k2hr"]
 background_mode = 2
@@ -461,6 +463,7 @@ script = ExtResource("4_sgil3")
 tracker = NodePath("../../../UltraleapHandTracking")
 img = SubResource("Image_yrupp")
 data_img = SubResource("Image_gfwfe")
+distortion_texture = ExtResource("7_vascd")
 
 [node name="TextureRect2" type="TextureRect" parent="CanvasLayer/HBoxContainer" node_paths=PackedStringArray("tracker")]
 layout_mode = 2
@@ -472,6 +475,7 @@ side = 1
 tracker = NodePath("../../../UltraleapHandTracking")
 img = SubResource("Image_oyr74")
 data_img = SubResource("Image_gt7mg")
+distortion_texture = ExtResource("9_douml")
 
 [node name="Hands" type="Node3D" parent="."]
 script = ExtResource("8_uah46")
@@ -654,6 +658,21 @@ mesh = SubResource("CapsuleMesh_dnlrb")
 transform = Transform3D(0.999972, 0.00737919, 0.00080738, -0.00737915, 0.976307, 0.216265, 0.000807624, -0.216265, 0.976334, -5.24223e-05, -0.0163794, 0.00153229)
 mesh = SubResource("CapsuleMesh_mtm4i")
 
+[node name="CanvasLayer2" type="CanvasLayer" parent="."]
+visible = false
+
+[node name="TextureRect" type="TextureRect" parent="CanvasLayer2"]
+offset_right = 438.0
+offset_bottom = 330.0
+texture = ExtResource("7_vascd")
+
+[node name="TextureRect2" type="TextureRect" parent="CanvasLayer2"]
+offset_left = 673.0
+offset_top = 12.0
+offset_right = 1111.0
+offset_bottom = 342.0
+texture = ExtResource("9_douml")
+
 [connection signal="device_added" from="UltraleapHandTracking" to="." method="_on_hand_tracking_device_added"]
 [connection signal="device_added" from="UltraleapHandTracking" to="HUD" method="on_device_added"]
 [connection signal="device_removed" from="UltraleapHandTracking" to="." method="_on_hand_tracking_device_removed"]

demo/materials/SkyImages.tres

@@ -1,15 +1,20 @@
-[gd_resource type="Sky" load_steps=5 format=3 uid="uid://drk5ocwpf0ggn"]
+[gd_resource type="Sky" load_steps=7 format=3 uid="uid://dlont1iyrrl3g"]
 
 [ext_resource type="Shader" path="res://shaders/SkyImages.gdshader" id="1_biwbg"]
 [ext_resource type="Texture2D" uid="uid://64qjvjymo7oj" path="res://textures/LeftImage.tres" id="2_uqhtk"]
+[ext_resource type="Texture2D" uid="uid://7h3wom5btmhp" path="res://textures/LeftDistortion.tres" id="3_11h6l"]
 [ext_resource type="Texture2D" uid="uid://t5jb84gwhfx4" path="res://textures/RightImage.tres" id="3_kniuh"]
+[ext_resource type="Texture2D" uid="uid://dwx41gu1dkwgt" path="res://textures/RightDistortion.tres" id="5_fpw2i"]
 
 [sub_resource type="ShaderMaterial" id="ShaderMaterial_7aiux"]
 shader = ExtResource("1_biwbg")
+shader_parameter/undistort = true
 shader_parameter/default_colour = Color(0, 0, 0, 1)
 shader_parameter/gammaExponent = 1.6
 shader_parameter/left = ExtResource("2_uqhtk")
 shader_parameter/right = ExtResource("3_kniuh")
+shader_parameter/left_distortion = ExtResource("3_11h6l")
+shader_parameter/right_distortion = ExtResource("5_fpw2i")
 
 [resource]
 sky_material = SubResource("ShaderMaterial_7aiux")

demo/scripts/ImagePuller.gd

@@ -7,11 +7,14 @@ enum Side { Left, Right }
 @export var tracker : UltraleapHandTracking
 @export var img : Image
 @export var data_img : Image
+@export var distortion_texture : Texture2D
 
 @onready var bg_data : PackedByteArray = PackedByteArray()
 @onready var data : PackedByteArray
+@onready var distortion_matrix : PackedVector2Array
 
 var last_size : Vector2i = Vector2i(0, 0)
+var last_matrix_version = 999
 
 var device_index : int = 0
 	
@@ -48,6 +51,12 @@ func _process(_delta):
 		flip_h = tracker.tracking_mode == UltraleapTypes.TrackingMode.Desktop
 
 		data = image.get_data()
+		if image.get_matrix_version() != last_matrix_version:
+			last_matrix_version = image.get_matrix_version()
+			distortion_matrix = image.get_distortion_matrix()
+			var distortion_img : Image = Image.new()
+			distortion_img.set_data(64, 64, false, Image.FORMAT_RGF, distortion_matrix.to_byte_array())
+			distortion_texture.set_image(distortion_img)
 
 		if data.size() == 384 * 384:
 			data_img.set_data(384, 384, false, Image.FORMAT_L8, data)

demo/shaders/SkyImages.gdshader

@@ -3,10 +3,34 @@ shader_type sky;
 uniform sampler2D left : hint_default_black;
 uniform sampler2D right : hint_default_black;
 
+uniform sampler2D left_distortion : hint_default_black;
+uniform sampler2D right_distortion : hint_default_black;
+
+uniform bool undistort = true;
+
 uniform vec3 default_colour : source_color;
 
 uniform float gammaExponent = 1.0;
 
+vec3 get_pixel(sampler2D left_texture, sampler2D right_texture, sampler2D left_disto, sampler2D right_disto, float is_left_side, vec2 uv, bool use_undistortion) {
+	if (undistort) {
+		vec2 distortionIndex = is_left_side * texture(left_disto, vec2(uv.x * 2.0f, 1.0 - uv.y)).xy
+			 + (1.0 - is_left_side) * texture(right_disto, vec2(uv.x * 2.0f, 1.0 - uv.y)).xy;
+		float hIndex = distortionIndex.r;
+		float vIndex = distortionIndex.g;
+		
+		if(vIndex > 0.0 && vIndex < 1.0 && hIndex > 0.0 && hIndex < 1.0) {
+			return is_left_side * texture(left_texture, distortionIndex).rrr
+				+ (1.0 - is_left_side) * texture(right_texture, distortionIndex).rrr;
+		}
+		else {
+			return vec3(0.0, 0.0, 0.0);
+		}
+	}
+	return is_left_side * texture(left_texture, vec2(uv.x * 2.0f, uv.y)).xyz
+		+ (1.0 - is_left_side) * texture(right_texture, vec2(uv.x * 2.0f, uv.y)).xyz;
+}
+
 void sky() {
 	ivec2 left_texture_size = textureSize(left, 0);
 	ivec2 right_texture_size = textureSize(right, 0);
@@ -22,8 +46,10 @@ void sky() {
 	
 	float is_left_side = step(SCREEN_UV.x, 0.5);
 	
-	vec3 pix = is_left_side * texture(left, vec2(SCREEN_UV.x * 2.0f, SCREEN_UV.y)).xyz
-		+ (1.0 - is_left_side) * texture(right, vec2(SCREEN_UV.x * 2.0f, SCREEN_UV.y)).xyz;
+	vec3 pix = get_pixel(left, right, left_distortion, right_distortion, is_left_side, SCREEN_UV, undistort);
+	
+	//vec3 pix = is_left_side * texture(left, vec2(SCREEN_UV.x * 2.0f, SCREEN_UV.y)).xyz
+	//	+ (1.0 - is_left_side) * texture(right, vec2(SCREEN_UV.x * 2.0f, SCREEN_UV.y)).xyz;
 		
 	float text_is_valid = is_left_side * left_text_is_valid + (1.0 - is_left_side) * right_text_is_valid;
 	

demo/textures/LeftDistortion.tres

@@ -0,0 +1,3 @@
+[gd_resource type="ImageTexture" format=3 uid="uid://7h3wom5btmhp"]
+
+[resource]

demo/textures/RightDistortion.tres

@@ -0,0 +1,3 @@
+[gd_resource type="ImageTexture" format=3 uid="uid://dwx41gu1dkwgt"]
+
+[resource]

src/image.cpp

@@ -10,6 +10,8 @@ using namespace godot;
 
 void UltraleapImage::_bind_methods() {
     ClassDB::bind_method(D_METHOD("get_data"), &UltraleapImage::get_data);
+    ClassDB::bind_method(D_METHOD("get_distortion_matrix"), &UltraleapImage::get_distortion_matrix);
+    ClassDB::bind_method(D_METHOD("get_matrix_version"), &UltraleapImage::get_matrix_version);
 }
 
 void UltraleapImage::fill_images_data(Ref<UltraleapImage> left, Ref<UltraleapImage> right, const LEAP_IMAGE* images) {
@@ -21,14 +23,17 @@ void UltraleapImage::fill_images_data(Ref<UltraleapImage> left, Ref<UltraleapIma
         return;
     }
 
+    // We'll shove data 8 bytes by 8 bytes to go fast
     PackedInt64Array temp_left = PackedInt64Array();
     PackedInt64Array temp_right = PackedInt64Array();
 
     uint32_t tot_pixels = height * width;
 
+    uint64_t* data_left = (uint64_t*)((uint8_t*)images->data + images->offset);
+    uint64_t* data_right = (uint64_t*)((uint8_t*)(images + 1)->data + (images + 1)->offset);
+
+    // Fill the data 8 bytes at a time
     for (uint32_t i = 0; i < tot_pixels / 8; i++) {
-        uint64_t* data_left = (uint64_t*)((uint8_t*)images->data + images->offset);
-        uint64_t* data_right = (uint64_t*)((uint8_t*)(images + 1)->data + (images + 1)->offset);
         uint64_t pixel_left = data_left[i];
         uint64_t pixel_right = data_right[i];
         temp_left.push_back(pixel_left);
@@ -37,4 +42,32 @@ void UltraleapImage::fill_images_data(Ref<UltraleapImage> left, Ref<UltraleapIma
 
     left->data = temp_left.to_byte_array();
     right->data = temp_right.to_byte_array();
+
+    // Check and store left image distortion matrix
+    if (left->matrix_version != images->matrix_version) {
+        left->matrix_version = images->matrix_version;
+        left->distortion_matrix.clear();
+
+        for (uint32_t i = 0; i < 64; i++) {
+            for (uint32_t j = 0; j < 64; j++) {
+                float x = images->distortion_matrix->matrix[i][j].x;
+                float y = images->distortion_matrix->matrix[i][j].y;
+                left->distortion_matrix.push_back(Vector2(x, y));
+            }
+        }
+    }
+
+    // Check and store left image distortion matrix
+    if (right->matrix_version != (images + 1)->matrix_version) {
+        right->matrix_version = (images + 1)->matrix_version;
+        right->distortion_matrix.clear();
+
+        for (uint32_t i = 0; i < 64; i++) {
+            for (uint32_t j = 0; j < 64; j++) {
+                float x = images->distortion_matrix->matrix[i][j].x;
+                float y = images->distortion_matrix->matrix[i][j].y;
+                right->distortion_matrix.push_back(Vector2(x, y));
+            }
+        }
+    }
 }

src/image.h

@@ -18,13 +18,18 @@ class UltraleapImage : public Resource {
 
 public:
     PackedByteArray data = PackedByteArray();
+    PackedVector2Array distortion_matrix = PackedVector2Array();
 
     // Getters / Setters
 
     PackedByteArray get_data() { return data; }
+    PackedVector2Array get_distortion_matrix() { return distortion_matrix; }
+    uint64_t get_matrix_version() { return matrix_version; }
 
     static void fill_images_data(Ref<UltraleapImage> left, Ref<UltraleapImage> right, const LEAP_IMAGE* images);
 
+    uint64_t matrix_version = 999;
+
 protected:
     static void _bind_methods();
 };

src/ultraleap.cpp

@@ -151,7 +151,7 @@ void UltraleapHandTracking::serviceMessageLoop() {
     while (_isRunning) {
         result = LeapPollConnection(connectionHandle, 100, &msg);
 
-        if (result != eLeapRS_Success) {
+        if (result != eLeapRS_Success && result != eLeapRS_Timeout) {
             UtilityFunctions::print(UltraleapTypes::ultraleap_result_to_string(result));
             continue;
         }