extends Node class_name VRPN static func marshal(data : PackedByteArray): # 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 # 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 # time sec var time_msec := header.get_32() as int # time 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])) # sender description match message_type: -1: var body := StreamPeerBuffer.new() body.data_array = data.slice(block_offset+header_size) body.big_endian = true # get length of string var body_length = body.get_32() # get string var sender_name = body.get_string(body_length) #print("sender id {0} is '{1}'".format([sender_id,sender_name])) 4: # quat pos # set body var body := StreamPeerBuffer.new() # set header size body.data_array = data.slice(block_offset+header_size,block_offset+header_size+length) body.big_endian = true # 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("message id {0}".format([message_type])) static func aligned_size(actual_size : int,alignment : int = 8) -> int: return (actual_size + alignment - 1) & ~(alignment - 1)