Merge branch 'main' of http://gitlab.passervr.com/passer/csharp/controlcore

LinearAlgebra.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 028f10636fdaa594a9a9969924e5ff18
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

LinearAlgebra/Direction.cs

@@ -0,0 +1,32 @@
+namespace Passer.LinearAlgebra {
+
+    public class Direction {
+        public float horizontal;
+        public float vertical;
+
+        public Direction() {
+            horizontal = 0;
+            vertical = 0;
+        }
+        public Direction(float horizontal, float vertical) {
+            this.horizontal = horizontal;
+            this.vertical = vertical;
+            //Normalize();
+        }
+
+        public readonly static Direction forward = new(0, 0);
+        public readonly static Direction backward = new(-180, 0);
+        public readonly static Direction up = new(0, 90);
+        public readonly static Direction down = new(0, -90);
+        public readonly static Direction left = new(-90, 0);
+        public readonly static Direction right = new(90, 0);
+
+        public void Normalize() {
+            if (this.vertical > 90 ||    this.vertical < -90) {
+                this.horizontal += 180;
+                this.vertical = 180 - this.vertical;
+            }
+        }
+    }
+
+}

LinearAlgebra/Direction.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 9975e4702de32624e8d3deaf84669f2f
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

LinearAlgebra/Spherical.cs

@@ -3,16 +3,18 @@ namespace Passer.LinearAlgebra
     public class Spherical
     {
         public float distance;
-        public float horizontal;
-        public float vertical;
+        public Direction direction;
 
         public static Spherical zero = new(0, 0, 0);
 
         public Spherical(float distance, float horizontal, float vertical)
         {
             this.distance = distance;
-            this.horizontal = horizontal;
-            this.vertical = vertical;
+            this.direction = new Direction(horizontal, vertical);
+        }
+        public Spherical(float distance, Direction direction) {
+            this.distance = distance;
+            this.direction = direction;
         }
     }
 }

LinearAlgebra/SwingTwist.cs

@@ -0,0 +1,20 @@
+namespace Passer.LinearAlgebra {
+
+    public class SwingTwist {
+        public Direction swing;
+        public float twist;
+
+        public static readonly SwingTwist zero = new(0, 0, 0);
+
+        public SwingTwist(Direction swing, float twist) {
+            this.swing = swing;
+            this.twist = twist;
+        }
+        public SwingTwist(float horizontalSwing, float verticalSwing, float twist) {
+            this.swing = new Direction(horizontalSwing, verticalSwing);
+            this.swing.Normalize();
+            this.twist = twist;
+        }
+    }
+
+}

LinearAlgebra/SwingTwist.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 003a6ec763b101642b0589486f087aef
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

LowLevelMessages.cs

@@ -1,17 +1,14 @@
 using Passer.LinearAlgebra;
 
