Hartmut/gdvrpn
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

MVP with positon and rotation VRPN_PosQuat

Browse source
This commit is contained in:
Hartmut Seichter 2025-06-27 22:09:32 +02:00
parent 4921713026
commit 88a889be72
1 changed files with 45 additions and 55 deletions
Download patch file Download diff file Expand all files Collapse all files

100
VRPN.gd
View file

@ -1,78 +1,68 @@
extends Node extends Node
class_name VRPN class_name VRPN
static func marshal(data : PackedByteArray): static func marshal(data : PackedByteArray):
# need to fix that
var block_offset : int = 0 var block_offset : int = 0
var header_size = aligned_size(20) # kinda redundant as we take the seq number as well var header_size = aligned_size(20) # kinda redundant as we take the seq number as well
#print(data) # 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
while block_offset < data.size(): # read header
var length := header.get_32() as int # length of message
print("...") var time_sec := header.get_32() as int # time sec
var time_msec := header.get_32() as int # time micro sec
# reader for stream var sender_id := header.get_32() as int # sender id
var header := StreamPeerBuffer.new() var message_type := header.get_32() as int # type of message (payload)
header.data_array = data.slice(block_offset,block_offset+header_size) var sequence_num := header.get_32() as int # inofficial sequence number (padding)
header.big_endian = true # make sure we read as big endian
if false:
# read header print("length '%d'" % length)
var length := header.get_32() as int # length of message print("time_sec '%d'" % time_sec)
var time_sec := header.get_32() as int # time sec print("time_msec '%d'" % time_msec)
var time_msec := header.get_32() as int # time micro sec print("sender_id '%d'" % sender_id)
var sender_id = header.get_32() # sender id print("message_type '%d'" % message_type)
var message_type = header.get_32() # type of message (payload) print("sequence_num '%d'" % sequence_num)
var sequence_num = header.get_32() # inofficial sequence number (padding)
# print out
if false: print("block_offset:{0} header_size:{1} length:{2}".format([block_offset,header_size,length]))
print("length %d" % length)
print("time_sec %d" % time_sec) # sender description
print("time_msec %d" % time_msec) match message_type:
print("sender_id %d" % sender_id) -1:
print("message_type %d" % message_type)
print("sequence_num %d" % sequence_num)
print("block_offset:{0} header_size:{1} length:{2}".format([block_offset,header_size,length]))
# update blockoffset
block_offset += length
continue
# sender description
if message_type == -1:
var body := StreamPeerBuffer.new() var body := StreamPeerBuffer.new()
body.data_array = data.slice(24) body.data_array = data.slice(block_offset+header_size)
body.big_endian = true body.big_endian = true
# get length of string
var body_length = body.get_32() var body_length = body.get_32()
#print(body_length) # get string
#print(body.data_array)
var sender_name = body.get_string(body_length) var sender_name = body.get_string(body_length)
print("sender id {0} is '{1}'".format([sender_id,sender_name])) print("sender id {0} is '{1}'".format([sender_id,sender_name]))
# quat pos 4: # quat pos
if message_type == 4: # set body
var body := StreamPeerBuffer.new() var body := StreamPeerBuffer.new()
# set header size
body.data_array = data.slice(header_size,length) body.data_array = data.slice(block_offset+header_size,block_offset+header_size+length)
body.big_endian = true body.big_endian = true
# get id
var sensor_id = body.get_32() var sensor_id = body.get_32()
var padding = body.get_32() var padding = body.get_32()
var pos = Vector3(body.get_float(),body.get_float(),body.get_float()) var pos = Vector3(body.get_double(),body.get_double(),body.get_double())
var quat_w = body.get_float() # VRPN quaternions are w,xyz
var quat_x = body.get_float() var quat_w = body.get_double()
var quat_y = body.get_float() var quat_x = body.get_double()
var quat_z = body.get_float() var quat_y = body.get_double()
var quat_z = body.get_double()
var quat = Quaternion(quat_x,quat_y,quat_z,quat_w) var quat = Quaternion(quat_x,quat_y,quat_z,quat_w)
#print("Pos_Quat ",sensor_id,pos,quat)
print("------------")
static func aligned_size(actual_size : int,alignment : int = 8) -> int: static func aligned_size(actual_size : int,alignment : int = 8) -> int:
return (actual_size + alignment - 1) & ~(alignment - 1) return (actual_size + alignment - 1) & ~(alignment - 1)