Cleanup

Messages/ClientMsg.cs

@@ -0,0 +1,23 @@
+namespace Passer.Control.Core {
+
+    public class ClientMsg : IMessage {
+        public const byte Id = 0xA0;
+        public const byte length = 2;
+        public byte networkId;
+
+        public ClientMsg(byte networkId) {
+            this.networkId = networkId;
+        }
+
+        public ClientMsg(byte[] buffer) {
+            this.networkId = buffer[1];
+        }
+
+        public override byte Serialize(ref byte[] buffer) {
+            byte ix = 0;
+            buffer[ix++] = ClientMsg.Id;
+            buffer[ix++] = networkId;
+            return ClientMsg.length;
+        }
+    }
+}

Messages/CustomMsg.cs

@@ -0,0 +1,45 @@
+namespace Passer.Control.Core {
+
+    public class CustomMsg : IMessage {
+        public const byte Id = 0xB1;
+        public byte networkId;
+        public byte thingId;
+        public byte[] bytes;
+
+        public CustomMsg(byte[] buffer) {
+            byte ix = 1;
+            this.networkId = buffer[ix++];
+            this.thingId = buffer[ix++];
+            byte length = (byte)(buffer.Length - ix);
+            this.bytes = new byte[length];
+            for (uint bytesIx = 0; bytesIx < length; bytesIx++)
+                this.bytes[bytesIx] = buffer[ix++];
+        }
+        public CustomMsg(byte networkId, byte thingId, byte[] bytes) : base() {
+            this.networkId = networkId;
+            this.thingId = thingId;
+            this.bytes = bytes;
+        }
+
+        public override byte Serialize(ref byte[] buffer) {
+            byte ix = 0;
+            buffer[ix++] = CustomMsg.Id;
+            buffer[ix++] = this.networkId;
+            buffer[ix++] = this.thingId;
+            //buffer[ix++] = (byte)bytes.Length;
+            foreach (byte b in bytes)
+                buffer[ix++] = b;
+
+            return ix;
+        }
+
+        //public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
+        //    byte[] buffer = await Receive(dataStream, packetSize);
+
+        //    CustomMsg msg = new(buffer);
+        //    client.messageQueue.Enqueue(msg);
+        //    return true;
+        //}
+    }
+
+}

Messages/LowLevelMessages.cs

@@ -0,0 +1,109 @@
+using Passer.LinearAlgebra;
+
+namespace Passer.Control.Core
+{
+    public class LowLevelMessages
+    {
+
+        public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v)
+        {
+            SendFloat16(buffer, ref ix, new float16(v.distance));
+            SendAngle8(buffer, ref ix, v.direction.horizontal);
+            SendAngle8(buffer, ref ix, v.direction.horizontal);
+        }
+
+        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);
+            Spherical v = new(distance, horizontal, vertical);
+            return v;
+        }
+
+        public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s)
+        {
+            Quat32 q32 = Quat32.FromSwingTwist(s);
+            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)
+            {
+                qx = -qx;
+                qy = -qy;
+                qz = -qz;
+                qw = -qw;
+            }
+
+            buffer[ix++] = (byte)qx;
+            buffer[ix++] = (byte)qy;
+            buffer[ix++] = (byte)qz;
+            buffer[ix++] = (byte)qw;
+        }
+        public static Quat32 ReceiveQuat32(byte[] data, ref byte ix)
+        {
+            Quat32 q = new(
+                (data[ix++] - 128.0F) / 127.0F,
+                (data[ix++] - 128.0F) / 127.0F,
+                (data[ix++] - 128.0F) / 127.0F,
+                data[ix++] / 255.0F);
+            return q;
+        }
+
+        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);
+
+            SwingTwist r = SwingTwist.FromQuat32(q32);
+            return r;
+        }
+
+        public static void SendAngle8(byte[] buffer, ref byte ix, float angle)
+        {
+            // Normalize angle
+            while (angle >= 180)
+                angle -= 360;
+            while (angle < -180)
+                angle += 360;
+            buffer[ix++] = (byte)((angle / 360.0f) * 256.0f);
+        }
+
+        public static float ReceiveAngle8(byte[] data, ref byte ix)
+        {
+            float value = (data[ix++] * 180) / 128.0F;
+            return value;
+        }
+
+        public static void SendFloat16(byte[] data, ref byte ix, float f)
+        {
+            float16 f16 = new(f);
+            ushort binary = f16.GetBinary();
+            data[ix++] = (byte)(binary >> 8);
+            data[ix++] = (byte)(binary & 255);
+        }
+        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)
+        {
+            ushort value = (ushort)(data[ix++] << 8 | data[ix++]);
+            float16 f16 = new();
+            f16.SetBinary(value);
+            float f = f16.toFloat();
+            return f;
+        }
+    }
+
+}