-public class LowLevelMessages
-{
+public class LowLevelMessages {
 
-    public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v)
-    {
+    public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v) {
         SendFloat16(buffer, ref ix, new float16(v.distance));
-        SendAngle8(buffer, ref ix, v.horizontal);
-        SendAngle8(buffer, ref ix, v.vertical);
+        SendAngle8(buffer, ref ix, v.direction.horizontal);
+        SendAngle8(buffer, ref ix, v.direction.horizontal);
     }
 
-    public static Spherical ReceiveSpherical(byte[] data, ref byte ix)
-    {
+    public static Spherical ReceiveSpherical(byte[] data, ref byte ix) {
         float distance = ReceiveFloat16(data, ref ix);
         float horizontal = ReceiveAngle8(data, ref ix);
         float vertical = ReceiveAngle8(data, ref ix);
@@ -19,14 +16,18 @@ public class LowLevelMessages
         return v;
     }
 
-    public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q)
-    {
+    public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s) {
+        UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(-s.swing.vertical, s.swing.horizontal, s.twist);
+        Quat32 q32 = new(q.x, q.y, q.z, q.w);
+        SendQuat32(buffer, ref ix, q32);
+
+    }
+    public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q) {
         int qx = (int)(q.x * 127 + 128);
         int qy = (int)(q.y * 127 + 128);
         int qz = (int)(q.z * 127 + 128);
         int qw = (int)(q.w * 255);
-        if (q.w < 0)
-        {
+        if (q.w < 0) {
             qx = -qx;
             qy = -qy;
             qz = -qz;
@@ -38,8 +39,7 @@ public class LowLevelMessages
         buffer[ix++] = (byte)qz;
         buffer[ix++] = (byte)qw;
     }
-    public static Quat32 ReceiveQuat32(byte[] data, ref byte ix)
-    {
+    public static Quat32 ReceiveQuat32(byte[] data, ref byte ix) {
         Quat32 q = new(
             (data[ix++] - 128.0F) / 127.0F,
             (data[ix++] - 128.0F) / 127.0F,
@@ -48,8 +48,14 @@ public class LowLevelMessages
         return q;
     }
 
-    public static void SendAngle8(byte[] buffer, ref byte ix, float angle)
-    {
+    public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix) {
+        Quat32 q32 = ReceiveQuat32(data, ref ix);
+        UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w);
+        SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z);
+        return r;
+    }
+
+    public static void SendAngle8(byte[] buffer, ref byte ix, float angle) {
         // Normalize angle
         while (angle >= 180)
             angle -= 360;
@@ -58,8 +64,7 @@ public class LowLevelMessages
         buffer[ix++] = (byte)((angle / 360.0f) * 256.0f);
     }
 
-    public static float ReceiveAngle8(byte[] data, ref byte ix)
-    {
+    public static float ReceiveAngle8(byte[] data, ref byte ix) {
         float value = (data[ix++] * 180) / 128.0F;
         return value;
     }
@@ -70,15 +75,13 @@ public class LowLevelMessages
         data[ix++] = (byte)(binary >> 8);
         data[ix++] = (byte)(binary & 255);
     }
-    public static void SendFloat16(byte[] data, ref byte ix, float16 f)
-    {
+    public static void SendFloat16(byte[] data, ref byte ix, float16 f) {
         ushort binary = f.GetBinary();
         data[ix++] = (byte)(binary >> 8);
         data[ix++] = (byte)(binary & 255);
     }
 
-    public static float ReceiveFloat16(byte[] data, ref byte ix)
-    {
+    public static float ReceiveFloat16(byte[] data, ref byte ix) {
         ushort value = (ushort)(data[ix++] << 8 | data[ix++]);
         float16 f16 = new();
         f16.SetBinary(value);

Messages.cs

@@ -2,7 +2,7 @@ using System.IO;
 using System.Threading.Tasks;
 
 using Passer.LinearAlgebra;
-namespace Passer.Control {
+namespace Passer.Control.Core {
 
     public class IMessage {
         public IMessage() { }
@@ -15,17 +15,17 @@ namespace Passer.Control {
 
         public bool SendTo(Participant client) {
             Serialize(ref client.buffer);
-            return client.SendBuffer();
+            return client.SendBuffer(client.buffer.Length);
         }
 
         public static bool SendMsg(Participant client, IMessage msg) {
             msg.Serialize(ref client.buffer);
-            return client.SendBuffer();
+            return client.SendBuffer(client.buffer.Length);
         }
 
         public static bool PublishMsg(Participant client, IMessage msg) {
             msg.Serialize(ref client.buffer);
-            return client.PublishBuffer();
+            return client.PublishBuffer(client.buffer.Length);
         }
 
         public static async Task<byte[]> Receive(Stream dataStream, byte packetSize) {
@@ -68,10 +68,10 @@ namespace Passer.Control {
             ClientMsg msg = new(networkId);
             return SendMsg(client, msg);
         }
-        public static bool Publish(Participant client, byte networkId) {
-            ClientMsg msg = new(networkId);
-            return PublishMsg(client, msg);
-        }
+        //public static bool Publish(Participant client, byte networkId) {
+        //    ClientMsg msg = new(networkId);
+        //    return PublishMsg(client, msg);
+        //}
         public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
             if (packetSize != length)
                 return false;
@@ -80,17 +80,38 @@ namespace Passer.Control {
             ClientMsg msg = new(buffer);
 
             if (client.networkId == 0) {
-                client.networkId = (byte)(Participant.clients.Count);
-                NetworkIdMsg.Send(client, client.networkId);
+                //    client.networkId = (byte)(Participant.clients.Count);
+                //    NetworkIdMsg.Send(client, client.networkId);
                 client.messageQueue.Enqueue(msg);
             }
             else if (msg.networkId == 0) {
-                NetworkIdMsg.Send(client, client.networkId);
+                //    NetworkIdMsg.Send(client, client.networkId);
                 client.messageQueue.Enqueue(msg);
             }
 
             return true;
         }
+
+        public static bool SendTo(Participant participant, byte networkId) {
+            if (participant == null)
+                return false;
+
+            byte ix = 0;
+            participant.buffer[ix++] = ClientMsg.Id;
+            participant.buffer[ix++] = networkId;
+
+            return participant.SendBuffer(ix);
+        }
+        public static bool Publish(Participant participant, byte networkId) {
+            if (participant == null)
+                return false;
+
+            byte ix = 0;
+            participant.buffer[ix++] = ClientMsg.Id;
+            participant.buffer[ix++] = networkId;
+
+            return participant.PublishBuffer(ix);
+        }
     }
 
     #endregion Client
@@ -131,6 +152,18 @@ namespace Passer.Control {
             client.messageQueue.Enqueue(msg);
             return true;
         }
+
+        public static bool SendTo(Participant participant, byte networkId) {
+            if (participant == null)
+                return false;
+
+            byte ix = 0;
+            participant.buffer[ix++] = NetworkIdMsg.Id;
+            participant.buffer[ix++] = networkId;
+
+            return participant.SendBuffer(ix);
+
+        }
     }
 
     #endregion Network Id
@@ -186,7 +219,7 @@ namespace Passer.Control {
 
     public class ThingMsg : IMessage {
         public const byte length = 5;
-        public const byte Id = 0x80;
+        public const byte id = 0x80;
         public byte networkId;
         public byte thingId;
         public byte thingType;
@@ -202,7 +235,7 @@ namespace Passer.Control {
 
         public override byte Serialize(ref byte[] buffer) {
             byte ix = 0;
-            buffer[ix++] = ThingMsg.Id;
+            buffer[ix++] = ThingMsg.id;
             buffer[ix++] = this.networkId;
             buffer[ix++] = this.thingId;
             buffer[ix++] = this.thingType;
@@ -217,7 +250,7 @@ namespace Passer.Control {
             this.parentId = buffer[ix];
         }
 
-        public static bool Send(Participant client, CoreThing thing) {
+        public static bool Send(Participant client, Thing thing) {
             ThingMsg msg = new(thing.networkId, thing.id, thing.type, thing.parent.id);
             return SendMsg(client, msg);
         }
@@ -242,6 +275,23 @@ namespace Passer.Control {
             client.messageQueue.Enqueue(msg);
             return true;
         }
+
+        public static bool SendTo(Participant participant, Thing thing) {
+            if (participant == null || thing == null)
+                return false;
+
+            byte ix = 0;
+            participant.buffer[ix++] = ThingMsg.id;
+            participant.buffer[ix++] = participant.networkId;
+            participant.buffer[ix++] = thing.id;
+            participant.buffer[ix++] = thing.type;
+            if (thing.parent != null)
+                participant.buffer[ix++] = thing.parent.id;
+            else
+                participant.buffer[ix++] = 0;
+
+            return participant.SendBuffer(ix);
+        }
     }
 
     #endregion Thing
@@ -281,7 +331,7 @@ namespace Passer.Control {
             this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
         }
 
-        public static bool Send(Participant client, CoreThing thing) {
+        public static bool Send(Participant client, Thing thing) {
             if (string.IsNullOrEmpty(thing.name))
                 return true; // nothing sent, but still a success!
 
@@ -300,6 +350,25 @@ namespace Passer.Control {
             client.messageQueue.Enqueue(msg);
             return true;
         }
+
+        public static bool SendTo(Participant participant, Thing thing) {
+            if (participant == null || thing == null || thing.name == null)
+                return false;
+
+            byte nameLength = (byte)thing.name.Length;
+            if (participant.buffer.Length < 3 + nameLength)
+                return false;
+
+            byte ix = 0;
+            participant.buffer[ix++] = NameMsg.Id;
+            participant.buffer[ix++] = participant.networkId;
+            participant.buffer[ix++] = thing.id;
+            participant.buffer[ix++] = nameLength;
+            for (int nameIx = 0; nameIx < nameLength; nameIx++)
+                participant.buffer[ix++] = (byte)thing.name[nameIx];
+
+            return participant.SendBuffer(ix);
+        }
     }
 
     #endregion
@@ -344,7 +413,7 @@ namespace Passer.Control {
             url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
         }
 
-        public static bool Send(Participant client, CoreThing thing) {
+        public static bool Send(Participant client, Thing thing) {
             if (string.IsNullOrEmpty(thing.modelUrl))
                 return true; // nothing sent, but still a success!
 
@@ -364,6 +433,27 @@ namespace Passer.Control {
             client.messageQueue.Enqueue(msg);
             return true;
         }
+
+        public static bool SendTo(Participant participant, Thing thing) {
+            if (participant == null || thing == null || thing.modelUrl == null)
+                return false;
+
+            byte urlLength = (byte)thing.modelUrl.Length;
+            if (participant.buffer.Length < 3 + urlLength)
+                return false;
+
+            byte ix = 0;
+            participant.buffer[ix++] = ModelUrlMsg.Id;
+            participant.buffer[ix++] = participant.networkId;
+            participant.buffer[ix++] = thing.id; // Thing Id
+            LowLevelMessages.SendFloat16(participant.buffer, ref ix, new float16(1.0f));
+
+            participant.buffer[ix++] = urlLength;
+            for (int urlIx = 0; urlIx < urlLength; urlIx++)
+                participant.buffer[ix++] = (byte)thing.modelUrl[urlIx];
+
+            return participant.SendBuffer(ix);
+        }
     }
 
     #endregion Model URL
@@ -383,11 +473,11 @@ namespace Passer.Control {
         public const byte Pose_AngularVelocity = 0x08;
 
         public Spherical position;
-        public Quat32 orientation;
+        public SwingTwist orientation;
         public Spherical linearVelocity;
         public Spherical angularVelocity;
 
-        public PoseMsg(byte networkId, byte thingId, Spherical position, Quat32 orientation) {
+        public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation) {
             this.networkId = networkId;
             this.thingId = thingId;
             this.position = position;
@@ -400,7 +490,7 @@ namespace Passer.Control {
             if (this.orientation != null)
                 this.poseType |= Pose_Orientation;
             else
-                this.orientation = new Quat32(0, 0, 0, 1);
+                this.orientation = SwingTwist.zero;
         }
         public PoseMsg(byte[] buffer) : base(buffer) { }
 
@@ -411,8 +501,14 @@ namespace Passer.Control {
             buffer[ix++] = this.thingId;
             buffer[ix++] = this.poseType;
 
-            LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
-            LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation);
+            if ((poseType & Pose_Position) != 0)
+                LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
+            if ((poseType & Pose_Orientation) != 0)
+                LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation);
+            if ((poseType & Pose_LinearVelocity) != 0)
+                LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity);
+            if ((poseType & Pose_AngularVelocity) != 0)
+                LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity);
             return ix;
         }
         public override void Deserialize(byte[] buffer) {
@@ -424,14 +520,14 @@ namespace Passer.Control {
             if ((poseType & Pose_Position) != 0)
                 this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
             if ((poseType & Pose_Orientation) != 0)
-                this.orientation = LowLevelMessages.ReceiveQuat32(buffer, ref ix);
+                this.orientation = LowLevelMessages.ReceiveSwingTwist(buffer, ref ix);
             if ((poseType & Pose_LinearVelocity) != 0)
                 this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
             if ((poseType & Pose_AngularVelocity) != 0)
                 this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
         }
 
-        public static bool Send(Participant client, byte thingId, Spherical position, Quat32 orientation) {
+        public static bool Send(Participant client, byte thingId, Spherical position, SwingTwist orientation) {
             PoseMsg msg = new(client.networkId, thingId, position, orientation);
             return SendMsg(client, msg);
         }
@@ -446,6 +542,28 @@ namespace Passer.Control {
             client.messageQueue.Enqueue(msg);
             return true;
         }
+
+        public static bool SendTo(Participant participant, Thing thing) {
+            if (participant == null || thing == null)
+                return false;
+
+            byte ix = 0;
+            participant.buffer[ix++] = PoseMsg.Id;
+            participant.buffer[ix++] = participant.networkId;
+            participant.buffer[ix++] = thing.id;
+            participant.buffer[ix++] = thing.poseUpdated;
+
+            if ((thing.poseUpdated & Pose_Position) != 0 && thing.position != null)
+                LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.position);
+            if ((thing.poseUpdated & Pose_Orientation) != 0 && thing.orientation != null)
+                LowLevelMessages.SendQuat32(participant.buffer, ref ix, thing.orientation);
+            if ((thing.poseUpdated & Pose_LinearVelocity) != 0 && thing.linearVelocity != null)
+                LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.linearVelocity);
+            if ((thing.poseUpdated & Pose_AngularVelocity) != 0 && thing.angularVelocity != null)
+                LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.angularVelocity);
+
+            return participant.SendBuffer(ix);
+        }
     }
 
     #endregion Pose

Participant.cs

@@ -1,14 +1,17 @@
 using System.Collections.Generic;
 using System.Collections.Concurrent;
+using System.Net;
 using System.Net.Sockets;
 using System.IO;
+using System.Threading.Tasks;
 
-namespace Passer.Control
+namespace Passer.Control.Core
 {
 
     public class Participant
     {
         //public ConnectionMethod connection;
+        public IPEndPoint endPoint;
         public UdpClient udpClient;
         public string ipAddress;
         public string broadcastIpAddress = "255.255.255.255";
@@ -21,45 +24,147 @@ namespace Passer.Control
 
         public readonly ConcurrentQueue<IMessage> messageQueue = new();
 
-        public static Participant GetClient(string ipAddress, int port)
+        public Participant GetClient(string ipAddress, int port)
         {
-            foreach (Participant c in clients)
+            foreach (Participant c in others)
             {
                 if (c.ipAddress == ipAddress && c.port == port)
                     return c;
             }
             return null;
         }
-        static public List<Participant> clients = new List<Participant>();
+        public List<Participant> others = new List<Participant>();
 
-        public Participant(UdpClient udpClient, int port)
+        #region Init
+
+        public Participant() {
+            this.dataStream = new EchoStream();
+            others.Add(this);
+        }
+
+        public Participant(UdpClient udpClient, int port) : this()
         {
             this.udpClient = udpClient;
             this.ipAddress = null;
             this.port = port;
-            this.dataStream = new EchoStream();
-            clients.Add(this);
+            //this.dataStream = new EchoStream();
+            //clients.Add(this);
         }
 
-        public bool SendBuffer()
-        {
-            if (this.ipAddress == null)
-                return false;
+        #endregion Init
 
-            //UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}");
-            this.udpClient.Send(this.buffer, this.buffer.Length, this.ipAddress, this.port);
+        #region Update
+
+        private static readonly float publishInterval = 3.0f;
+        private float nextPublishMe = 0;
+
+
+        public virtual void Update(float currentTime) {
+            if (currentTime > this.nextPublishMe) {
+                ClientMsg.Publish(this, this.networkId);
+                this.nextPublishMe = currentTime + Participant.publishInterval;
+            }
+
+            for (int ix = 0; ix < this.others.Count; ix++) {
+                Participant client = this.others[ix];
+                if (client == null)
+                    continue;
+
+                client.ProcessMessages();
+            }
+            Thing.UpdateAll(currentTime);
+        }
+
+        #endregion Update
+
+        #region Send
+
+        public bool SendBuffer(int bufferSize)
+        {
+            //if (this.ipAddress == null)
+            //    return false;
+
+            UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}");
+            //this.udpClient.Send(this.buffer, bufferSize, this.ipAddress, this.port);
+            this.udpClient.Send(this.buffer, bufferSize, this.endPoint);
             return true;
         }
 
-        public bool PublishBuffer()
+        public bool PublishBuffer(int bufferSize)
         {
             if (this.broadcastIpAddress == null)
                 return false;
 
-            this.udpClient.Send(this.buffer, this.buffer.Length, this.broadcastIpAddress, this.port);
+            this.udpClient.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port);
             return true;
         }
 
+        #endregion
+
+        #region Receive
+
+        public async Task ReceiveData() {
+            while (true) {
+                byte packetSize = (byte)this.dataStream.ReadByte();
+                if (packetSize != 0xFF)
+                    await ReceiveData(this.dataStream, this, packetSize);
+                // else timeout
+            }
+        }
+
+        public static async Task ReceiveData(Stream dataStream, Participant client, byte packetSize) {
+            byte msgId = (byte)dataStream.ReadByte();
+            if (msgId == 0xFF) {
+                // Timeout
+                return;
+            }
+
+            //UnityEngine.Debug.Log($"R {msgId} from {client.ipAddress}");
+            bool result = false;
+            switch (msgId) {
+                case ClientMsg.Id: // 0xA0 / 160
+                    result = await ClientMsg.Receive(dataStream, client, packetSize);
+                    break;
+                case NetworkIdMsg.Id: // 0xA1 / 161
+                    result = await NetworkIdMsg.Receive(dataStream, client, packetSize);
+                    break;
+                case InvestigateMsg.Id: // 0x81
+                    result = await InvestigateMsg.Receive(dataStream, client, packetSize);
+                    break;
+                case ThingMsg.id: // 0x80 / 128
+                    result = await ThingMsg.Receive(dataStream, client, packetSize);
+                    break;
+                case NameMsg.Id: // 0x91 / 145
+                    result = await NameMsg.Receive(dataStream, client, packetSize);
+                    break;
+                case ModelUrlMsg.Id: // 0x90 / 144
+                    result = await ModelUrlMsg.Receive(dataStream, client, packetSize);
+                    break;
+                case PoseMsg.Id: // 0x10 / 16
+                    result = await PoseMsg.Receive(dataStream, client, packetSize);
+                    break;
+                case CustomMsg.Id: // 0xB1 / 177
+                    result = await CustomMsg.Receive(dataStream, client, packetSize);
+                    break;
+                case TextMsg.Id: // 0xB0 / 176
+                    result = await TextMsg.Receive(dataStream, client, packetSize);
+                    break;
+                case DestroyMsg.Id: // 0x20 / 32
+                    result = await DestroyMsg.Receive(dataStream, client, packetSize);
+                    break;
+                default:
+                    break;
+            }
+            if (result == false) {
+                packetSize = msgId; // skip 1 byte, msgId is possibly a packet size byte
+                await ReceiveData(dataStream, client, packetSize);
+            }
+        }
+
+        #endregion
+
+        #region Process
+
         public virtual void ProcessMessages()
         {
             while (this.messageQueue.TryDequeue(out IMessage msg))
@@ -129,7 +234,7 @@ namespace Passer.Control
 
         private void ForwardMessage(IMessage msg)
         {
-            foreach (Participant client in Participant.clients)
+            foreach (Participant client in others)
             {
                 if (client == this)
                     continue;
@@ -137,5 +242,7 @@ namespace Passer.Control
                 IMessage.SendMsg(client, msg);
             }
         }
+
+        #endregion
     }
 }

SiteServer.cs

@@ -1,7 +1,7 @@
 using System.IO;
 using System.Threading.Tasks;
 
-namespace Passer.Control {
+namespace Passer.Control.Core {
 
     public static class SiteServer {
 
@@ -33,7 +33,7 @@ namespace Passer.Control {
                 case InvestigateMsg.Id: // 0x81
                     result = await InvestigateMsg.Receive(dataStream, client, packetSize);
                     break;
-                case ThingMsg.Id: // 0x80 / 128
+                case ThingMsg.id: // 0x80 / 128
                     result = await ThingMsg.Receive(dataStream, client, packetSize);
                     break;
                 case NameMsg.Id: // 0x91 / 145

Thing.cs

@@ -1,43 +1,229 @@
 using System.Collections.Generic;
+using Passer.LinearAlgebra;
 
-namespace Passer.Control
+namespace Passer.Control.Core
 {
+
     /// <summary>
     /// A thing is the basic building block
     /// </summary>
-    public class CoreThing
+    public class Thing
     {
         public Participant participant;
+
+        public delegate void ChangeHandler();
+        public delegate void SphericalHandler(Spherical v);
+
         public byte networkId;
         public byte id;
-        public CoreThing parent;
-        public List<CoreThing> children;
-        public byte type;
-        public string name;
-        public string modelUrl;
-        //protected Sensor sensor;
 
-        protected virtual void Init()
+        public event ChangeHandler OnParentChanged;
+        private Thing _parent;
+        public Thing parent
         {
+            get => _parent;
+            set
+            {
+                if (_parent == value)
+                    return;
+
+                if (value == null)
+                {
+                    _parent.RemoveChild(this);
+                    _parent = null;
+                }
+                else
+                {
+                    value.AddChild(this);
+                    OnParentChanged?.Invoke();
+                }
+            }
         }
 
-        public CoreThing(Participant client, byte networkId, byte thingId, byte thingType = 0)
+        public void AddChild(Thing child)
+        {
+            if (children.Find(thing => thing == child) != null)
+                return;
+            child._parent = this;
+            children.Add(child);
+        }
+        public void RemoveChild(Thing child)
+        {
+            children.Remove(child);
+        }
+
+        [System.NonSerialized]
+        public List<Thing> children = new List<Thing>();
+        public byte type;
+
+        public event ChangeHandler OnNameChanged;
+        private string _name;
+        public string name
+        {
+            get => _name;
+            set
+            {
+                if (_name != value)
+                {
+                    _name = value;
+                    OnNameChanged?.Invoke();
+                }
+            }
+        }
+
+        public string modelUrl;
+
+        public byte poseUpdated = 0x00;
+        public event ChangeHandler OnPositionChanged;
+        private Spherical _position;
+        public Spherical position
+        {
+            get { return _position; }
+            set
+            {
+                if (_position != value)
+                {
+                    _position = value;
+                    OnPositionChanged?.Invoke();
+                }
+            }
+        }
+
+        public event ChangeHandler OnOrientationChanged;
+        private SwingTwist _orientation;
+        public SwingTwist orientation
+        {
+            get { return _orientation; }
+            set
+            {
+                if (_orientation != value)
+                {
+                    _orientation = value;
+                    OnOrientationChanged?.Invoke();
+                }
+            }
+        }
+
+        public event SphericalHandler OnLinearVelocityChanged;
+        private Spherical _linearVelocity;
+        public Spherical linearVelocity
+        {
+            get => _linearVelocity;
+            set
+            {
+                if (_linearVelocity != value)
+                {
+                    _linearVelocity = value;
+                    OnLinearVelocityChanged?.Invoke(_linearVelocity);
+                }
+            }
+        }
+        public Spherical angularVelocity;
+
+        public virtual void Init(bool invokeEvent = true)
+        {
+            Thing.Add(this, invokeEvent);
+        }
+
+        public Thing(bool initialize = true)
+        {
+            if (initialize)
+            {
+                //this.Init();
+                Thing.Add(this);
+            }
+        }
+        public Thing(Participant client, byte networkId, byte thingId, byte thingType = 0)
         {
             this.participant = client;
             this.id = thingId;
             this.type = thingType;
             this.networkId = networkId;
             this.Init();
-            allThings.Add(this);
+            Thing.Add(this);
         }
 
-        private static readonly List<CoreThing> allThings = new();
-
-        public static CoreThing Get(byte networkId, byte thingId)
+        public virtual void Update(float currentTime)
         {
-            CoreThing thing = allThings.Find(aThing => aThing.networkId == networkId && aThing.id == thingId);
+            // should recurse over children...
+        }
+
+        public virtual void ProcessBytes(byte[] bytes)
+        {
+            //if (sensor != null)
+            //    sensor.ProcessBytes(bytes);
+        }
+
+        // Experimental
+
+        public float stressLevel = 0;
+
+        protected delegate void ReceptorFunc(Sensor sensor);
+        protected void SetupReceptor(Sensor sensor, ReceptorFunc receptor)
+        {
+            sensor.Signaller += (sensor => Receptor(receptor, sensor));
+        }
+        protected void Receptor(ReceptorFunc receptor, Sensor sensor)
+        {
+            if (sensor.signalStrength <= stressLevel)
+                return;
+
+            receptor(sensor);
+        }
+
+        //---------- All Things
+
+        private static readonly List<Thing> allThings = new();
+
+        public delegate void ThingHandler(Thing t);
+        public static event ThingHandler OnNewThing;
+
+        public static void Add(Thing thing, bool invokeEvent = true)
+        {
+            Thing foundThing = Get(thing.networkId, thing.id);
+
+            if (foundThing == null)
+            {
+                if (thing.id == 0)
+                    thing.id = (byte)(allThings.Count + 1);
+                allThings.Add(thing);
+                if (invokeEvent)
+                    OnNewThing?.Invoke(thing);
+                UnityEngine.Debug.Log($"Add thing [{thing.networkId}/{thing.id}] {thing.name}");
+            }
+        }
+
+        public static Thing Get(byte networkId, byte thingId)
+        {
+            Thing thing = allThings.Find(aThing => IsThing(aThing, networkId, thingId));
             return thing;
         }
 
+        private static bool IsThing(Thing thing, byte networkId, byte thingId)
+        {
+            if (thing == null)
+                return false;
+            return (thing.networkId == networkId) && (thing.id == thingId);
+        }
+
+        public static void Remove(byte networkId, byte thingId)
+        {
+            allThings.RemoveAll(t => t.networkId == networkId && t.id == thingId);
+        }
+
+        public static Thing[] GetAllThings()
+        {
+            return allThings.ToArray();
+        }
+
+        public static void UpdateAll(float currentTime)
+        {
+            foreach (Thing thing in allThings)
+            {
+                if (thing.parent == null) // update only root things
+                    thing.Update(currentTime);
+            }
+        }
+
     }
 }

Thing.cs.meta

@@ -1,5 +1,5 @@
 fileFormatVersion: 2
-guid: 0e8b48bc91446304eaaccbfdde4cc4af
+guid: 7429282ee0e367445bd1e2111631b27d
 MonoImporter:
   externalObjects: {}
   serializedVersion: 2