93 lines
2.6 KiB
Python
93 lines
2.6 KiB
Python
import numpy as np
|
|
from LinearAlgebra.SwingTwist import SwingTwist
|
|
from LinearAlgebra.Spherical import Spherical
|
|
from LinearAlgebra.Quaternion import Quaternion
|
|
from LinearAlgebra.Angle import Angle
|
|
|
|
def SendAngle8(buffer, ref_ix, angle):
|
|
"""! Send an 8-bit angle value """
|
|
# angle = Angle.Normalize(angle)
|
|
|
|
ix = ref_ix[0]
|
|
buffer[ix] = int((angle.InDegrees() / 360) * 256).to_bytes(1, 'big', signed=True)[0]
|
|
ref_ix[0] += 1
|
|
|
|
def ReceiveAngle8(buffer: bytes, ref_ix: list[int]) -> Angle:
|
|
angle: Angle = Angle()
|
|
angle.value = buffer[ref_ix[0]]
|
|
ref_ix[0] += 1
|
|
return angle
|
|
|
|
def SendFloat16(buffer, ix_ref, value):
|
|
"""! Send a 16-bit floating point 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:
|
|
"""! Receive a 16-bit floating point value """
|
|
ix = ix_ref[0]
|
|
# if ix < len(buffer) - 1:
|
|
binary = (buffer[ix] << 8) + buffer[ix+1]
|
|
# else:
|
|
# binary = 0
|
|
value16 = np.uint16(binary).view(np.float16)
|
|
ix_ref[0] += 2
|
|
return float(value16)
|
|
|
|
def SendSpherical(buffer, ix_ref, vector):
|
|
"""! Send a spherical vector
|
|
"""
|
|
SendFloat16(buffer, ix_ref, vector.distance)
|
|
SendAngle8(buffer, ix_ref, vector.direction.horizontal)
|
|
SendAngle8(buffer, ix_ref, vector.direction.vertical)
|
|
|
|
def ReceiveSpherical(buffer: bytes, ref_ix: list[int]) -> Spherical:
|
|
"""! Receive a spherical vector
|
|
"""
|
|
distance = ReceiveFloat16(buffer, ref_ix)
|
|
horizontal = ReceiveAngle8(buffer, ref_ix)
|
|
vertical = ReceiveAngle8(buffer, ref_ix)
|
|
v: Spherical = Spherical.Degrees(distance, horizontal, vertical)
|
|
return v
|
|
|
|
def SendQuat32(buffer, ref_ix, q):
|
|
"""! Send a 32-bit quaternion value """
|
|
if isinstance(q, SwingTwist):
|
|
q = q.ToQuaternion()
|
|
|
|
ix = ref_ix[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
|
|
]
|
|
ref_ix[0] += 4
|
|
|
|
def ReceiveQuaternion(buffer: bytes, ref_ix: list[int]):
|
|
qx: float = (buffer[ref_ix[0]] - 128) / 127
|
|
ref_ix[0] += 1
|
|
qy: float = (buffer[ref_ix[0]] - 128) / 127
|
|
ref_ix[0] += 1
|
|
qz: float = (buffer[ref_ix[0]] - 128) / 127
|
|
ref_ix[0] += 1
|
|
qw: float = buffer[ref_ix[0]] / 255
|
|
ref_ix[0] += 1
|
|
q: Quaternion = Quaternion(qx, qy, qz, qw)
|
|
return q
|