diff --git a/LowLevelMessages.cs b/LowLevelMessages.cs index f279dbc..43f3eeb 100644 --- a/LowLevelMessages.cs +++ b/LowLevelMessages.cs @@ -3,9 +3,9 @@ using Passer; public class LowLevelMessages { public static void SendSpherical(byte[] buffer, ref uint ix, Spherical v) { - SendFloat16(buffer, ref ix, new float16(v.distance)); 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 uint ix) { diff --git a/Messages.cs b/Messages.cs index eacfea6..f23e497 100644 --- a/Messages.cs +++ b/Messages.cs @@ -3,366 +3,522 @@ using System.Collections.Concurrent; using System.IO; using System.Net.Sockets; using System.Threading.Tasks; +using System; namespace Passer.Control { - public class Client { - //public ConnectionMethod connection; - public UdpClient udpClient; - public string ipAddress; - public int port; + public class Client { + //public ConnectionMethod connection; + public UdpClient udpClient; + public string ipAddress; + public int port; - public byte networkId; + public byte networkId; - public readonly ConcurrentQueue messageQueue = new(); + public readonly ConcurrentQueue messageQueue = new(); - public static Client GetClient(string ipAddress, int port) { - foreach (Client c in clients) { - if (c.ipAddress == ipAddress && c.port == port) - return c; - } - return null; + public static Client GetClient(string ipAddress, int port) { + foreach (Client c in clients) { + if (c.ipAddress == ipAddress && c.port == port) + return c; + } + return null; + } + static public List clients = new List(); + + public static Client NewClient() { + Client client = new(); + clients.Add(client); + client.networkId = 0; + + return client; + } + + public static Client NewUDPClient(UdpClient udpClient, string ipAddress, int port) { + Client client = NewClient(); + client.ipAddress = null; + client.port = port; + client.udpClient = udpClient; + return client; + } + } + + public class IMessage { + public IMessage() { } + 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) { + return SendMsg(client, msg.Serialize()); + } + 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 bool PublishMsg(Client client, IMessage msg) { + return PublishMsg(client, msg.Serialize()); } - static public List clients = new List(); + public static bool PublishMsg(Client client, byte[] data) { + if (client == null) + return false; - public static Client NewClient() { - Client client = new(); - clients.Add(client); - client.networkId = 0; - - return client; + client.udpClient.Send(data, data.Length, "127.0.0.1", client.port); + return true; } - public static Client NewUDPClient(UdpClient udpClient, string ipAddress, int port) { - Client client = NewClient(); - client.ipAddress = null; - client.port = port; - client.udpClient = udpClient; - return client; + 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 Client + + 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[] data) : base(data) { } + + public override byte[] Serialize() { + byte[] buffer = new byte[ClientMsg.length]; + buffer[0] = ClientMsg.Id; + buffer[1] = networkId; + return buffer; } - } - - public class IMessage { - public IMessage() { } - 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) { - return SendMsg(client, msg.Serialize()); - } - 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) { - 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 Client - - public class ClientMsg : IMessage { - public const byte length = 2; - public byte clientId; - - public ClientMsg(byte[] data) : base(data) { } public override void Deserialize(byte[] data) { - base.Deserialize(data); - uint ix = 0; - clientId = data[ix++]; + base.Deserialize(data); + uint ix = 0; + networkId = data[ix]; + } + + public static bool Send(Client client, byte networkId) { + ClientMsg msg = new(networkId); + return SendMsg(client, msg); + } + 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) { + if (packetSize != length) + return false; + + byte[] buffer = await Receive(dataStream, packetSize); + ClientMsg msg = new(buffer); + + if (client.networkId == 0) { + client.networkId = (byte)(Client.clients.Count); + NetworkIdMsg.Send(client, client.networkId); + //if (string.IsNullOrEmpty(sceneUrl) == false) + //SendModelUrl(client, sceneUrl); + } + else if (msg.networkId == 0) { + NetworkIdMsg.Send(client, client.networkId); + //if (string.IsNullOrEmpty(sceneUrl) == false) + //SendModelUrl(client, sceneUrl); + } + + return true; + } + } + + #endregion Client + + #region Network Id + + public class NetworkIdMsg : IMessage { + public const byte Id = 0xA1; + public const byte length = 2; + public byte networkId; + + NetworkIdMsg(byte networkId) { + this.networkId = networkId; + } + NetworkIdMsg(byte[] data) : base(data) { } + + public override byte[] Serialize() { + byte[] data = new byte[NetworkIdMsg.length]; + data[0] = NetworkIdMsg.Id; + data[1] = this.networkId; + return data; + } + public override void Deserialize(byte[] data) { + uint ix = 0; + this.networkId = data[ix]; } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + public static bool Send(Client client, byte networkId) { + NetworkIdMsg msg = new(networkId); + return SendMsg(client, msg); + //byte[] data = new byte[NetworkIdMsg.length]; + //data[0] = NetworkIdMsg.Id; + //data[1] = client.networkId; + //return SendMsg(client, data); + } + 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) { - client.networkId = (byte)(Client.clients.Count); - NetworkIdMsg.Send(client); - //if (string.IsNullOrEmpty(sceneUrl) == false) - //SendModelUrl(client, sceneUrl); - } - else if (msg.clientId == 0) { - NetworkIdMsg.Send(client); - //if (string.IsNullOrEmpty(sceneUrl) == false) - //SendModelUrl(client, sceneUrl); - } - - return true; + NetworkIdMsg msg = new(buffer); + client.messageQueue.Enqueue(msg); + return true; } - } - - #endregion Client - - #region Network Id - - public class NetworkIdMsg : IMessage { - public const byte Id = 0xA1; - public const byte length = 2; - - public static bool Send(Client client) { - byte[] data = new byte[NetworkIdMsg.length]; - data[0] = NetworkIdMsg.Id; - data[1] = client.networkId; - return SendMsg(client, data); - } - } + } #endregion Network Id + #region Investigate + + 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[] data) : base(data) { } + + 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[] data) { + uint ix = 0; + this.networkId = data[ix++]; + this.thingId = data[ix++]; + } + + public static bool Send(Client client, byte thingId) { + InvestigateMsg msg = new(client.networkId, thingId); + return SendMsg(client, msg); + } + public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + if (packetSize != length) + return false; + + byte[] buffer = await Receive(dataStream, packetSize); + InvestigateMsg msg = new(buffer); + client.messageQueue.Enqueue(msg); + return true; + + } + } + + #endregion Investigate + #region Thing public class ThingMsg : IMessage { - public const byte length = 4; - public const byte Id = 0x80; - public byte objectId; - public byte objectType; - public byte parentId; + public const byte length = 5; + public const byte Id = 0x80; + public byte objectId; + public byte objectType; + public byte parentId; - public ThingMsg(byte[] data) : base(data) { } - public override void Deserialize(byte[] data) { - uint ix = 0; - objectId = data[ix++]; - objectType = data[ix++]; - parentId = data[ix]; - } + public ThingMsg(byte[] data) : base(data) { } + public override void Deserialize(byte[] data) { + uint ix = 0; + objectId = data[ix++]; + objectType = data[ix++]; + parentId = data[ix]; + } - public static bool Send(Client client, Thing obj) { - if (obj == null) - return false; + public static bool Send(Client client, byte thingId, byte thingType, byte parentId) { + byte[] data = new byte[ThingMsg.length]; + data[0] = ThingMsg.Id; + data[1] = client.networkId; + data[2] = thingId; + data[3] = thingType; + data[4] = parentId; + return SendMsg(client, data); + } + public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + if (packetSize != length) + return false; - byte[] data = new byte[4]; - data[0] = ThingMsg.Id; - data[1] = obj.networkId; - data[2] = obj.thingId; - data[3] = obj.objectType; - data[4] = 0x00; // not supported yet - return SendMsg(client, data); - } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { - if (packetSize != length) - return false; + byte[] buffer = await Receive(dataStream, packetSize); + ThingMsg msg = new(buffer); - byte[] buffer = await Receive(dataStream, packetSize); - ThingMsg msg = new(buffer); + client.messageQueue.Enqueue(msg); + return true; + } + } - client.messageQueue.Enqueue(msg); - return true; - } - } + #endregion Thing - #endregion Thing + #region Name - #region Name + public class NameMsg : IMessage { + public const byte Id = 0x91; // 145 + public const byte length = 3; + public byte networkId = 0; + public byte thingId; + public byte len; + public string name; - 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) { - 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) { - byte[] buffer = await Receive(dataStream, packetSize); - NameMsg msg = new(buffer); - - client.messageQueue.Enqueue(msg); - return true; - } - } - - #endregion - - #region Model URL - - 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) { - this.url = url; - this.scale = scale; - this.position = Spherical.zero; - } - public ModelUrlMsg(byte[] data) : base(data) { } + public NameMsg(byte thingId, string name) { + this.thingId = thingId; + this.name = name; + } + public NameMsg(byte[] data) : base(data) { } 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[6] = 0x3C; // Dummy float16 value 1 - data[7] = 0x00; - - data[8] = (byte)url.Length; - for (int ix = 0; ix < this.url.Length; ix++) - data[9 + ix] = (byte)url[ix]; - return data; + byte[] buffer = new byte[length + this.name.Length]; + buffer[0] = NameMsg.Id; + buffer[1] = this.thingId; + buffer[2] = (byte)this.name.Length; + for (int ix = 0; ix < this.name.Length; ix++) + buffer[3 + ix] = (byte)this.name[ix]; + return buffer; } public override void Deserialize(byte[] data) { - uint ix = 0; - this.objectId = data[ix++]; - this.position = LowLevelMessages.ReceiveSpherical(data, ref ix); - this.scale = LowLevelMessages.ReceiveFloat16(data, ref ix); - int strlen = data[ix++]; - url = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen); + uint ix = 0; + this.thingId = data[ix++]; + int strlen = data[ix++]; + this.name = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen); + } + + public static bool Send(Client client, byte thingId, string name) { + NameMsg msg = new(thingId, name); + return SendMsg(client, msg); + } + public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + byte[] buffer = await Receive(dataStream, packetSize); + NameMsg msg = new(buffer); + + client.messageQueue.Enqueue(msg); + return true; + } + } + + #endregion + + #region Model URL + + public class ModelUrlMsg : IMessage { + public const byte Id = 0x90; // (144) Model URL + public byte thingId; + public Spherical position; + public float scale; + public string url; + + public ModelUrlMsg(byte thingId, string url, float scale = 1) { + this.thingId = thingId; + this.url = url; + this.scale = scale; + this.position = Spherical.zero; + } + public ModelUrlMsg(byte[] data) : base(data) { } + + public override byte[] Serialize() { + byte[] data = new byte[this.url.Length + 9]; + data[0] = ModelUrlMsg.Id; + data[1] = this.thingId; // Thing Id + // data[2]..[5] == position 0, 0, 0 + data[6] = 0x3C; // Dummy float16 value 1 + data[7] = 0x00; + + data[8] = (byte)url.Length; + for (int ix = 0; ix < this.url.Length; ix++) + data[9 + ix] = (byte)url[ix]; + return data; + } + public override void Deserialize(byte[] data) { + uint ix = 0; + this.thingId = data[ix++]; + this.position = LowLevelMessages.ReceiveSpherical(data, ref ix); + this.scale = LowLevelMessages.ReceiveFloat16(data, ref ix); + int strlen = data[ix++]; + url = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen); + } + + public static bool Send(Client client, byte thingId, string modelUrl) { + ModelUrlMsg msg = new(thingId, modelUrl); + return SendMsg(client, msg); + } + 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); + return true; + } + } + + #endregion Model URL + + #region Pose + + public class PoseMsg : IMessage { + public const byte Id = 0x10; + public const byte length = 3 + 4 + 4; + public byte thingId; + + public byte poseType; + public const byte Pose_Position = 0x01; + public const byte Pose_Orientation = 0x02; + + public Spherical position; + public Quat32 orientation; + + public PoseMsg(byte thingId, Spherical position, Quat32 orientation) { + 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 = new Quat32(0, 0, 0, 1); } - - 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) { - byte[] buffer = await Receive(dataStream, packetSize); - ModelUrlMsg msg = new(buffer); - client.messageQueue.Enqueue(msg); - return true; - } - } - - #endregion Model URL - - #region Pose - - public class PoseMsg : IMessage { - public const byte length = 3 + 4 + 4; - public byte thingId; - public byte poseType; - - public Spherical position; - public Quat32 orientation; - public PoseMsg(byte[] data) : base(data) { } - public override void Deserialize(byte[] data) { - uint ix = 0; - thingId = data[ix++]; - poseType = data[ix++]; - - //if ((poseType & Pose_Position) != 0) - position = LowLevelMessages.ReceiveSpherical(data, ref ix); - //if ((poseType & Pose_Orientation) != 0) { - orientation = LowLevelMessages.ReceiveQuat32(data, ref ix); - } - - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { - if (packetSize != length) - return false; - - byte[] buffer = await Receive(dataStream, packetSize); - PoseMsg msg = new(buffer); - - client.messageQueue.Enqueue(msg); - return true; - - } - } - - #endregion Pose - - #region Bytes - - 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() { - this.networkId = networkId; - this.thingId = thingId; - this.bytes = bytes; - } public override byte[] Serialize() { - byte[] buffer = new byte[4 + this.bytes.Length]; - int ix = 0; - buffer[ix++] = BytesMsg.Id; - buffer[ix++] = this.networkId; - buffer[ix++] = this.thingId; - buffer[ix++] = (byte)bytes.Length; - foreach (byte b in bytes) - buffer[ix++] = b; + byte[] buffer = new byte[PoseMsg.length]; + uint ix = 0; + buffer[ix++] = PoseMsg.Id; + buffer[ix++] = this.thingId; + buffer[ix++] = this.poseType; - return buffer; + LowLevelMessages.SendSpherical(buffer, ref ix, position); + LowLevelMessages.SendQuat32(buffer, ref ix, orientation); + return buffer; } public override void Deserialize(byte[] data) { - //this.bytes = data; - uint ix = 0; - this.thingId = data[ix++]; - this.bytes = new byte[data.Length - ix]; - for (uint bytesIx = 0; ix < data.Length; ix++, bytesIx++) - this.bytes[bytesIx] = data[ix]; - } + uint ix = 0; + thingId = data[ix++]; + poseType = data[ix++]; + + //if ((poseType & Pose_Position) != 0) + position = LowLevelMessages.ReceiveSpherical(data, ref ix); + //if ((poseType & Pose_Orientation) != 0) { + orientation = LowLevelMessages.ReceiveQuat32(data, ref ix); + } + + public static bool Send(Client client, byte thingId, Spherical position, Quat32 orientation) { + PoseMsg msg = new(thingId, position, orientation); + return SendMsg(client, msg); + } + public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + if (packetSize != length) + return false; + + byte[] buffer = await Receive(dataStream, packetSize); + PoseMsg msg = new(buffer); + + client.messageQueue.Enqueue(msg); + return true; + } + } + + #endregion Pose + + #region Bytes + + 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() { + this.networkId = networkId; + this.thingId = thingId; + this.bytes = bytes; + } + public override byte[] Serialize() { + byte[] buffer = new byte[4 + this.bytes.Length]; + int ix = 0; + buffer[ix++] = BytesMsg.Id; + buffer[ix++] = this.networkId; + buffer[ix++] = this.thingId; + buffer[ix++] = (byte)bytes.Length; + foreach (byte b in bytes) + buffer[ix++] = b; + + return buffer; + } + public override void Deserialize(byte[] data) { + //this.bytes = data; + uint ix = 0; + this.thingId = data[ix++]; + this.bytes = new byte[data.Length - ix]; + for (uint bytesIx = 0; ix < data.Length; ix++, bytesIx++) + this.bytes[bytesIx] = data[ix]; + } - public static void Send(Client client, byte thingId, byte[] bytes) { - BytesMsg msg = new(client.networkId, thingId, bytes); - SendMsg(client, msg); - } + 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) { - byte[] buffer = await Receive(dataStream, packetSize); - BytesMsg msg = new(buffer); - client.messageQueue.Enqueue(msg); - return true; - } - } + // received bytes + 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); + return true; + } + } - #endregion Bytes + #endregion Bytes - #region Destroy + #region Destroy - public class DestroyMsg : IMessage { - public const byte length = 2; - public byte objectId; + public class DestroyMsg : IMessage { + public const byte Id = 0x20; + public const byte length = 2; + public byte objectId; - public DestroyMsg(byte[] data) : base(data) { } + public DestroyMsg(byte[] data) : base(data) { } - public override void Deserialize(byte[] data) { - objectId = data[0]; - } + public override void Deserialize(byte[] data) { + objectId = data[0]; + } - public static async Task Receive(Stream dataStream, Client client, byte packetSize) { - if (packetSize != length) - return false; + public static async Task Receive(Stream dataStream, Client client, byte packetSize) { + if (packetSize != length) + return false; - byte[] buffer = await Receive(dataStream, packetSize); - DestroyMsg msg = new(buffer); + byte[] buffer = await Receive(dataStream, packetSize); + DestroyMsg msg = new(buffer); - client.messageQueue.Enqueue(msg); - return true; - } - } + client.messageQueue.Enqueue(msg); + return true; + } + } - #endregion Destroy + #endregion Destroy }