From d3b5dbe322b6cc42f49671f7ac7b038b8f6f8ebd Mon Sep 17 00:00:00 2001
From: Hartmut Seichter <hartmut@technotecture.com>
Date: Mon, 30 Jun 2025 19:56:15 +0200
Subject: [PATCH] all pieces are there - now refactoring into something usable

---
 Root.tscn       |  1 -
 SocketClient.gd |  7 ++++--
 VRPN.gd         | 60 ++++++++++++++++++++++++++++++++++---------------
 3 files changed, 47 insertions(+), 21 deletions(-)

diff --git a/Root.tscn b/Root.tscn
index ddc3977..2e37dfb 100644
--- a/Root.tscn
+++ b/Root.tscn
@@ -4,7 +4,6 @@
 
 [node name="Node3D" type="Node3D"]
 script = ExtResource("1_gxo8o")
-vrpn_server = "212.201.64.122"
 
 [connection signal="connected" from="." to="." method="_on_connected"]
 [connection signal="data" from="." to="." method="_on_data"]
diff --git a/SocketClient.gd b/SocketClient.gd
index d75c83d..92f3ea3 100644
--- a/SocketClient.gd
+++ b/SocketClient.gd
@@ -5,12 +5,15 @@ signal data(data:Array)
 signal disconnected
 signal error
 
+# Graphics Interaction Lab OptiTrack system @212.201.64.122
 
 @onready var _stream: StreamPeerTCP = StreamPeerTCP.new()
 
-@export var vrpn_server : String = "localhost"
+@export var vrpn_server : String = "127.0.0.1"
 @export var vrpn_port : int = 3883
 
+var session : VRPN.Session = VRPN.Session.new()
+
 func _ready() -> void:
 	self.connect_to_host(vrpn_server,vrpn_port)
 
@@ -67,7 +70,7 @@ func _on_data(data : Array):
 		# kaboom we just send back the same cookie :) 
 		self.send(bytes)
 	else:
-		VRPN.marshall_block(bytes)
+		VRPN.marshall_block(bytes,session)
 
 
 			
diff --git a/VRPN.gd b/VRPN.gd
index 4426d80..a1ffa42 100644
--- a/VRPN.gd
+++ b/VRPN.gd
@@ -2,10 +2,18 @@ extends Node
 
 class_name VRPN
 
+enum { POS_QUAT }
+
 const magic_cookie_start : String = "vrpn: ver."
 
+class Session:
+	var sensors : Dictionary[int,String] = {}
+	var messages : Dictionary[int,String] = {}
+	func _init():
+		pass
 
-static func marshall_block(data : PackedByteArray) -> void:
+
+static func marshall_block(data : PackedByteArray,session : Session) -> void:
 	
 	# need to fix that 
 	var block_offset : int = 0
@@ -27,7 +35,7 @@ static func marshall_block(data : PackedByteArray) -> void:
 		var message_type := header.get_32() as int # type of message (payload)
 		var sequence_num := header.get_32() as int # inofficial sequence number (padding)
 		
-		if true:
+		if false:
 			print("length       '%d'" % length)
 			print("time_sec     '%d'" % time_sec)
 			print("time_msec    '%d'" % time_msec)
@@ -36,31 +44,46 @@ static func marshall_block(data : PackedByteArray) -> void:
 			print("sequence_num '%d'" % sequence_num)
 		
 		# print
-		print("block_offset:{0} header_size:{1} length:{2}".format([block_offset,header_size,length]))
+		#print("block_offset:{0} header_size:{1} length:{2}".format([block_offset,header_size,length]))
 		
-		marshall_body(data.slice(block_offset+header_size,block_offset+length),message_type)
+		marshall_body(data.slice(block_offset+header_size,block_offset+length),message_type,sender_id,session)
 		
 		# next datablock
 		block_offset += aligned_size(length)
 		
+static func decode_string(stream : StreamPeerBuffer) -> String:
+	var len = stream.get_32()
+	return stream.get_string(len)
+	
 		
-static func marshall_body(data : PackedByteArray,message_type : int):
+static func marshall_body(data : PackedByteArray,message_type : int, sender_id: int, session : Session):
 	var body := StreamPeerBuffer.new()
 	body.data_array = data
 	body.big_endian = true
-		# sender description
-	match message_type:
-		-1,-2:
-			# get length of string
-			var body_length = body.get_32()
-			# get string
-			var sender_name = body.get_string(body_length)
-			print("sender name is %s:" % sender_name)
-			#print(body.data_array)
-		4: # quat pos
+
+	# only take message_type directly for negative (-1,-2)
+	# messages that provide dynamic descriptors
+	if message_type < 0:
+		# message and sender descriptions
+		match message_type:
+			-1:
+				var name = decode_string(body)
+				print("sensor name is '%s' with '%d" % [name,sender_id])
+				session.sensors[sender_id] = name
+			-2:
+				var name = decode_string(body)
+				print("message name is '%s' for message_type '%d'" % [name,sender_id])
+				session.messages[sender_id] = name
+			# nothing to decode
+		return
+	
+	# now we use the string identifiers 
+	# because they are supposedly dynamically assigned
+	match session.messages[message_type]:
+		'vrpn_Tracker Pos_Quat': # quat pos
 			# get id
 			var sensor_id = body.get_32()
-			var padding = body.get_32()
+			var padding = body.get_32() # padding 
 			var pos = Vector3(body.get_double(),body.get_double(),body.get_double())
 			# VRPN quaternions are w,xyz
 			var quat_w = body.get_double()
@@ -68,9 +91,10 @@ static func marshall_body(data : PackedByteArray,message_type : int):
 			var quat_y = body.get_double()
 			var quat_z = body.get_double()
 			var quat = Quaternion(quat_x,quat_y,quat_z,quat_w)
-			print("sensor id {0} {1} {2}".format([sensor_id,pos,quat]))
+			print("Tracker:{0} Sensor:{1} Pos:{2} Quat:{3}".format([session.sensors[sender_id],sensor_id,pos,quat]))
 		_:
-			print("unhandled message type {0}".format([message_type]))
+			pass
+			#print("unhandled message type {0}".format([message_type]))
 
 
 static func aligned_size(actual_size : int, alignment : int = 8) -> int: