diff --git a/LowLevelMessages.py b/LowLevelMessages.py index acbd01f..b0e31ae 100644 --- a/LowLevelMessages.py +++ b/LowLevelMessages.py @@ -1,4 +1,5 @@ import numpy as np +from .SwingTwist import SwingTwist def SendAngle8(buffer, ix_ref, angle): # Normalize angle @@ -21,12 +22,22 @@ def SendFloat16(buffer, ix_ref, value): ] 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) diff --git a/Messages.py b/Messages.py index 6243c0f..f8baa83 100644 --- a/Messages.py +++ b/Messages.py @@ -62,7 +62,15 @@ class NetworkIdMsg(IMessage): self.network_id ] return NetworkIdMsg.length - + +class InvestigateMsg(): + id = 0x81 + length = 3 + + def __init__(self, buffer): + self.network_id = buffer[1] + self.thing_id = buffer[2] + class ThingMsg(IMessage): id = 0x80 length = 5 @@ -151,7 +159,7 @@ class PoseMsg(IMessage): id = 0x10 length = 4 - def __init__(self, network_id, thing, poseType): + def __init__(self, network_id, thing): self.network_id = network_id self.thing = thing @@ -176,3 +184,28 @@ class PoseMsg(IMessage): if self.thing.pose_updated & Thing.AngularVelocity: LowLevelMessages.SendSpherical(buffer, ix, self.thing.angularVelocity) return ix[0] + +class BinaryMsg(): + id = 0xB1 + + def __init__(self, buffer): + self.network_id = buffer[1] + self.thing_id = buffer[2] + self.thing: Thing = Thing.Get(self.network_id, self.thing_id) + self.data = buffer[3:] + + def SendTo(participant, thing, data: bytearray): + length = 3 + + if thing.network_id is None or thing is None or data is None: + return False + + participant.buffer[0:length] = [ + id, + participant.network_id, + thing.id + ] + full_length = length + len(data) + participant.buffer[length:full_length] = data + participant.SendBuffer(full_length) + return True \ No newline at end of file diff --git a/Participant.py b/Participant.py index 64b8a09..d99b7f2 100644 --- a/Participant.py +++ b/Participant.py @@ -52,6 +52,8 @@ class Participant: pass def ProcessNetworkIdMsg(self, data: bytearray): pass + def ProcessInvestigateMsg(self, data: bytearray): + pass def ReceiveData(self, data): msgId = data[0] @@ -62,4 +64,9 @@ class Participant: case Messages.NetworkIdMsg.id: msg = Messages.NetworkIdMsg(data) self.ProcessNetworkIdMsg(msg) - + case Messages.InvestigateMsg.id: + msg = Messages.InvestigateMsg(data) + self.ProcessInvestigateMsg(msg) + case Messages.BinaryMsg.id: + msg = Messages.BinaryMsg(data) + msg.thing.ProcessBinary(msg.data) diff --git a/Quaternion.py b/Quaternion.py index 6a926ca..0340227 100644 --- a/Quaternion.py +++ b/Quaternion.py @@ -1,8 +1,44 @@ +import math + +Deg2Rad = (math.pi * 2) / 360 + class Quaternion: + def __init__(self): self.x = 0 self.y = 0 self.z = 0 self.w = 1 + + @staticmethod + def Euler(x, y, z): + yaw = x * Deg2Rad + pitch = y * Deg2Rad + roll = z * Deg2Rad + + roll_over_2 = roll * 0.5 + sin_roll_over_2 = math.sin(roll_over_2) + cos_roll_over_2 = math.cos(roll_over_2) + + pitch_over_2 = pitch * 0.5 + sin_pitch_over_2 = math.sin(pitch_over_2) + cos_pitch_over_2 = math.cos(pitch_over_2) + + yaw_over_2 = yaw * 0.5 + sin_yaw_over_2 = math.sin(yaw_over_2) + cos_yaw_over_2 = math.cos(yaw_over_2) + + result = Quaternion() + result.w = (cos_yaw_over_2 * cos_pitch_over_2 * cos_roll_over_2 + + sin_yaw_over_2 * sin_pitch_over_2 * sin_roll_over_2) + result.x = (sin_yaw_over_2 * cos_pitch_over_2 * cos_roll_over_2 + + cos_yaw_over_2 * sin_pitch_over_2 * sin_roll_over_2) + result.y = (cos_yaw_over_2 * sin_pitch_over_2 * cos_roll_over_2 - + sin_yaw_over_2 * cos_pitch_over_2 * sin_roll_over_2) + result.z = (cos_yaw_over_2 * cos_pitch_over_2 * sin_roll_over_2 - + sin_yaw_over_2 * sin_pitch_over_2 * cos_roll_over_2) + + return result + Quaternion.identity = Quaternion() \ No newline at end of file diff --git a/SiteServer.py b/SiteServer.py index 8c810f2..137e4e0 100644 --- a/SiteServer.py +++ b/SiteServer.py @@ -11,18 +11,42 @@ class SiteServer(Participant): def Update(self, currentTime): ready_to_read, _, _ = select.select([self.udpSocket], [], [], 0.1) # Timeout of 0.1 seconds - if ready_to_read: + while ready_to_read: data, addr = self.udpSocket.recvfrom(1024) self.ReceiveData(data) + ready_to_read, _, _ = select.select([self.udpSocket], [], [], 0.1) # Timeout of 0.1 seconds return super().Update(currentTime) - - def ProcessNetworkIdMsg(self, thing_msg): - self.network_id = thing_msg.network_id - thing = next(iter(Thing.allThings)) + + def SendThingInfo(self, thing, recurse = False): + if thing is None: + return + thing_msg = Messages.ThingMsg(self.network_id, thing) thing_msg.SendTo(self) name_msg = Messages.NameMsg(self.network_id, thing) name_msg.SendTo(self) model_msg = Messages.ModelUrlMsg(self.network_id, thing) model_msg.SendTo(self) + pose_msg = Messages.PoseMsg(self.network_id, thing) + pose_msg.SendTo(self) + + if recurse: + for child in thing.children: + self.SendThingInfo(child, True) + + def ProcessNetworkIdMsg(self, thing_msg): + self.network_id = thing_msg.network_id + # HACK: send the root things first + for thing in Thing.allThings: + if thing is not None and thing.parent_id == 0: + self.SendThingInfo(thing) + # then sent the rest + for thing in Thing.allThings: + if thing is not None and thing.parent_id != 0: + self.SendThingInfo(thing) + + def ProcessInvestigateMsg(self, msg: Messages.InvestigateMsg): + thing = Thing.Get(msg.network_id, msg.thing_id) + if thing is not None: + self.SendThingInfo(thing) diff --git a/SwingTwist.py b/SwingTwist.py new file mode 100644 index 0000000..e312b15 --- /dev/null +++ b/SwingTwist.py @@ -0,0 +1,18 @@ +from .Direction import Direction +from .Quaternion import Quaternion + +class SwingTwist: + def __init__(self, swing: Direction, twist: float): + if swing.vertical > 90 or swing.vertical < -90: + swing.horizontal += 180 + swing.vertical = 180 - swing.vertical + twist += 180 + + self.swing = swing + self.twist = twist + + def ToQuaternion(self) -> Quaternion: + q = Quaternion.Euler(-self.swing.vertical, + self.swing.horizontal, + self.twist) + return q \ No newline at end of file diff --git a/Thing.py b/Thing.py index a39a350..9aac044 100644 --- a/Thing.py +++ b/Thing.py @@ -2,23 +2,28 @@ from .Spherical import Spherical from .Quaternion import Quaternion class Thing: - allThings = set() + class Type: + Undetermined = 0x00 + Switch = 0x01 + DistanceSensor = 0x02 + DirectionalSensor = 0x03 + Animator = 0x40 Position = 0x01 Orientation = 0x02 LinearVelocity = 0x04 AngularVelocity = 0x08 - def __init__(self, parent=None): + def __init__(self, type=Type.Undetermined, parent=None, name=None): self.networkId = 0 self.id = 0 - self.type = 0 + self.type = type if parent is None: self.parent_id = 0 else: self.parent_id = parent.id - self.name = None + self.name = name self.model_url = None self.position = Spherical.zero @@ -33,11 +38,25 @@ class Thing: def update(self, currentTime): pass + def ProcessBinary(self, data): + pass + + allThings = set({ None }) + @staticmethod def Add(thing): thing.id = len(Thing.allThings) Thing.allThings.add(thing) + @staticmethod + def Get(networkId, thingId): + for thing in Thing.allThings: + if thing is not None: + if thing.networkId == networkId and thing.id == thingId: + return thing + return None + def UpdateAll(currentTime): for thing in Thing.allThings: - thing.update(currentTime) + if thing is not None: + thing.update(currentTime) diff --git a/__init__.py b/__init__.py index 69ea4cb..b2489e2 100644 --- a/__init__.py +++ b/__init__.py @@ -1,7 +1,14 @@ -__all__ = ['Direction', 'Spherical', 'Thing', 'Participant', 'Messages', 'SiteServer'] +__all__ = ['Direction', + 'Spherical', + 'Thing', + 'Participant', + 'Messages', + 'SiteServer', + 'SwingTwist'] from .Direction import Direction from .Participant import Participant from .Thing import Thing from .Spherical import Spherical +from .SwingTwist import SwingTwist from .SiteServer import SiteServer \ No newline at end of file