From eb4513291f0a465a4a041b05119900a541fc792e Mon Sep 17 00:00:00 2001
From: Hartmut Seichter <hartmut@gmail.com>
Date: Fri, 27 Jun 2025 17:55:19 +0200
Subject: [PATCH] WIP

---
 VRPN.gd     | 75 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 VRPN.gd.uid |  1 +
 2 files changed, 76 insertions(+)
 create mode 100644 VRPN.gd
 create mode 100644 VRPN.gd.uid

diff --git a/VRPN.gd b/VRPN.gd
new file mode 100644
index 0000000..f9a49f5
--- /dev/null
+++ b/VRPN.gd
@@ -0,0 +1,75 @@
+extends Node
+
+class_name VRPN
+	
+	
+	
+static func marshal(data : PackedByteArray):
+	
+	var block_offset : int = 0
+	var header_size = aligned_size(20) # kinda redundant as we take the seq number as well
+	
+	#print(data)
+	
+	while block_offset < data.size():
+		
+		var header := StreamPeerBuffer.new()
+		header.data_array = data.slice(block_offset,header_size)
+		header.big_endian = true # make sure we read as big endian
+		
+		# Header
+		var length := header.get_32() as int # length of message
+		var time_sec := header.get_32() as int  # time sec
+		var time_msec := header.get_32() as int # time micro sec
+		var sender_id = header.get_32()  # sender id 
+		var message_type = header.get_32() # type of message (payload)
+		var sequence_num = header.get_32() # inofficial sequence number (padding)
+		
+		if false:
+			print("length       %d" % length)
+			print("time_sec     %d" % time_sec)
+			print("time_msec    %d" % time_msec)
+			print("sender_id    %d" % sender_id)
+			print("message_type %d" % message_type)
+			print("sequence_num %d" % sequence_num)
+		
+		
+		print("block_offset:{0} header_size:{1} length:{2}".format([block_offset,header_size,length]))
+		
+		# update blockoffset
+		block_offset += length
+		
+		continue
+		
+		# sender description
+		if message_type == -1:
+			var body := StreamPeerBuffer.new()
+			body.data_array = data.slice(24)
+			body.big_endian = true
+			var body_length = body.get_32()
+			#print(body_length)
+			#print(body.data_array)
+			
+			var sender_name = body.get_string(body_length)
+			print("sender id {0} is '{1}'".format([sender_id,sender_name]))
+		
+		# quat pos
+		if message_type == 4:
+			var body := StreamPeerBuffer.new()
+		
+			body.data_array = data.slice(header_size,length)
+			body.big_endian = true
+			var sensor_id = body.get_32()
+			var padding = body.get_32()
+			var pos = Vector3(body.get_float(),body.get_float(),body.get_float())
+			var quat_w = body.get_float()
+			var quat_x = body.get_float()
+			var quat_y = body.get_float()
+			var quat_z = body.get_float()
+			var quat = Quaternion(quat_x,quat_y,quat_z,quat_w)
+			
+		#print("Pos_Quat ",sensor_id,pos,quat)
+	print("------------")
+
+static func aligned_size(actual_size : int,alignment : int = 8) -> int:		
+	return (actual_size + alignment - 1) & ~(alignment - 1)
diff --git a/VRPN.gd.uid b/VRPN.gd.uid
new file mode 100644
index 0000000..06400fa
--- /dev/null
+++ b/VRPN.gd.uid
@@ -0,0 +1 @@
+uid://dmq3i7qmo1qe0