From 673fd3d2587fb2c8be4bd2bd464258380f441979 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 5 Dec 2024 18:06:19 +0100 Subject: [PATCH 1/3] Reimplemented almost everything in C# --- Messages.cs | 145 ++++++++++++++++++++++++++++++++++------------------ 1 file changed, 95 insertions(+), 50 deletions(-) diff --git a/Messages.cs b/Messages.cs index b517f4d..d1c3514 100644 --- a/Messages.cs +++ b/Messages.cs @@ -4,9 +4,10 @@ using System.IO; using System.Net.Sockets; using System.Threading.Tasks; -namespace Passer.Control { - - public class Client { +namespace Passer.Control +{ + public partial class Client + { //public ConnectionMethod connection; public UdpClient udpClient; public string ipAddress; @@ -16,8 +17,10 @@ namespace Passer.Control { public readonly ConcurrentQueue messageQueue = new(); - public static Client GetClient(string ipAddress, int port) { - foreach (Client c in clients) { + public static Client GetClient(string ipAddress, int port) + { + foreach (Client c in clients) + { if (c.ipAddress == ipAddress && c.port == port) return c; } @@ -25,7 +28,8 @@ namespace Passer.Control { } static public List clients = new List(); - public static Client NewClient() { + public static Client NewClient() + { Client client = new(); clients.Add(client); client.networkId = 0; @@ -33,7 +37,8 @@ namespace Passer.Control { return client; } - public static Client NewUDPClient(UdpClient udpClient, string ipAddress, int port) { + public static Client NewUDPClient(UdpClient udpClient, string ipAddress, int port) + { Client client = NewClient(); client.ipAddress = null; client.port = port; @@ -42,29 +47,35 @@ namespace Passer.Control { } } - public class IMessage { + public class IMessage + { public IMessage() { } - public IMessage(byte[] data) { + public IMessage(byte[] data) + { Deserialize(data); } public virtual byte[] Serialize() { return null; } public virtual void Deserialize(byte[] data) { } - public static bool SendMsg(Client client, IMessage msg) { + public static bool SendMsg(Client client, IMessage msg) + { return SendMsg(client, msg.Serialize()); } - public static bool SendMsg(Client client, byte[] data) { + public static bool SendMsg(Client client, byte[] data) + { if (client == null || client.ipAddress == null) return false; client.udpClient.Send(data, data.Length, client.ipAddress, client.port); return true; } - public static async Task Receive(Stream dataStream, byte packetSize) { + 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) { + 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); @@ -75,31 +86,37 @@ namespace Passer.Control { #region Client - public class ClientMsg : IMessage { + public class ClientMsg : IMessage + { + public const byte Id = 0xA0; public const byte length = 2; public byte clientId; public ClientMsg(byte[] data) : base(data) { } - public override void Deserialize(byte[] data) { + public override void Deserialize(byte[] data) + { base.Deserialize(data); uint ix = 0; clientId = data[ix++]; } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { if (packetSize != length) return false; byte[] buffer = await Receive(dataStream, packetSize); ClientMsg msg = new(buffer); - if (client.networkId == 0) { + if (client.networkId == 0) + { client.networkId = (byte)(Client.clients.Count); NetworkIdMsg.Send(client); //if (string.IsNullOrEmpty(sceneUrl) == false) //SendModelUrl(client, sceneUrl); } - else if (msg.clientId == 0) { + else if (msg.clientId == 0) + { NetworkIdMsg.Send(client); //if (string.IsNullOrEmpty(sceneUrl) == false) //SendModelUrl(client, sceneUrl); @@ -113,11 +130,13 @@ namespace Passer.Control { #region Network Id - public class NetworkIdMsg : IMessage { + public class NetworkIdMsg : IMessage + { public const byte Id = 0xA1; public const byte length = 2; - public static bool Send(Client client) { + public static bool Send(Client client) + { byte[] data = new byte[NetworkIdMsg.length]; data[0] = NetworkIdMsg.Id; data[1] = client.networkId; @@ -129,22 +148,25 @@ namespace Passer.Control { #region Thing - public class ThingMsg : IMessage { + public class ThingMsg : IMessage + { public const byte length = 4; public const byte Id = 0x80; - public byte objectId; - public byte objectType; + public byte thingId; + public byte thingType; public byte parentId; public ThingMsg(byte[] data) : base(data) { } - public override void Deserialize(byte[] data) { + public override void Deserialize(byte[] data) + { uint ix = 0; - objectId = data[ix++]; - objectType = data[ix++]; + thingId = data[ix++]; + thingType = data[ix++]; parentId = data[ix]; } - public static bool Send(Client client, byte networkId, byte thingId, byte thingType) { + public static bool Send(Client client, byte networkId, byte thingId, byte thingType) + { byte[] data = new byte[4]; data[0] = ThingMsg.Id; data[1] = networkId; @@ -153,7 +175,8 @@ namespace Passer.Control { data[4] = 0x00; // parent not supported yet return SendMsg(client, data); } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { if (packetSize != length) return false; @@ -169,21 +192,24 @@ namespace Passer.Control { #region Name - public class NameMsg : IMessage { + public class NameMsg : IMessage + { public byte networkId = 0; public byte objectId; public byte len; public string name; public NameMsg(byte[] data) : base(data) { } - public override void Deserialize(byte[] data) { + public override void Deserialize(byte[] data) + { uint ix = 0; this.objectId = data[ix++]; int strlen = data[ix++]; this.name = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen); } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { byte[] buffer = await Receive(dataStream, packetSize); NameMsg msg = new(buffer); @@ -196,25 +222,28 @@ namespace Passer.Control { #region Model URL - public class ModelUrlMsg : IMessage { + public class ModelUrlMsg : IMessage + { public const byte Id = 0x90; // (144) Model URL public byte objectId; public Spherical position; public float scale; public string url; - public ModelUrlMsg(string url, float scale = 1) { + public ModelUrlMsg(string url, float scale = 1) + { this.url = url; this.scale = scale; this.position = Spherical.zero; } public ModelUrlMsg(byte[] data) : base(data) { } - public override byte[] Serialize() { + public override byte[] Serialize() + { byte[] data = new byte[this.url.Length + 9]; data[0] = ModelUrlMsg.Id; data[1] = 0x00; // Thing Id - // data[2]..[5] == position 0, 0, 0 + // data[2]..[5] == position 0, 0, 0 data[6] = 0x3C; // Dummy float16 value 1 data[7] = 0x00; @@ -223,7 +252,8 @@ namespace Passer.Control { data[9 + ix] = (byte)url[ix]; return data; } - public override void Deserialize(byte[] data) { + public override void Deserialize(byte[] data) + { uint ix = 0; this.objectId = data[ix++]; this.position = LowLevelMessages.ReceiveSpherical(data, ref ix); @@ -232,11 +262,13 @@ namespace Passer.Control { url = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen); } - public static bool Send(Client client, string modelUrl) { + public static bool Send(Client client, string modelUrl) + { ModelUrlMsg msg = new(modelUrl); return SendMsg(client, msg); } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { byte[] buffer = await Receive(dataStream, packetSize); ModelUrlMsg msg = new(buffer); client.messageQueue.Enqueue(msg); @@ -248,7 +280,9 @@ namespace Passer.Control { #region Pose - public class PoseMsg : IMessage { + public class PoseMsg : IMessage + { + public const byte Id = 0x10; public const byte length = 3 + 4 + 4; public byte thingId; public byte poseType; @@ -258,7 +292,8 @@ namespace Passer.Control { public PoseMsg(byte[] data) : base(data) { } - public override void Deserialize(byte[] data) { + public override void Deserialize(byte[] data) + { uint ix = 0; thingId = data[ix++]; poseType = data[ix++]; @@ -269,7 +304,8 @@ namespace Passer.Control { orientation = LowLevelMessages.ReceiveQuat32(data, ref ix); } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { if (packetSize != length) return false; @@ -286,19 +322,22 @@ namespace Passer.Control { #region Bytes - public class BytesMsg : IMessage { + public class BytesMsg : IMessage + { public const byte Id = 0xB1; public byte networkId; public byte thingId; public byte[] bytes; public BytesMsg(byte[] data) : base(data) { } - public BytesMsg(byte networkId, byte thingId, byte[] bytes) : base() { + public BytesMsg(byte networkId, byte thingId, byte[] bytes) : base() + { this.networkId = networkId; this.thingId = thingId; this.bytes = bytes; } - public override byte[] Serialize() { + public override byte[] Serialize() + { byte[] buffer = new byte[4 + this.bytes.Length]; int ix = 0; buffer[ix++] = BytesMsg.Id; @@ -310,7 +349,8 @@ namespace Passer.Control { return buffer; } - public override void Deserialize(byte[] data) { + public override void Deserialize(byte[] data) + { //this.bytes = data; uint ix = 0; this.thingId = data[ix++]; @@ -320,13 +360,15 @@ namespace Passer.Control { } - public static void Send(Client client, byte thingId, byte[] bytes) { + public static void Send(Client client, byte thingId, byte[] bytes) + { BytesMsg msg = new(client.networkId, thingId, bytes); SendMsg(client, msg); } // received bytes - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { byte[] buffer = await Receive(dataStream, packetSize); BytesMsg msg = new(buffer); client.messageQueue.Enqueue(msg); @@ -338,17 +380,20 @@ namespace Passer.Control { #region Destroy - public class DestroyMsg : IMessage { + public class DestroyMsg : IMessage + { public const byte length = 2; public byte objectId; public DestroyMsg(byte[] data) : base(data) { } - public override void Deserialize(byte[] data) { + public override void Deserialize(byte[] data) + { objectId = data[0]; } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { if (packetSize != length) return false; From 0d022c26ef0168f1ea76749640f67c263150f385 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 12 Dec 2024 17:42:45 +0100 Subject: [PATCH 2/3] Spawned a GLB ant! --- Client.cs | 34 ++++++--- Messages.cs | 201 +++++++++++++++++++++++++++++++++++----------------- Thing.cs | 40 +++++++++++ 3 files changed, 198 insertions(+), 77 deletions(-) create mode 100644 Thing.cs diff --git a/Client.cs b/Client.cs index c5ca156..5498b2e 100644 --- a/Client.cs +++ b/Client.cs @@ -3,21 +3,27 @@ using System.Collections.Concurrent; using System.Net.Sockets; using System.IO; -namespace Passer.Control { +namespace Passer.Control +{ - public class Client { + public class Client + { //public ConnectionMethod connection; public UdpClient udpClient; public string ipAddress; public int port; public Stream dataStream; + public byte[] buffer = new byte[256]; + public byte networkId = 0; public readonly ConcurrentQueue messageQueue = new(); - public static Client GetClient(string ipAddress, int port) { - foreach (Client c in clients) { + public static Client GetClient(string ipAddress, int port) + { + foreach (Client c in clients) + { if (c.ipAddress == ipAddress && c.port == port) return c; } @@ -25,7 +31,8 @@ namespace Passer.Control { } static public List clients = new List(); - public Client(UdpClient udpClient, int port) { + public Client(UdpClient udpClient, int port) + { this.udpClient = udpClient; this.ipAddress = null; this.port = port; @@ -33,13 +40,16 @@ namespace Passer.Control { clients.Add(this); } - public virtual void ProcessMessages() { + public virtual void ProcessMessages() + { while (this.messageQueue.TryDequeue(out IMessage msg)) - ProcessMessage(msg); + ProcessMessage(msg); } - public void ProcessMessage(IMessage msg) { - switch (msg) { + public void ProcessMessage(IMessage msg) + { + switch (msg) + { case ClientMsg clientMsg: ProcessClient(clientMsg); break; @@ -97,8 +107,10 @@ namespace Passer.Control { protected virtual void ProcessDestroy(DestroyMsg destroy) { } - private void ForwardMessage(IMessage msg) { - foreach (Client client in Client.clients) { + private void ForwardMessage(IMessage msg) + { + foreach (Client client in Client.clients) + { if (client == this) continue; //UnityEngine.Debug.Log($"---> {client.ipAddress}"); diff --git a/Messages.cs b/Messages.cs index c49ac02..9727c16 100644 --- a/Messages.cs +++ b/Messages.cs @@ -1,21 +1,26 @@ using System.IO; using System.Threading.Tasks; -namespace Passer.Control { +namespace Passer.Control +{ - public class IMessage { + public class IMessage + { public IMessage() { } - public IMessage(byte[] buffer) { + public IMessage(byte[] buffer) + { Deserialize(buffer); } public virtual byte[] Serialize() { return null; } public virtual void Deserialize(byte[] buffer) { } - public static bool SendMsg(Client client, IMessage msg) { + public static bool SendMsg(Client client, IMessage msg) + { return SendMsg(client, msg.Serialize()); } - public static bool SendMsg(Client client, byte[] buffer) { + public static bool SendMsg(Client client, byte[] buffer) + { if (client == null || client.ipAddress == null) return false; @@ -24,10 +29,12 @@ namespace Passer.Control { return true; } - public static bool PublishMsg(Client client, IMessage msg) { + public static bool PublishMsg(Client client, IMessage msg) + { return PublishMsg(client, msg.Serialize()); } - public static bool PublishMsg(Client client, byte[] buffer) { + public static bool PublishMsg(Client client, byte[] buffer) + { if (client == null) return false; @@ -35,10 +42,12 @@ namespace Passer.Control { return true; } - public static async Task Receive(Stream dataStream, byte packetSize) { + 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) { + 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); @@ -49,49 +58,58 @@ namespace Passer.Control { #region Client - public class ClientMsg : IMessage { + public class ClientMsg : IMessage + { public const byte Id = 0xA0; public const byte length = 2; public byte networkId; - public ClientMsg(byte networkId) { + public ClientMsg(byte networkId) + { this.networkId = networkId; } public ClientMsg(byte[] buffer) : base(buffer) { } - public override byte[] Serialize() { + public override byte[] Serialize() + { byte[] buffer = new byte[ClientMsg.length]; buffer[0] = ClientMsg.Id; buffer[1] = networkId; return buffer; } - public override void Deserialize(byte[] buffer) { + public override void Deserialize(byte[] buffer) + { base.Deserialize(buffer); uint ix = 0; networkId = buffer[ix]; } - public static bool Send(Client client, byte networkId) { + public static bool Send(Client client, byte networkId) + { ClientMsg msg = new(networkId); return SendMsg(client, msg); } - public static bool Publish(Client client, byte networkId) { + public static bool Publish(Client client, byte networkId) + { ClientMsg msg = new(networkId); return PublishMsg(client, msg); } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { if (packetSize != length) return false; byte[] buffer = await Receive(dataStream, packetSize); ClientMsg msg = new(buffer); - if (client.networkId == 0) { + if (client.networkId == 0) + { client.networkId = (byte)(Client.clients.Count); NetworkIdMsg.Send(client, client.networkId); client.messageQueue.Enqueue(msg); } - else if (msg.networkId == 0) { + else if (msg.networkId == 0) + { NetworkIdMsg.Send(client, client.networkId); client.messageQueue.Enqueue(msg); } @@ -104,32 +122,38 @@ namespace Passer.Control { #region Network Id - public class NetworkIdMsg : IMessage { + public class NetworkIdMsg : IMessage + { public const byte Id = 0xA1; public const byte length = 2; public byte networkId; - NetworkIdMsg(byte networkId) { + NetworkIdMsg(byte networkId) + { this.networkId = networkId; } NetworkIdMsg(byte[] buffer) : base(buffer) { } - public override byte[] Serialize() { + public override byte[] Serialize() + { byte[] buffer = new byte[NetworkIdMsg.length]; buffer[0] = NetworkIdMsg.Id; buffer[1] = this.networkId; return buffer; } - public override void Deserialize(byte[] buffer) { + public override void Deserialize(byte[] buffer) + { uint ix = 0; this.networkId = buffer[ix]; } - public static bool Send(Client client, byte networkId) { + public static bool Send(Client client, byte networkId) + { NetworkIdMsg msg = new(networkId); return SendMsg(client, msg); } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { if (packetSize != length) return false; @@ -144,37 +168,43 @@ namespace Passer.Control { #region Investigate - public class InvestigateMsg : IMessage { + 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) { + public InvestigateMsg(byte networkId, byte thingId) + { this.networkId = networkId; this.thingId = thingId; } public InvestigateMsg(byte[] buffer) : base(buffer) { } - public override byte[] Serialize() { + public override byte[] Serialize() + { byte[] buffer = new byte[InvestigateMsg.length]; buffer[0] = InvestigateMsg.Id; buffer[1] = this.networkId; buffer[2] = this.thingId; return buffer; } - public override void Deserialize(byte[] buffer) { + public override void Deserialize(byte[] buffer) + { uint ix = 0; this.networkId = buffer[ix++]; this.thingId = buffer[ix++]; } - public static bool Send(Client client, byte networkId, byte thingId) { + public static bool Send(Client client, byte networkId, byte thingId) + { InvestigateMsg msg = new(networkId, thingId); //UnityEngine.Debug.Log($"Send investigate [{msg.networkId}/{msg.thingId}]"); return SendMsg(client, msg); } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { if (packetSize != length) return false; @@ -192,7 +222,8 @@ namespace Passer.Control { #region Thing - public class ThingMsg : IMessage { + public class ThingMsg : IMessage + { public const byte length = 5; public const byte Id = 0x80; public byte networkId; @@ -200,7 +231,8 @@ namespace Passer.Control { public byte thingType; public byte parentId; - public ThingMsg(byte networkId, byte thingId, byte thingType, byte parentId) { + public ThingMsg(byte networkId, byte thingId, byte thingType, byte parentId) + { this.networkId = networkId; this.thingId = thingId; this.thingType = thingType; @@ -208,7 +240,8 @@ namespace Passer.Control { } public ThingMsg(byte[] buffer) : base(buffer) { } - public override byte[] Serialize() { + public override byte[] Serialize() + { byte[] buffer = new byte[ThingMsg.length]; byte ix = 0; buffer[ix++] = ThingMsg.Id; @@ -218,7 +251,8 @@ namespace Passer.Control { buffer[ix] = this.parentId; return buffer; } - public override void Deserialize(byte[] buffer) { + public override void Deserialize(byte[] buffer) + { uint ix = 0; this.networkId = buffer[ix++]; this.thingId = buffer[ix++]; @@ -226,12 +260,14 @@ namespace Passer.Control { this.parentId = buffer[ix]; } - public static bool Send(Client client, byte networkId, byte thingId, byte thingType, byte parentId) { + public static bool Send(Client client, byte networkId, byte thingId, byte thingType, byte parentId) + { ThingMsg msg = new(networkId, thingId, thingType, parentId); //UnityEngine.Debug.Log($"Send thing [{msg.networkId}/{msg.thingId}]"); return SendMsg(client, msg); } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { if (packetSize != length) return false; @@ -252,7 +288,8 @@ namespace Passer.Control { #region Name - public class NameMsg : IMessage { + public class NameMsg : IMessage + { public const byte Id = 0x91; // 145 public const byte length = 4; public byte networkId; @@ -260,14 +297,16 @@ namespace Passer.Control { public byte len; public string name; - public NameMsg(byte networkId, byte thingId, string name) { + public NameMsg(byte networkId, byte thingId, string name) + { this.networkId = networkId; this.thingId = thingId; this.name = name; } public NameMsg(byte[] buffer) : base(buffer) { } - public override byte[] Serialize() { + public override byte[] Serialize() + { byte[] buffer = new byte[length + this.name.Length]; byte ix = 0; buffer[ix++] = NameMsg.Id; @@ -278,7 +317,8 @@ namespace Passer.Control { buffer[ix] = (byte)this.name[nameIx]; return buffer; } - public override void Deserialize(byte[] buffer) { + public override void Deserialize(byte[] buffer) + { byte ix = 0; this.networkId = buffer[ix++]; this.thingId = buffer[ix++]; @@ -286,11 +326,13 @@ namespace Passer.Control { this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen); } - public static bool Send(Client client, byte networkId, byte thingId, string name) { + public static bool Send(Client client, byte networkId, byte thingId, string name) + { NameMsg msg = new(networkId, thingId, name); return SendMsg(client, msg); } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { byte[] buffer = await Receive(dataStream, packetSize); NameMsg msg = new(buffer); @@ -303,7 +345,8 @@ namespace Passer.Control { #region Model URL - public class ModelUrlMsg : IMessage { + public class ModelUrlMsg : IMessage + { public const byte Id = 0x90; // (144) Model URL public byte networkId; public byte thingId; @@ -311,7 +354,8 @@ namespace Passer.Control { public float scale; public string url; - public ModelUrlMsg(byte networkId, byte thingId, string url, float scale = 1) { + public ModelUrlMsg(byte networkId, byte thingId, string url, float scale = 1) + { this.networkId = networkId; this.thingId = thingId; this.url = url; @@ -320,7 +364,8 @@ namespace Passer.Control { } public ModelUrlMsg(byte[] buffer) : base(buffer) { } - public override byte[] Serialize() { + public override byte[] Serialize() + { byte[] buffer = new byte[this.url.Length + 6]; byte ix = 0; buffer[ix++] = ModelUrlMsg.Id; @@ -333,7 +378,8 @@ namespace Passer.Control { buffer[ix] = (byte)url[urlIx]; return buffer; } - public override void Deserialize(byte[] buffer) { + public override void Deserialize(byte[] buffer) + { byte ix = 0; this.networkId = buffer[ix++]; this.thingId = buffer[ix++]; @@ -342,11 +388,16 @@ namespace Passer.Control { url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen); } - public static bool Send(Client client, byte networkId, byte thingId, string modelUrl) { + public static bool Send(Client client, byte networkId, byte thingId, string modelUrl) + { + if (string.IsNullOrEmpty(modelUrl)) + return true; // nothing sent, but still a success! + ModelUrlMsg msg = new(networkId, thingId, modelUrl); return SendMsg(client, msg); } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { byte[] buffer = await Receive(dataStream, packetSize); ModelUrlMsg msg = new(buffer); client.messageQueue.Enqueue(msg); @@ -358,7 +409,8 @@ namespace Passer.Control { #region Pose - public class PoseMsg : IMessage { + public class PoseMsg : IMessage + { public const byte Id = 0x10; public const byte length = 4 + 4 + 4; public byte networkId; @@ -371,7 +423,8 @@ namespace Passer.Control { public Spherical position; public Quat32 orientation; - public PoseMsg(byte networkId, byte thingId, Spherical position, Quat32 orientation) { + public PoseMsg(byte networkId, byte thingId, Spherical position, Quat32 orientation) + { this.networkId = networkId; this.thingId = thingId; this.position = position; @@ -388,7 +441,8 @@ namespace Passer.Control { } public PoseMsg(byte[] buffer) : base(buffer) { } - public override byte[] Serialize() { + public override byte[] Serialize() + { byte[] buffer = new byte[PoseMsg.length]; byte ix = 0; buffer[ix++] = PoseMsg.Id; @@ -400,7 +454,8 @@ namespace Passer.Control { LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation); return buffer; } - public override void Deserialize(byte[] buffer) { + public override void Deserialize(byte[] buffer) + { byte ix = 0; this.networkId = buffer[ix++]; this.thingId = buffer[ix++]; @@ -412,11 +467,13 @@ namespace Passer.Control { this.orientation = LowLevelMessages.ReceiveQuat32(buffer, ref ix); } - public static bool Send(Client client, byte thingId, Spherical position, Quat32 orientation) { + public static bool Send(Client client, byte thingId, Spherical position, Quat32 orientation) + { PoseMsg msg = new(client.networkId, thingId, position, orientation); return SendMsg(client, msg); } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { if (packetSize != length) return false; @@ -436,20 +493,23 @@ namespace Passer.Control { #region Custom - public class CustomMsg : IMessage { + public class CustomMsg : IMessage + { public const byte Id = 0xB1; public byte networkId; public byte thingId; public byte[] bytes; public CustomMsg(byte[] buffer) : base(buffer) { } - public CustomMsg(byte networkId, byte thingId, byte[] bytes) : base() { + public CustomMsg(byte networkId, byte thingId, byte[] bytes) : base() + { this.networkId = networkId; this.thingId = thingId; this.bytes = bytes; } - public override byte[] Serialize() { + public override byte[] Serialize() + { byte[] buffer = new byte[3 + this.bytes.Length]; byte ix = 0; buffer[ix++] = CustomMsg.Id; @@ -461,7 +521,8 @@ namespace Passer.Control { return buffer; } - public override void Deserialize(byte[] buffer) { + public override void Deserialize(byte[] buffer) + { byte ix = 0; this.networkId = buffer[ix++]; this.thingId = buffer[ix++]; @@ -471,11 +532,13 @@ namespace Passer.Control { this.bytes[bytesIx] = buffer[ix++]; } - public static void Send(Client client, byte thingId, byte[] bytes) { + public static void Send(Client client, byte thingId, byte[] bytes) + { CustomMsg msg = new(client.networkId, thingId, bytes); SendMsg(client, msg); } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { byte[] buffer = await Receive(dataStream, packetSize); CustomMsg msg = new(buffer); client.messageQueue.Enqueue(msg); @@ -487,18 +550,21 @@ namespace Passer.Control { #region Text - public class TextMsg : IMessage { + public class TextMsg : IMessage + { public const byte Id = 0xB0; public string text; public TextMsg(byte[] buffer) : base(buffer) { } - public override void Deserialize(byte[] 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, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { byte[] buffer = await Receive(dataStream, packetSize); TextMsg msg = new(buffer); @@ -511,7 +577,8 @@ namespace Passer.Control { #region Destroy - public class DestroyMsg : IMessage { + public class DestroyMsg : IMessage + { public const byte Id = 0x20; public const byte length = 2; public byte networkId; @@ -519,12 +586,14 @@ namespace Passer.Control { public DestroyMsg(byte[] buffer) : base(buffer) { } - public override void Deserialize(byte[] buffer) { + public override void Deserialize(byte[] buffer) + { this.networkId = buffer[0]; this.thingId = buffer[1]; } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static async Task Receive(Stream dataStream, Client client, byte packetSize) + { if (packetSize != length) return false; diff --git a/Thing.cs b/Thing.cs new file mode 100644 index 0000000..4e60d27 --- /dev/null +++ b/Thing.cs @@ -0,0 +1,40 @@ +using System.Collections.Generic; + +namespace Passer.Control +{ + + public class Thing + { + public Client client; + public byte networkId; + public byte id; + public Thing parent; + public byte type; + public string name; + public string modelUrl; + //protected Sensor sensor; + + protected virtual void Init() + { + } + + public Thing(Client client, byte networkId, byte objId, byte objType) + { + this.client = client; + this.id = objId; + this.type = objType; + this.networkId = networkId; + this.Init(); + allThings.Add(this); + } + + public static List allThings = new(); + + public static Thing Get(byte networkId, byte thingId) + { + Thing thing = allThings.Find(aThing => aThing.networkId == networkId && aThing.id == thingId); + return thing; + } + + } +} From 17fa48a26673454f22f9c48f76efb5a36c9ffc5a Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 13 Dec 2024 13:37:53 +0100 Subject: [PATCH 3/3] Ant is hopping around (pose does not look right yet...) --- Client.cs | 20 ++++++++++ LowLevelMessages.cs | 32 ++++++++++------ Messages.cs | 93 ++++++++++++++++++--------------------------- Quat32.cs | 11 ++++-- Spherical.cs | 9 +++-- Thing.cs | 16 ++++---- 6 files changed, 99 insertions(+), 82 deletions(-) diff --git a/Client.cs b/Client.cs index 5498b2e..8464ce8 100644 --- a/Client.cs +++ b/Client.cs @@ -11,6 +11,7 @@ namespace Passer.Control //public ConnectionMethod connection; public UdpClient udpClient; public string ipAddress; + public string broadcastIpAddress = "255.255.255.255"; public int port; public Stream dataStream; @@ -40,6 +41,25 @@ namespace Passer.Control clients.Add(this); } + public bool SendBuffer() + { + if (this.ipAddress == null) + return false; + + //UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}"); + this.udpClient.Send(this.buffer, this.buffer.Length, this.ipAddress, this.port); + return true; + } + + public bool PublishBuffer() + { + if (this.broadcastIpAddress == null) + return false; + + this.udpClient.Send(this.buffer, this.buffer.Length, this.broadcastIpAddress, this.port); + return true; + } + public virtual void ProcessMessages() { while (this.messageQueue.TryDequeue(out IMessage msg)) diff --git a/LowLevelMessages.cs b/LowLevelMessages.cs index abeef3a..ad6dbe6 100644 --- a/LowLevelMessages.cs +++ b/LowLevelMessages.cs @@ -1,14 +1,17 @@ -using Passer; +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) + { SendAngle8(buffer, ref ix, v.horizontal); SendAngle8(buffer, ref ix, v.vertical); SendFloat16(buffer, ref ix, new float16(v.distance)); } - public static Spherical ReceiveSpherical(byte[] data, ref byte ix) { + public static Spherical ReceiveSpherical(byte[] data, ref byte ix) + { float horizontal = ReceiveAngle8(data, ref ix); float vertical = ReceiveAngle8(data, ref ix); float distance = ReceiveFloat16(data, ref ix); @@ -16,12 +19,14 @@ 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, 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; @@ -33,7 +38,8 @@ 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, @@ -42,7 +48,8 @@ public class LowLevelMessages { return q; } - public static void SendAngle8(byte[] buffer, ref byte ix, float angle) { + public static void SendAngle8(byte[] buffer, ref byte ix, float angle) + { // Normalize angle while (angle >= 180) angle -= 360; @@ -51,18 +58,21 @@ 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; } - 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); diff --git a/Messages.cs b/Messages.cs index 9727c16..096279e 100644 --- a/Messages.cs +++ b/Messages.cs @@ -1,6 +1,7 @@ using System.IO; using System.Threading.Tasks; +using Passer.LinearAlgebra; namespace Passer.Control { @@ -12,34 +13,19 @@ namespace Passer.Control Deserialize(buffer); } - public virtual byte[] Serialize() { return null; } + public virtual byte Serialize(ref byte[] buffer) { return 0; } public virtual void Deserialize(byte[] buffer) { } public static bool SendMsg(Client client, IMessage msg) { - return SendMsg(client, msg.Serialize()); - } - public static bool SendMsg(Client client, byte[] buffer) - { - if (client == null || client.ipAddress == null) - return false; - - //UnityEngine.Debug.Log($"Send msg {buffer[0]} to {client.ipAddress}"); - client.udpClient.Send(buffer, buffer.Length, client.ipAddress, client.port); - return true; + msg.Serialize(ref client.buffer); + return client.SendBuffer(); } public static bool PublishMsg(Client client, IMessage msg) { - return PublishMsg(client, msg.Serialize()); - } - public static bool PublishMsg(Client client, byte[] buffer) - { - if (client == null) - return false; - - client.udpClient.Send(buffer, buffer.Length, "127.0.0.1", client.port); - return true; + msg.Serialize(ref client.buffer); + return client.PublishBuffer(); } public static async Task Receive(Stream dataStream, byte packetSize) @@ -70,12 +56,12 @@ namespace Passer.Control } public ClientMsg(byte[] buffer) : base(buffer) { } - public override byte[] Serialize() + public override byte Serialize(ref byte[] buffer) { - byte[] buffer = new byte[ClientMsg.length]; - buffer[0] = ClientMsg.Id; - buffer[1] = networkId; - return buffer; + byte ix = 0; + buffer[ix++] = ClientMsg.Id; + buffer[ix++] = networkId; + return ix; } public override void Deserialize(byte[] buffer) { @@ -134,12 +120,12 @@ namespace Passer.Control } NetworkIdMsg(byte[] buffer) : base(buffer) { } - public override byte[] Serialize() + public override byte Serialize(ref byte[] buffer) { - byte[] buffer = new byte[NetworkIdMsg.length]; - buffer[0] = NetworkIdMsg.Id; - buffer[1] = this.networkId; - return buffer; + byte ix = 0; + buffer[ix++] = NetworkIdMsg.Id; + buffer[ix++] = this.networkId; + return ix; } public override void Deserialize(byte[] buffer) { @@ -182,13 +168,13 @@ namespace Passer.Control } public InvestigateMsg(byte[] buffer) : base(buffer) { } - public override byte[] Serialize() + public override byte Serialize(ref byte[] buffer) { - byte[] buffer = new byte[InvestigateMsg.length]; - buffer[0] = InvestigateMsg.Id; - buffer[1] = this.networkId; - buffer[2] = this.thingId; - return buffer; + byte ix = 0; + buffer[ix++] = InvestigateMsg.Id; + buffer[ix++] = this.networkId; + buffer[ix++] = this.thingId; + return ix; } public override void Deserialize(byte[] buffer) { @@ -240,16 +226,15 @@ namespace Passer.Control } public ThingMsg(byte[] buffer) : base(buffer) { } - public override byte[] Serialize() + public override byte Serialize(ref byte[] buffer) { - byte[] buffer = new byte[ThingMsg.length]; byte ix = 0; buffer[ix++] = ThingMsg.Id; buffer[ix++] = this.networkId; buffer[ix++] = this.thingId; buffer[ix++] = this.thingType; - buffer[ix] = this.parentId; - return buffer; + buffer[ix++] = this.parentId; + return ix; } public override void Deserialize(byte[] buffer) { @@ -305,17 +290,16 @@ namespace Passer.Control } public NameMsg(byte[] buffer) : base(buffer) { } - public override byte[] Serialize() + public override byte Serialize(ref byte[] buffer) { - byte[] buffer = new byte[length + this.name.Length]; byte ix = 0; buffer[ix++] = NameMsg.Id; buffer[ix++] = this.networkId; buffer[ix++] = this.thingId; buffer[ix++] = (byte)this.name.Length; - for (int nameIx = 0; nameIx < this.name.Length; nameIx++, ix++) - buffer[ix] = (byte)this.name[nameIx]; - return buffer; + for (int nameIx = 0; nameIx < this.name.Length; nameIx++) + buffer[ix++] = (byte)this.name[nameIx]; + return ix; } public override void Deserialize(byte[] buffer) { @@ -364,9 +348,8 @@ namespace Passer.Control } public ModelUrlMsg(byte[] buffer) : base(buffer) { } - public override byte[] Serialize() + public override byte Serialize(ref byte[] buffer) { - byte[] buffer = new byte[this.url.Length + 6]; byte ix = 0; buffer[ix++] = ModelUrlMsg.Id; buffer[ix++] = this.networkId; @@ -374,9 +357,9 @@ namespace Passer.Control LowLevelMessages.SendFloat16(buffer, ref ix, new float16(1.0f)); buffer[ix++] = (byte)url.Length; - for (int urlIx = 0; urlIx < this.url.Length; urlIx++, ix++) - buffer[ix] = (byte)url[urlIx]; - return buffer; + for (int urlIx = 0; urlIx < this.url.Length; urlIx++) + buffer[ix++] = (byte)url[urlIx]; + return ix; } public override void Deserialize(byte[] buffer) { @@ -441,9 +424,8 @@ namespace Passer.Control } public PoseMsg(byte[] buffer) : base(buffer) { } - public override byte[] Serialize() + public override byte Serialize(ref byte[] buffer) { - byte[] buffer = new byte[PoseMsg.length]; byte ix = 0; buffer[ix++] = PoseMsg.Id; buffer[ix++] = this.networkId; @@ -452,7 +434,7 @@ namespace Passer.Control LowLevelMessages.SendSpherical(buffer, ref ix, this.position); LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation); - return buffer; + return ix; } public override void Deserialize(byte[] buffer) { @@ -508,9 +490,8 @@ namespace Passer.Control this.bytes = bytes; } - public override byte[] Serialize() + public override byte Serialize(ref byte[] buffer) { - byte[] buffer = new byte[3 + this.bytes.Length]; byte ix = 0; buffer[ix++] = CustomMsg.Id; buffer[ix++] = this.networkId; @@ -519,7 +500,7 @@ namespace Passer.Control foreach (byte b in bytes) buffer[ix++] = b; - return buffer; + return ix; } public override void Deserialize(byte[] buffer) { diff --git a/Quat32.cs b/Quat32.cs index dc6cbc4..1f41a99 100644 --- a/Quat32.cs +++ b/Quat32.cs @@ -1,15 +1,18 @@ -namespace Passer { - public class Quat32 { +namespace Passer.LinearAlgebra +{ + public class Quat32 + { public float x; public float y; public float z; public float w; - public Quat32(float x, float y, float z, float w) { + public Quat32(float x, float y, float z, float w) + { this.x = x; this.y = y; this.z = z; this.w = w; - } + } } } \ No newline at end of file diff --git a/Spherical.cs b/Spherical.cs index 3ecb663..0ba8933 100644 --- a/Spherical.cs +++ b/Spherical.cs @@ -1,12 +1,15 @@ -namespace Passer { - public class Spherical { +namespace Passer.LinearAlgebra +{ + public class Spherical + { public float distance; public float horizontal; public float vertical; public static Spherical zero = new(0, 0, 0); - public Spherical(float distance, float horizontal, float vertical) { + public Spherical(float distance, float horizontal, float vertical) + { this.distance = distance; this.horizontal = horizontal; this.vertical = vertical; diff --git a/Thing.cs b/Thing.cs index 4e60d27..cf6cbea 100644 --- a/Thing.cs +++ b/Thing.cs @@ -3,12 +3,12 @@ using System.Collections.Generic; namespace Passer.Control { - public class Thing + public class CoreThing { public Client client; public byte networkId; public byte id; - public Thing parent; + public CoreThing parent; public byte type; public string name; public string modelUrl; @@ -18,21 +18,21 @@ namespace Passer.Control { } - public Thing(Client client, byte networkId, byte objId, byte objType) + public CoreThing(Client client, byte networkId, byte thingId, byte thingType = 0) { this.client = client; - this.id = objId; - this.type = objType; + this.id = thingId; + this.type = thingType; this.networkId = networkId; this.Init(); allThings.Add(this); } - public static List allThings = new(); + private static readonly List allThings = new(); - public static Thing Get(byte networkId, byte thingId) + public static CoreThing Get(byte networkId, byte thingId) { - Thing thing = allThings.Find(aThing => aThing.networkId == networkId && aThing.id == thingId); + CoreThing thing = allThings.Find(aThing => aThing.networkId == networkId && aThing.id == thingId); return thing; }