Messages/Messages.cs

@@ -0,0 +1,253 @@
+using System.IO;
+using System.Threading.Tasks;
+
+using Passer.LinearAlgebra;
+namespace Passer.Control.Core {
+
+    public class IMessage {
+        public IMessage() { }
+        public IMessage(byte[] buffer) {
+            Deserialize(buffer);
+        }
+
+        public virtual byte Serialize(ref byte[] buffer) { return 0; }
+        public virtual void Deserialize(byte[] buffer) { }
+
+        public bool SendTo(Participant client) {
+            Serialize(ref client.buffer);
+            return client.SendBuffer(client.buffer.Length);
+        }
+
+        public static bool SendMsg(Participant client, IMessage msg) {
+            msg.Serialize(ref client.buffer);
+            return client.SendBuffer(client.buffer.Length);
+        }
+
+        public static bool PublishMsg(Participant client, IMessage msg) {
+            msg.Serialize(ref client.buffer);
+            return client.PublishBuffer(client.buffer.Length);
+        }
+
+        public static async Task<byte[]> Receive(Stream dataStream, byte packetSize) {
+            byte[] buffer = new byte[packetSize - 1]; //  without msgId
+            int byteCount = dataStream.Read(buffer, 0, packetSize - 1);
+            while (byteCount < packetSize - 1) {
+                // not all bytes have been read, wait and try again
+                await Task.Delay(1);
+                byteCount += dataStream.Read(buffer, byteCount, packetSize - 1 - byteCount);
+            }
+            return buffer;
+        }
+    }
+
+    #region Investigate
+
+    public class InvestigateMsg : IMessage {
+        public const byte Id = 0x81;
+        public const byte length = 3;
+        public byte networkId;
+        public byte thingId;
+
+        public InvestigateMsg(byte networkId, byte thingId) {
+            this.networkId = networkId;
+            this.thingId = thingId;
+        }
+        public InvestigateMsg(byte[] buffer) : base(buffer) { }
+
+        public override byte Serialize(ref byte[] buffer) {
+            byte ix = 0;
+            buffer[ix++] = InvestigateMsg.Id;
+            buffer[ix++] = this.networkId;
+            buffer[ix++] = this.thingId;
+            return ix;
+        }
+        public override void Deserialize(byte[] buffer) {
+            uint ix = 0;
+            this.networkId = buffer[ix++];
+            this.thingId = buffer[ix++];
+        }
+
+        //public static bool Send(Participant client, CoreThing thing) {
+        //    InvestigateMsg msg = new(thing.networkId, thing.id);
+        //    return SendMsg(client, msg);
+        //}
+        public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
+            if (packetSize != length)
+                return false;
+
+            byte[] buffer = await Receive(dataStream, packetSize);
+            InvestigateMsg msg = new(buffer);
+            //UnityEngine.Debug.Log($"Receive investigate [{msg.networkId}/{msg.thingId}]");
+
+            client.messageQueue.Enqueue(msg);
+            return true;
+
+        }
+    }
+
+    #endregion Investigate
+
+    #region Pose
+
+    public class PoseMsg : IMessage {
+        public const byte Id = 0x10;
+        public const byte length = 4 + 4 + 4;
+        public byte networkId;
+        public byte thingId;
+
+        public byte poseType;
+        public const byte Pose_Position = 0x01;
+        public const byte Pose_Orientation = 0x02;
+        public const byte Pose_LinearVelocity = 0x04;
+        public const byte Pose_AngularVelocity = 0x08;
+
+        public Spherical position;
+        public SwingTwist orientation;
+        public Spherical linearVelocity;
+        public Spherical angularVelocity;
+
+        public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation) {
+            this.networkId = networkId;
+            this.thingId = thingId;
+            this.position = position;
+            this.orientation = orientation;
+            this.poseType = 0;
+            if (this.position != null)
+                this.poseType |= Pose_Position;
+            else
+                this.position = new Spherical(0, 0, 0);
+            if (this.orientation != null)
+                this.poseType |= Pose_Orientation;
+            else
+                this.orientation = SwingTwist.zero;
+        }
+        public PoseMsg(byte[] buffer) : base(buffer) { }
+
+        public override byte Serialize(ref byte[] buffer) {
+            byte ix = 0;
+            buffer[ix++] = PoseMsg.Id;
+            buffer[ix++] = this.networkId;
+            buffer[ix++] = this.thingId;
+            buffer[ix++] = this.poseType;
+
+            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) {
+            byte ix = 0;
+            this.networkId = buffer[ix++];
+            this.thingId = buffer[ix++];
+            this.poseType = buffer[ix++];
+
+            if ((poseType & Pose_Position) != 0)
+                this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
+            if ((poseType & Pose_Orientation) != 0)
+                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, SwingTwist orientation) {
+            PoseMsg msg = new(client.networkId, thingId, position, orientation);
+            return SendMsg(client, msg);
+        }
+        public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
+            byte[] buffer = await Receive(dataStream, packetSize);
+            PoseMsg msg = new(buffer);
+
+            // Do no process poses with nwid == 0 (== local)
+            if (msg.networkId == 0)
+                return true;
+
+            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
+
+    #region Text
+
+    public class TextMsg : IMessage {
+        public const byte Id = 0xB0;
+        public string text;
+
+        public TextMsg(byte[] buffer) : base(buffer) { }
+        public override void Deserialize(byte[] buffer) {
+            uint ix = 0;
+            uint strlen = buffer[ix++];
+            this.text = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, (int)strlen);
+        }
+
+        public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
+            byte[] buffer = await Receive(dataStream, packetSize);
+            TextMsg msg = new(buffer);
+
+            client.messageQueue.Enqueue(msg);
+            return true;
+        }
+    }
+
+    #endregion
+
+    #region Destroy
+
+    public class DestroyMsg : IMessage {
+        public const byte Id = 0x20;
+        public const byte length = 3;
+        public byte networkId;
+        public byte thingId;
+
+        public DestroyMsg(byte[] buffer) : base(buffer) { }
+
+        public override void Deserialize(byte[] buffer) {
+            this.networkId = buffer[0];
+            this.thingId = buffer[1];
+        }
+
+        public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
+            if (packetSize != length)
+                return false;
+
+            byte[] buffer = await Receive(dataStream, packetSize);
+            DestroyMsg msg = new(buffer);
+
+            client.messageQueue.Enqueue(msg);
+            return true;
+        }
+    }
+
+
+    #endregion Destroy
+}

