58 lines
1.4 KiB
Python
58 lines
1.4 KiB
Python
import numpy as np
|
|
from SwingTwist import SwingTwist
|
|
|
|
def SendAngle8(buffer, ix_ref, angle):
|
|
# Normalize angle
|
|
while angle >= 180:
|
|
angle -= 360
|
|
while angle < -180:
|
|
angle += 360
|
|
|
|
ix = ix_ref[0]
|
|
buffer[ix] = round((angle / 360) * 256).to_bytes(1, 'big')[0]
|
|
ix_ref[0] += 1
|
|
|
|
def SendFloat16(buffer, ix_ref, value):
|
|
ix = ix_ref[0]
|
|
value16 = np.float16(value)
|
|
binary = value16.view(np.uint16)
|
|
buffer[ix:ix+2] = [
|
|
(binary & 0xFF00) >> 8,
|
|
(binary & 0x00FF)
|
|
]
|
|
ix_ref[0] += 2
|
|
|
|
def ReceiveFloat16(buffer, ix_ref) -> float:
|
|
ix = ix_ref[0]
|
|
binary = (buffer[ix] << 8) + buffer[ix+1]
|
|
value16 = np.uint16(binary).view(np.float16)
|
|
ix_ref[0] += 2
|
|
return float(value16)
|
|
|
|
def SendSpherical(buffer, ix_ref, vector):
|
|
SendFloat16(buffer, ix_ref, vector.distance)
|
|
SendAngle8(buffer, ix_ref, vector.direction.horizontal)
|
|
SendAngle8(buffer, ix_ref, vector.direction.vertical)
|
|
|
|
def SendQuat32(buffer, ix_ref, q):
|
|
if isinstance(q, SwingTwist):
|
|
q = q.ToQuaternion()
|
|
|
|
ix = ix_ref[0]
|
|
qx = (int)(q.x * 127 + 128)
|
|
qy = (int)(q.y * 127 + 128)
|
|
qz = (int)(q.z * 127 + 128)
|
|
qw = (int)(q.w * 255)
|
|
if q.w < 0:
|
|
qx = -qx
|
|
qy = -qy
|
|
qz = -qz
|
|
qw = -qw
|
|
buffer[ix:ix+4] = [
|
|
qx,
|
|
qy,
|
|
qz,
|
|
qw
|
|
]
|
|
ix_ref[0] += 4
|