77 lines
2.5 KiB
GDScript
77 lines
2.5 KiB
GDScript
extends Node
|
|
|
|
class_name VRPN
|
|
|
|
const magic_cookie_start : String = "vrpn: ver."
|
|
|
|
|
|
static func marshall_block(data : PackedByteArray) -> 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 true:
|
|
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)
|
|
|
|
# next datablock
|
|
block_offset += aligned_size(length)
|
|
|
|
|
|
static func marshall_body(data : PackedByteArray,message_type : int):
|
|
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
|
|
# get id
|
|
var sensor_id = body.get_32()
|
|
var padding = body.get_32()
|
|
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("sensor id {0} {1} {2}".format([sensor_id,pos,quat]))
|
|
_:
|
|
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)
|