straight walking ant

Direction.py

@@ -0,0 +1,44 @@
+import math
+
+class Direction:
+    def __init__(self, horizontal=0, vertical=0):
+        self.horizontal = horizontal
+        self.vertical = vertical
+
+    def __add__(self, other):
+        return Direction(self.horizontal + other.x, self.vertical + other.y)
+
+    def __sub__(self, other):
+        return Direction(self.horizontal - other.x, self.vertical - other.y)
+
+    def __mul__(self, scalar):
+        return Direction(self.horizontal * scalar, self.vertical * scalar)
+
+    def __truediv__(self, scalar):
+        if scalar != 0:
+            return Direction(self.horizontal / scalar, self.vertical / scalar)
+        else:
+            raise ValueError("Cannot divide by zero")
+
+    def magnitude(self):
+        return math.sqrt(self.horizontal**2 + self.vertical**2)
+
+    def normalize(self):
+        mag = self.magnitude()
+        if mag != 0:
+            return Direction(self.horizontal / mag, self.vertical / mag)
+        else:
+            return Direction(0, 0)
+
+    def dot(self, other):
+        return self.horizontal * other.x + self.vertical * other.y
+
+    def __repr__(self):
+        return f"Direction(x={self.horizontal}, y={self.vertical})"
+    
+Direction.forward = Direction(0, 0)
+Direction.backward = Direction(-180, 0)
+Direction.up = Direction(0, 90)
+Direction.down = Direction(0, -90)
+Direction.left = Direction(-90, 0)
+Direction.right = Direction(90, 0)

LowLevelMessages.py

@@ -0,0 +1,46 @@
+import numpy as np
+
+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 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):
+    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

Messages.py

@@ -1,32 +1,93 @@
-from controlcore_python.Participant import Participant
+# from .Participant import Participant
+from . import LowLevelMessages
+from .Spherical import Spherical
+from .Quaternion import Quaternion
 
 class IMessage:
-    def SendMsg(client: Participant, msg):
-        bufferSize =msg.Serialize(client.buffer)
-        return client.SendBuffer(bufferSize)
+    id = 0x00
 
     def Serialize(buffer):
         return 0
     
+    def SendTo(self, participant):
+        bufferSize = self.Serialize([participant.buffer])
+        if bufferSize == 0:
+            return False
+        return participant.SendBuffer(bufferSize)
+    
+    def Publish(self, participant):
+        bufferSize = self.Serialize([participant.buffer])
+        if bufferSize == 0:
+            return False
+        return participant.PublishBuffer(bufferSize)
+
+class ClientMsg(IMessage):
+    id = 0xA0
+    length = 2
+
+    def __init__(self, networkId):
+        if isinstance(networkId, int):
+            self.networkId = networkId
+        elif isinstance(networkId, bytes):
+            self.networkId = networkId[1]
+
+    def Serialize(self, buffer_ref):
+        if self.networkId is None:
+            return 0
+        
+        buffer: bytearray = buffer_ref[0]
+        buffer[0:2] = [
+            ClientMsg.id,
+            self.networkId
+        ]
+        return ClientMsg.length
+
 class NetworkIdMsg(IMessage):
     id = 0xA1
     length = 2
 
     def __init__(self, networkId):
+        self.networkId = None
+        if isinstance(networkId, int):
             self.networkId = networkId
+        elif isinstance(networkId, bytes):
+            self.networkId = networkId[1]
 
-    def Serialize(self, buffer):
-        ix = 0
-        buffer[ix] = NetworkIdMsg.id
-        ix+=1
-        buffer[ix] = self.networkId
-        ix+=1
-        return ix
+    def Serialize(self, buffer_ref):
+        if self.networkId is None:
+            return 0
+        
+        buffer: bytearray = buffer_ref[0]
+        buffer[0:2] = [
+            NetworkIdMsg.id,
+            self.networkId
+        ]
+        return NetworkIdMsg.length
+    
+class ThingMsg(IMessage):
+    id = 0x80
+    length = 5
+
+    def __init__(self, networkId, thing):
+        self.networkId = networkId
+        self.thingId = thing.id
+        self.thingType = thing.type
+        self.parentId = None
+
+    def Serialize(self, buffer_ref):
+        if self.networkId is None or self.thingId is None:
+            return 0
+        
+        buffer: bytearray = buffer_ref[0]
+        buffer[0:5] = [
+            ThingMsg.id,
+            self.networkId,
+            self.thingId,
+            0x00,
+            0x00
+        ]
+        return ThingMsg.length
 
-    @staticmethod
-    def Send(participant: Participant, networkId):
-        msg = NetworkIdMsg(networkId)
-        return IMessage.SendMsg(participant, msg)
 
 class ModelUrlMsg(IMessage):
     id = 0x90
@@ -69,7 +130,43 @@ class ModelUrlMsg(IMessage):
         #     ix+=1
         # return ix
     
