From e562e2433cd4010e90f8363139f40f39147e38bf Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 14 Jan 2025 09:10:55 +0100 Subject: [PATCH] Cleanup --- Messages/ClientMsg.cs | 23 ++++ Messages/CustomMsg.cs | 45 +++++++ Messages/LowLevelMessages.cs | 109 +++++++++++++++ Messages/Messages.cs | 253 +++++++++++++++++++++++++++++++++++ Messages/NameMsg.cs | 49 +++++++ Messages/NetworkIdMsg.cs | 22 +++ 6 files changed, 501 insertions(+) create mode 100644 Messages/ClientMsg.cs create mode 100644 Messages/CustomMsg.cs create mode 100644 Messages/LowLevelMessages.cs create mode 100644 Messages/Messages.cs create mode 100644 Messages/NameMsg.cs create mode 100644 Messages/NetworkIdMsg.cs diff --git a/Messages/ClientMsg.cs b/Messages/ClientMsg.cs new file mode 100644 index 0000000..d924b17 --- /dev/null +++ b/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; + } + } +} \ No newline at end of file diff --git a/Messages/CustomMsg.cs b/Messages/CustomMsg.cs new file mode 100644 index 0000000..5b491b1 --- /dev/null +++ b/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 Receive(Stream dataStream, Participant client, byte packetSize) { + // byte[] buffer = await Receive(dataStream, packetSize); + + // CustomMsg msg = new(buffer); + // client.messageQueue.Enqueue(msg); + // return true; + //} + } + +} \ No newline at end of file diff --git a/Messages/LowLevelMessages.cs b/Messages/LowLevelMessages.cs new file mode 100644 index 0000000..055096b --- /dev/null +++ b/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; + } + } + +} \ No newline at end of file diff --git a/Messages/Messages.cs b/Messages/Messages.cs new file mode 100644 index 0000000..83ba4e5 --- /dev/null +++ b/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 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 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 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 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 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 +} diff --git a/Messages/NameMsg.cs b/Messages/NameMsg.cs new file mode 100644 index 0000000..3d99f18 --- /dev/null +++ b/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; + } + } + +} \ No newline at end of file diff --git a/Messages/NetworkIdMsg.cs b/Messages/NetworkIdMsg.cs new file mode 100644 index 0000000..6b65913 --- /dev/null +++ b/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; + } + } + +} \ No newline at end of file