add moved files to correct location

addons/vrpn/scripts/VRPN.gd

@@ -0,0 +1,194 @@
+extends Node
+class_name VRPN
+
+# tracking associated data
+enum TrackingData { POS_QUAT, VELOCITY, ACCELERATION }
+
+# magic cookie
+const magic_cookie_start : String = "vrpn: ver."
+# kinda redundant as we take the seq number as well
+static var header_size : int = aligned_size(20)
+
+
+var sensors : Dictionary[int,String] = {}
+var messages : Dictionary[int,String] = {}
+
+signal connected(s:StreamPeerTCP)
+signal data(data:Array)
+signal disconnected
+signal error
+
+
+@onready var _stream: StreamPeerTCP = StreamPeerTCP.new()
+
+@export var tracker_receivers : Array[VRPN_Receiver] = []
+
+@export var vrpn_server : String = "127.0.0.1"
+@export var vrpn_port : int = 3883
+
+func _ready() -> void:
+	
+	self.connect_to_host(vrpn_server,vrpn_port)
+	
+	if not connected.has_connections():
+		connected.connect(self._on_connected)
+	if not data.has_connections():
+		data.connect(self._on_data)
+	if not disconnected.has_connections():
+		disconnected.connect(self._on_disconnected)
+	if not error.has_connections():
+		error.connect(self._on_error)
+
+func _process(delta: float) -> void:
+	var old_status = _stream.get_status()
+	_stream.poll()
+	var new_status = _stream.get_status()
+	if old_status != new_status:
+		match new_status:
+			_stream.STATUS_NONE:
+				emit_signal("disconnected")
+			_stream.STATUS_CONNECTING:
+				print("Connecting.")
+			_stream.STATUS_CONNECTED:
+				print("Connected.")
+				_stream.poll()
+				emit_signal("connected",_stream)
+			_stream.STATUS_ERROR:
+				print("Error with socket stream.")
+				emit_signal("error")
+
+	if new_status == _stream.STATUS_CONNECTED:
+		var available_bytes: int = _stream.get_available_bytes()
+		if available_bytes > 0:
+			var res = _stream.get_partial_data(available_bytes)
+			if res[0] != OK:
+				emit_signal("error")
+			else:
+				emit_signal("data", res[1])
+
+func connect_to_host(host: String, port: int) -> void:
+	print("Connecting to %s:%d" % [host, port])
+	if _stream.connect_to_host(host, port) != OK:
+		print("Error connecting to host.")
+		emit_signal("error")
+
+func send(data: PackedByteArray) -> bool:
+	if _stream.get_status() != _stream.STATUS_CONNECTED:
+		print("Error: Stream is not currently connected.")
+		return false
+	var error: int = _stream.put_data(data)
+	if error != OK:
+		print("Error writing to stream: ", error)
+		return false
+	return true
+	
+
+func _on_data(data : Array):
+	var bytes = PackedByteArray(data)
+	var as_cookie = bytes.get_string_from_ascii()
+	
+	# Cookie Hack!
+	if as_cookie.begins_with(VRPN.magic_cookie_start): # 
+		# kaboom we just send back the same cookie :) 
+		self.send(bytes)
+	else:
+		VRPN.marshall_block(bytes,self)
+
+
+func _on_connected(s : StreamPeerTCP):
+	print("Connected to",s.get_connected_host()) # Replace with function body.
+	
+func _on_disconnected():
+	print("Disconnected") # Replace with function body.
+
+func _on_error():
+	print("Error") # Replace with function body.
+	
+
+static func marshall_block(data : PackedByteArray,session : VRPN) -> void:
+	
+	# need to fix that 
+	var block_offset : int = 0
+	
+	while data.size() > block_offset:
+		# reader for stream
+		var header := StreamPeerBuffer.new()
+		# get block addresses
+		header.data_array = data.slice(block_offset,block_offset+header_size)
+		# make sure we read as big endian
+		header.big_endian = true 
+		
+		# read header
+		var length := header.get_32() as int # length of message
+		var time_sec := header.get_32() as int  # datetime sec
+		var time_msec := header.get_32() as int # datetime micro sec
+		var sender_id := header.get_32() as int  # sender id 
+		var message_type := header.get_32() as int # type of message (payload)
+		var sequence_num := header.get_32() as int # inofficial sequence number (padding)
+
+		# for debugging
+		#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)
+		
+		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, sender_id: int, session : VRPN):
+	var body := StreamPeerBuffer.new()
+	body.data_array = data
+	body.big_endian = true
+
+	# 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: # sensor names
+				var name = decode_string(body)
+				print("sensor name is '%s' with '%d" % [name,sender_id])
+				session.sensors[sender_id] = name
+			-2: # message names
+				var name = decode_string(body)
+				print("message name is '%s' for message_type '%d'" % [name,sender_id])
+				session.messages[sender_id] = name
+		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() # padding 
+			# position
+			var pos = Vector3(body.get_double(),body.get_double(),body.get_double())
+			# VRPN quat layout and Godot Quaternion c'tor identical with x,y,z,w
+			var quat = Quaternion(body.get_double(),body.get_double(),body.get_double(),body.get_double()).normalized()
+			# submit to listener
+			for r in session.tracker_receivers:
+				r._on_pos_quat({
+					"tracker" : session.sensors[sender_id],
+					"sensor" : sensor_id,
+					"position" : pos,
+					"rotation" : quat
+				})
+		'vrpn_Tracker Velocity':
+			pass
+		_:
+			pass
+			#print("unhandled message type {0}".format([message_type]))
+
+
+static func aligned_size(actual_size : int, alignment : int = 8) -> int:
+	return (actual_size + alignment - 1) & ~(alignment - 1)