Messages/NameMsg.cs

@@ -0,0 +1,49 @@
+#nullable enable
+
+namespace Passer.Control.Core {
+
+    public class NameMsg : IMessage {
+        public const byte Id = 0x91; // 145
+        public const byte length = 4;
+        public byte networkId;
+        public byte thingId;
+        public byte len;
+        public string? name = null;
+
+        public NameMsg(byte networkId, Thing thing) {
+            this.networkId = networkId;
+            this.thingId = thing.id;
+            this.name = thing.name;
+        }
+        public NameMsg(byte networkId, byte thingId, string name) {
+            this.networkId = networkId;
+            this.thingId = thingId;
+            this.name = name;
+        }
+        public NameMsg(byte[] buffer) {
+            byte ix = 1;
+            this.networkId = buffer[ix++];
+            this.thingId = buffer[ix++];
+            int strlen = buffer[ix++];
+            this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
+        }
+
+        public override byte Serialize(ref byte[] buffer) {
+            if (this.name == null)
+                return 0;
+
+            byte ix = 0;
+            buffer[ix++] = NameMsg.Id;
+            buffer[ix++] = this.networkId;
+            buffer[ix++] = this.thingId;
+
+            int nameLength = this.name.Length;
+
+            buffer[ix++] = (byte)nameLength;
+            for (int nameIx = 0; nameIx < nameLength; nameIx++)
+                buffer[ix++] = (byte)this.name[nameIx];
+            return ix;
+        }
+    }
+
+}

Messages/NetworkIdMsg.cs

@@ -0,0 +1,22 @@
+namespace Passer.Control.Core {
+
+    public class NetworkIdMsg : IMessage {
+        public const byte Id = 0xA1;
+        public const byte length = 2;
+        public byte networkId;
+
+        public NetworkIdMsg(byte networkId) {
+            this.networkId = networkId;
+        }
+        public NetworkIdMsg(byte[] buffer) {
+            this.networkId = buffer[1];
+        }
+
+        public override byte Serialize(ref byte[] buffer) {
+            buffer[0] = NetworkIdMsg.Id;
+            buffer[1] = this.networkId;
+            return NetworkIdMsg.length;
+        }
+    }
+
+}