-    @staticmethod
-    def Send(participant, networkId, thingId, modelUrl):
-        msg = ModelUrlMsg(networkId, thingId, modelUrl)
-        return IMessage.SendMsg(participant, msg)
+class PoseMsg(IMessage):
+    id = 0x10
+    length = 4 + 4 + 4
+
+    Position = 0x01
+    Orientation = 0x02
+    LinearVelocity = 0x04
+    AngularVelocity = 0x08
+
+    def __init__(self, networkId, thing, poseType):
+        self.networkId = networkId
+        self.thingId = thing.id
+    
+        self.poseType = poseType
+        self.position = Spherical.zero
+        self.orientation = Quaternion.identity
+        self.linearVelocity = thing.linearVelocity
+        self.angularVelocity = thing.angularVelocity
+
+    def Serialize(self, buffer_ref):
+        if (self.networkId is None) or (self.thingId is None):
+            return 0
+        
+        buffer: bytearray = buffer_ref[0]
+        buffer[0:4] = [
+            PoseMsg.id,
+            self.networkId,
+            self.thingId,
+            self.poseType
+        ]
+        ix = [4]
+        if self.poseType & PoseMsg.Position:
+            LowLevelMessages.SendSpherical(buffer, ix, self.position)
+        if self.poseType & PoseMsg.Orientation:
+            LowLevelMessages.SendQuat32(buffer, ix, self.orientation)
+        if self.poseType & PoseMsg.LinearVelocity:
+            LowLevelMessages.SendSpherical(buffer, ix, self.linearVelocity)
+        if self.poseType & PoseMsg.AngularVelocity:
+            LowLevelMessages.SendSpherical(buffer, ix, self.angularVelocity)
+        return ix[0]

Participant.py

@@ -1,12 +1,20 @@
 import socket
+from . import Messages
+from .Thing import Thing
+
 
 class Participant:
+    publishInterval = 3 # 3 seconds
     def __init__(self, ipAddress, port):
         self.buffer = bytearray(256)
         self.ipAddress = ipAddress
         self.port = port
         self.udpSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)        
-        self.udpSocket.bind(("0.0.0.0", 7681))
+        self.udpSocket.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
+        self.networkId = 0
+        #self.udpSocket.bind(("0.0.0.0", 7681))
+        self.buffer = bytearray(256)
+        self.nextPublishMe = 0
     
     def SendBuffer(self, bufferSize):
         if self.ipAddress is None:
@@ -17,3 +25,40 @@ class Participant:
         
         self.udpSocket.sendto(self.buffer[:bufferSize], (self.ipAddress, self.port))
         return True
+
+    def PublishBuffer(self, bufferSize):
+        if self.ipAddress is None:
+            return False
+        
+        if bufferSize <= 0:
+            return True
+        
+        self.udpSocket.sendto(self.buffer[:bufferSize], ('<broadcast>', self.port))
+        return True
+
+    def PublishMe(self):
+        msg = Messages.ClientMsg(self.networkId)
+        msg.Publish(self)
+
+    def Update(self, currentTime):
+        if (currentTime > self.nextPublishMe):
+            self.PublishMe()
+            self.nextPublishMe = currentTime + Participant.publishInterval
+
+        Thing.UpdateAll(currentTime)
+
+    def ProcessClientMsg(self, data: bytearray):
+        pass
+    def ProcessNetworkIdMsg(self, data: bytearray):
+        pass
+    
+    def ReceiveData(self, data):
+        msgId = data[0]
+        match msgId:
+            case Messages.ClientMsg.id:
+                msg = Messages.ClientMsg(data)
+                self.ProcessClientMsg(msg)
+            case Messages.NetworkIdMsg.id:
+                msg = Messages.NetworkIdMsg(data)
+                self.ProcessNetworkIdMsg(msg)
+

Quaternion.py

@@ -0,0 +1,8 @@
+class Quaternion:
+    def __init__(self):
+        self.x = 0
+        self.y = 0
+        self.z = 0
+        self.w = 1
+
+Quaternion.identity = Quaternion()

Spherical.py

@@ -0,0 +1,37 @@
+import math
+from .Direction import Direction
+
+class Spherical:
+    def __init__(self, distance, direction):
+        self.distance = distance
+        self.direction = direction
+
+    # def __init__(self, distance, horizontal, vertical):
+    #     self.distance = distance
+    #     self.direction = Direction(horizontal, vertical)
+
+    def to_cartesian(self):
+        x = self.distance * math.sin(self.direction.horizontal) * math.cos(self.direction.vertical)
+        y = self.distance * math.sin(self.direction.horizontal) * math.sin(self.direction.vertical)
+        z = self.distance * math.cos(self.direction.horizontal)
+        return x, y, z
+
+    def from_cartesian(self, x, y, z):
+        self.distance = math.sqrt(x**2 + y**2 + z**2)
+        self.direction.horizontal = math.acos(z / self.distance)
+        self.direction.vertical = math.atan2(y, x)
+
+    def __add__(self, other):
+        x1, y1, z1 = self.to_cartesian()
+        x2, y2, z2 = other.to_cartesian()
+        return Spherical.from_cartesian(x1 + x2, y1 + y2, z1 + z2)
+
+    def __sub__(self, other):
+        x1, y1, z1 = self.to_cartesian()
+        x2, y2, z2 = other.to_cartesian()
+        return Spherical.from_cartesian(x1 - x2, y1 - y2, z1 - z2)
+
+    def __repr__(self):
+        return f"Spherical(r={self.distance}, horizontal={self.direction.horizontal}, phi={self.direction.vertical})"
+
+Spherical.zero = Spherical(0, Direction.forward)

Thing.py

@@ -0,0 +1,22 @@
+class Thing:
+    allThings = set()
+
+    def __init__(self):
+        self.networkId = None
+        self.id = None
+        self.type = None
+
+        self.modelUrl = None
+        Thing.Add(self)
+    
+    def Update(self, currentTime):
+        pass
+
+    @staticmethod
+    def Add(thing):
+        thing.id = len(Thing.allThings)
+        Thing.allThings.add(thing)
+    
+    def UpdateAll(currentTime):
+        for thing in Thing.allThings:
+            thing.Update(currentTime)

__init__.py

@@ -0,0 +1,6 @@
+__all__ = ['Direction', 'Spherical', 'Thing', 'Participant', 'Messages']
+
+from .Direction import Direction
+from .Participant import Participant
+from .Thing import Thing
+from .Spherical import Spherical