gdvrpn/VRPN.gd

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,session : Session) -> void:
	
	# need to fix that 
	var block_offset : int = 0
	var header_size = aligned_size(20) # kinda redundant as we take the seq number as well

	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)
		
		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
		#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,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 : Session):
	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:
				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() # padding 
			var pos = Vector3(body.get_double(),body.get_double(),body.get_double())
			# VRPN quaternions are w,xyz
			var quat_w = body.get_double()
			var quat_x = body.get_double()
			var quat_y = body.get_double()
			var quat_z = body.get_double()
			var quat = Quaternion(quat_x,quat_y,quat_z,quat_w)
			print("Tracker:{0} Sensor:{1} Pos:{2} Quat:{3}".format([session.sensors[sender_id],sensor_id,pos,quat]))
		_:
			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)