From 1429f0a9d611c866b4795564c8e9f263dfdd6e18 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 30 Dec 2024 15:35:06 +0100 Subject: [PATCH] Make the local things work (somewhat) --- LinearAlgebra.meta | 8 + LinearAlgebra/Direction.cs | 32 +++ LinearAlgebra/Direction.cs.meta | 11 ++ Quat32.cs => LinearAlgebra/Quat32.cs | 0 .../Quat32.cs.meta | 0 Spherical.cs => LinearAlgebra/Spherical.cs | 10 +- .../Spherical.cs.meta | 0 LinearAlgebra/SwingTwist.cs | 20 ++ LinearAlgebra/SwingTwist.cs.meta | 11 ++ LowLevelMessages.cs | 47 ++--- Messages.cs | 164 ++++++++++++--- Participant.cs | 139 +++++++++++-- SiteServer.cs | 4 +- Thing.cs | 187 ++++++++++++++++-- Thing.cs.meta | 2 +- 15 files changed, 548 insertions(+), 87 deletions(-) create mode 100644 LinearAlgebra.meta create mode 100644 LinearAlgebra/Direction.cs create mode 100644 LinearAlgebra/Direction.cs.meta rename Quat32.cs => LinearAlgebra/Quat32.cs (100%) rename Quat32.cs.meta => LinearAlgebra/Quat32.cs.meta (100%) rename Spherical.cs => LinearAlgebra/Spherical.cs (53%) rename Spherical.cs.meta => LinearAlgebra/Spherical.cs.meta (100%) create mode 100644 LinearAlgebra/SwingTwist.cs create mode 100644 LinearAlgebra/SwingTwist.cs.meta diff --git a/LinearAlgebra.meta b/LinearAlgebra.meta new file mode 100644 index 0000000..a68ed37 --- /dev/null +++ b/LinearAlgebra.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 028f10636fdaa594a9a9969924e5ff18 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/LinearAlgebra/Direction.cs b/LinearAlgebra/Direction.cs new file mode 100644 index 0000000..9ab56fd --- /dev/null +++ b/LinearAlgebra/Direction.cs @@ -0,0 +1,32 @@ +namespace Passer.LinearAlgebra { + + public class Direction { + public float horizontal; + public float vertical; + + public Direction() { + horizontal = 0; + vertical = 0; + } + public Direction(float horizontal, float vertical) { + this.horizontal = horizontal; + this.vertical = vertical; + //Normalize(); + } + + public readonly static Direction forward = new(0, 0); + public readonly static Direction backward = new(-180, 0); + public readonly static Direction up = new(0, 90); + public readonly static Direction down = new(0, -90); + public readonly static Direction left = new(-90, 0); + public readonly static Direction right = new(90, 0); + + public void Normalize() { + if (this.vertical > 90 || this.vertical < -90) { + this.horizontal += 180; + this.vertical = 180 - this.vertical; + } + } + } + +} \ No newline at end of file diff --git a/LinearAlgebra/Direction.cs.meta b/LinearAlgebra/Direction.cs.meta new file mode 100644 index 0000000..206e1a6 --- /dev/null +++ b/LinearAlgebra/Direction.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9975e4702de32624e8d3deaf84669f2f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Quat32.cs b/LinearAlgebra/Quat32.cs similarity index 100% rename from Quat32.cs rename to LinearAlgebra/Quat32.cs diff --git a/Quat32.cs.meta b/LinearAlgebra/Quat32.cs.meta similarity index 100% rename from Quat32.cs.meta rename to LinearAlgebra/Quat32.cs.meta diff --git a/Spherical.cs b/LinearAlgebra/Spherical.cs similarity index 53% rename from Spherical.cs rename to LinearAlgebra/Spherical.cs index 0ba8933..06ba33d 100644 --- a/Spherical.cs +++ b/LinearAlgebra/Spherical.cs @@ -3,16 +3,18 @@ namespace Passer.LinearAlgebra public class Spherical { public float distance; - public float horizontal; - public float vertical; + public Direction direction; public static Spherical zero = new(0, 0, 0); public Spherical(float distance, float horizontal, float vertical) { this.distance = distance; - this.horizontal = horizontal; - this.vertical = vertical; + this.direction = new Direction(horizontal, vertical); + } + public Spherical(float distance, Direction direction) { + this.distance = distance; + this.direction = direction; } } } \ No newline at end of file diff --git a/Spherical.cs.meta b/LinearAlgebra/Spherical.cs.meta similarity index 100% rename from Spherical.cs.meta rename to LinearAlgebra/Spherical.cs.meta diff --git a/LinearAlgebra/SwingTwist.cs b/LinearAlgebra/SwingTwist.cs new file mode 100644 index 0000000..c73a28d --- /dev/null +++ b/LinearAlgebra/SwingTwist.cs @@ -0,0 +1,20 @@ +namespace Passer.LinearAlgebra { + + public class SwingTwist { + public Direction swing; + public float twist; + + public static readonly SwingTwist zero = new(0, 0, 0); + + public SwingTwist(Direction swing, float twist) { + this.swing = swing; + this.twist = twist; + } + public SwingTwist(float horizontalSwing, float verticalSwing, float twist) { + this.swing = new Direction(horizontalSwing, verticalSwing); + this.swing.Normalize(); + this.twist = twist; + } + } + +} \ No newline at end of file diff --git a/LinearAlgebra/SwingTwist.cs.meta b/LinearAlgebra/SwingTwist.cs.meta new file mode 100644 index 0000000..0c64209 --- /dev/null +++ b/LinearAlgebra/SwingTwist.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 003a6ec763b101642b0589486f087aef +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/LowLevelMessages.cs b/LowLevelMessages.cs index 7bba733..051b5f5 100644 --- a/LowLevelMessages.cs +++ b/LowLevelMessages.cs @@ -1,17 +1,14 @@ using Passer.LinearAlgebra; -public class LowLevelMessages -{ +public class LowLevelMessages { - public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v) - { + public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v) { SendFloat16(buffer, ref ix, new float16(v.distance)); - SendAngle8(buffer, ref ix, v.horizontal); - SendAngle8(buffer, ref ix, v.vertical); + SendAngle8(buffer, ref ix, v.direction.horizontal); + SendAngle8(buffer, ref ix, v.direction.horizontal); } - public static Spherical ReceiveSpherical(byte[] data, ref byte ix) - { + public static Spherical ReceiveSpherical(byte[] data, ref byte ix) { float distance = ReceiveFloat16(data, ref ix); float horizontal = ReceiveAngle8(data, ref ix); float vertical = ReceiveAngle8(data, ref ix); @@ -19,14 +16,18 @@ public class LowLevelMessages return v; } - public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q) - { + public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s) { + UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(-s.swing.vertical, s.swing.horizontal, s.twist); + Quat32 q32 = new(q.x, q.y, q.z, q.w); + SendQuat32(buffer, ref ix, q32); + + } + public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q) { int qx = (int)(q.x * 127 + 128); int qy = (int)(q.y * 127 + 128); int qz = (int)(q.z * 127 + 128); int qw = (int)(q.w * 255); - if (q.w < 0) - { + if (q.w < 0) { qx = -qx; qy = -qy; qz = -qz; @@ -38,8 +39,7 @@ public class LowLevelMessages buffer[ix++] = (byte)qz; buffer[ix++] = (byte)qw; } - public static Quat32 ReceiveQuat32(byte[] data, ref byte ix) - { + public static Quat32 ReceiveQuat32(byte[] data, ref byte ix) { Quat32 q = new( (data[ix++] - 128.0F) / 127.0F, (data[ix++] - 128.0F) / 127.0F, @@ -48,8 +48,14 @@ public class LowLevelMessages return q; } - public static void SendAngle8(byte[] buffer, ref byte ix, float angle) - { + public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix) { + Quat32 q32 = ReceiveQuat32(data, ref ix); + UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w); + SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z); + return r; + } + + public static void SendAngle8(byte[] buffer, ref byte ix, float angle) { // Normalize angle while (angle >= 180) angle -= 360; @@ -58,8 +64,7 @@ public class LowLevelMessages buffer[ix++] = (byte)((angle / 360.0f) * 256.0f); } - public static float ReceiveAngle8(byte[] data, ref byte ix) - { + public static float ReceiveAngle8(byte[] data, ref byte ix) { float value = (data[ix++] * 180) / 128.0F; return value; } @@ -70,15 +75,13 @@ public class LowLevelMessages data[ix++] = (byte)(binary >> 8); data[ix++] = (byte)(binary & 255); } - public static void SendFloat16(byte[] data, ref byte ix, float16 f) - { + public static void SendFloat16(byte[] data, ref byte ix, float16 f) { ushort binary = f.GetBinary(); data[ix++] = (byte)(binary >> 8); data[ix++] = (byte)(binary & 255); } - public static float ReceiveFloat16(byte[] data, ref byte ix) - { + public static float ReceiveFloat16(byte[] data, ref byte ix) { ushort value = (ushort)(data[ix++] << 8 | data[ix++]); float16 f16 = new(); f16.SetBinary(value); diff --git a/Messages.cs b/Messages.cs index d462d19..d671ec4 100644 --- a/Messages.cs +++ b/Messages.cs @@ -2,7 +2,7 @@ using System.IO; using System.Threading.Tasks; using Passer.LinearAlgebra; -namespace Passer.Control { +namespace Passer.Control.Core { public class IMessage { public IMessage() { } @@ -15,17 +15,17 @@ namespace Passer.Control { public bool SendTo(Participant client) { Serialize(ref client.buffer); - return client.SendBuffer(); + return client.SendBuffer(client.buffer.Length); } public static bool SendMsg(Participant client, IMessage msg) { msg.Serialize(ref client.buffer); - return client.SendBuffer(); + return client.SendBuffer(client.buffer.Length); } public static bool PublishMsg(Participant client, IMessage msg) { msg.Serialize(ref client.buffer); - return client.PublishBuffer(); + return client.PublishBuffer(client.buffer.Length); } public static async Task Receive(Stream dataStream, byte packetSize) { @@ -68,10 +68,10 @@ namespace Passer.Control { ClientMsg msg = new(networkId); return SendMsg(client, msg); } - public static bool Publish(Participant client, byte networkId) { - ClientMsg msg = new(networkId); - return PublishMsg(client, msg); - } + //public static bool Publish(Participant client, byte networkId) { + // ClientMsg msg = new(networkId); + // return PublishMsg(client, msg); + //} public static async Task Receive(Stream dataStream, Participant client, byte packetSize) { if (packetSize != length) return false; @@ -80,17 +80,38 @@ namespace Passer.Control { ClientMsg msg = new(buffer); if (client.networkId == 0) { - client.networkId = (byte)(Participant.clients.Count); - NetworkIdMsg.Send(client, client.networkId); + // client.networkId = (byte)(Participant.clients.Count); + // NetworkIdMsg.Send(client, client.networkId); client.messageQueue.Enqueue(msg); } else if (msg.networkId == 0) { - NetworkIdMsg.Send(client, client.networkId); + // NetworkIdMsg.Send(client, client.networkId); client.messageQueue.Enqueue(msg); } return true; } + + public static bool SendTo(Participant participant, byte networkId) { + if (participant == null) + return false; + + byte ix = 0; + participant.buffer[ix++] = ClientMsg.Id; + participant.buffer[ix++] = networkId; + + return participant.SendBuffer(ix); + } + public static bool Publish(Participant participant, byte networkId) { + if (participant == null) + return false; + + byte ix = 0; + participant.buffer[ix++] = ClientMsg.Id; + participant.buffer[ix++] = networkId; + + return participant.PublishBuffer(ix); + } } #endregion Client @@ -131,6 +152,18 @@ namespace Passer.Control { client.messageQueue.Enqueue(msg); return true; } + + public static bool SendTo(Participant participant, byte networkId) { + if (participant == null) + return false; + + byte ix = 0; + participant.buffer[ix++] = NetworkIdMsg.Id; + participant.buffer[ix++] = networkId; + + return participant.SendBuffer(ix); + + } } #endregion Network Id @@ -186,7 +219,7 @@ namespace Passer.Control { public class ThingMsg : IMessage { public const byte length = 5; - public const byte Id = 0x80; + public const byte id = 0x80; public byte networkId; public byte thingId; public byte thingType; @@ -202,7 +235,7 @@ namespace Passer.Control { public override byte Serialize(ref byte[] buffer) { byte ix = 0; - buffer[ix++] = ThingMsg.Id; + buffer[ix++] = ThingMsg.id; buffer[ix++] = this.networkId; buffer[ix++] = this.thingId; buffer[ix++] = this.thingType; @@ -217,7 +250,7 @@ namespace Passer.Control { this.parentId = buffer[ix]; } - public static bool Send(Participant client, CoreThing thing) { + public static bool Send(Participant client, Thing thing) { ThingMsg msg = new(thing.networkId, thing.id, thing.type, thing.parent.id); return SendMsg(client, msg); } @@ -242,6 +275,23 @@ namespace Passer.Control { client.messageQueue.Enqueue(msg); return true; } + + public static bool SendTo(Participant participant, Thing thing) { + if (participant == null || thing == null) + return false; + + byte ix = 0; + participant.buffer[ix++] = ThingMsg.id; + participant.buffer[ix++] = participant.networkId; + participant.buffer[ix++] = thing.id; + participant.buffer[ix++] = thing.type; + if (thing.parent != null) + participant.buffer[ix++] = thing.parent.id; + else + participant.buffer[ix++] = 0; + + return participant.SendBuffer(ix); + } } #endregion Thing @@ -281,7 +331,7 @@ namespace Passer.Control { this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen); } - public static bool Send(Participant client, CoreThing thing) { + public static bool Send(Participant client, Thing thing) { if (string.IsNullOrEmpty(thing.name)) return true; // nothing sent, but still a success! @@ -300,6 +350,25 @@ namespace Passer.Control { client.messageQueue.Enqueue(msg); return true; } + + public static bool SendTo(Participant participant, Thing thing) { + if (participant == null || thing == null || thing.name == null) + return false; + + byte nameLength = (byte)thing.name.Length; + if (participant.buffer.Length < 3 + nameLength) + return false; + + byte ix = 0; + participant.buffer[ix++] = NameMsg.Id; + participant.buffer[ix++] = participant.networkId; + participant.buffer[ix++] = thing.id; + participant.buffer[ix++] = nameLength; + for (int nameIx = 0; nameIx < nameLength; nameIx++) + participant.buffer[ix++] = (byte)thing.name[nameIx]; + + return participant.SendBuffer(ix); + } } #endregion @@ -344,7 +413,7 @@ namespace Passer.Control { url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen); } - public static bool Send(Participant client, CoreThing thing) { + public static bool Send(Participant client, Thing thing) { if (string.IsNullOrEmpty(thing.modelUrl)) return true; // nothing sent, but still a success! @@ -364,6 +433,27 @@ namespace Passer.Control { client.messageQueue.Enqueue(msg); return true; } + + public static bool SendTo(Participant participant, Thing thing) { + if (participant == null || thing == null || thing.modelUrl == null) + return false; + + byte urlLength = (byte)thing.modelUrl.Length; + if (participant.buffer.Length < 3 + urlLength) + return false; + + byte ix = 0; + participant.buffer[ix++] = ModelUrlMsg.Id; + participant.buffer[ix++] = participant.networkId; + participant.buffer[ix++] = thing.id; // Thing Id + LowLevelMessages.SendFloat16(participant.buffer, ref ix, new float16(1.0f)); + + participant.buffer[ix++] = urlLength; + for (int urlIx = 0; urlIx < urlLength; urlIx++) + participant.buffer[ix++] = (byte)thing.modelUrl[urlIx]; + + return participant.SendBuffer(ix); + } } #endregion Model URL @@ -383,11 +473,11 @@ namespace Passer.Control { public const byte Pose_AngularVelocity = 0x08; public Spherical position; - public Quat32 orientation; + public SwingTwist orientation; public Spherical linearVelocity; public Spherical angularVelocity; - public PoseMsg(byte networkId, byte thingId, Spherical position, Quat32 orientation) { + public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation) { this.networkId = networkId; this.thingId = thingId; this.position = position; @@ -400,7 +490,7 @@ namespace Passer.Control { if (this.orientation != null) this.poseType |= Pose_Orientation; else - this.orientation = new Quat32(0, 0, 0, 1); + this.orientation = SwingTwist.zero; } public PoseMsg(byte[] buffer) : base(buffer) { } @@ -411,8 +501,14 @@ namespace Passer.Control { buffer[ix++] = this.thingId; buffer[ix++] = this.poseType; - LowLevelMessages.SendSpherical(buffer, ref ix, this.position); - LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation); + if ((poseType & Pose_Position) != 0) + LowLevelMessages.SendSpherical(buffer, ref ix, this.position); + if ((poseType & Pose_Orientation) != 0) + LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation); + if ((poseType & Pose_LinearVelocity) != 0) + LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity); + if ((poseType & Pose_AngularVelocity) != 0) + LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity); return ix; } public override void Deserialize(byte[] buffer) { @@ -424,14 +520,14 @@ namespace Passer.Control { if ((poseType & Pose_Position) != 0) this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix); if ((poseType & Pose_Orientation) != 0) - this.orientation = LowLevelMessages.ReceiveQuat32(buffer, ref ix); + this.orientation = LowLevelMessages.ReceiveSwingTwist(buffer, ref ix); if ((poseType & Pose_LinearVelocity) != 0) this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); if ((poseType & Pose_AngularVelocity) != 0) this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix); } - public static bool Send(Participant client, byte thingId, Spherical position, Quat32 orientation) { + public static bool Send(Participant client, byte thingId, Spherical position, SwingTwist orientation) { PoseMsg msg = new(client.networkId, thingId, position, orientation); return SendMsg(client, msg); } @@ -446,6 +542,28 @@ namespace Passer.Control { client.messageQueue.Enqueue(msg); return true; } + + public static bool SendTo(Participant participant, Thing thing) { + if (participant == null || thing == null) + return false; + + byte ix = 0; + participant.buffer[ix++] = PoseMsg.Id; + participant.buffer[ix++] = participant.networkId; + participant.buffer[ix++] = thing.id; + participant.buffer[ix++] = thing.poseUpdated; + + if ((thing.poseUpdated & Pose_Position) != 0 && thing.position != null) + LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.position); + if ((thing.poseUpdated & Pose_Orientation) != 0 && thing.orientation != null) + LowLevelMessages.SendQuat32(participant.buffer, ref ix, thing.orientation); + if ((thing.poseUpdated & Pose_LinearVelocity) != 0 && thing.linearVelocity != null) + LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.linearVelocity); + if ((thing.poseUpdated & Pose_AngularVelocity) != 0 && thing.angularVelocity != null) + LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.angularVelocity); + + return participant.SendBuffer(ix); + } } #endregion Pose diff --git a/Participant.cs b/Participant.cs index c880d4a..be90013 100644 --- a/Participant.cs +++ b/Participant.cs @@ -1,14 +1,17 @@ using System.Collections.Generic; using System.Collections.Concurrent; +using System.Net; using System.Net.Sockets; using System.IO; +using System.Threading.Tasks; -namespace Passer.Control +namespace Passer.Control.Core { public class Participant { //public ConnectionMethod connection; + public IPEndPoint endPoint; public UdpClient udpClient; public string ipAddress; public string broadcastIpAddress = "255.255.255.255"; @@ -21,45 +24,147 @@ namespace Passer.Control public readonly ConcurrentQueue messageQueue = new(); - public static Participant GetClient(string ipAddress, int port) + public Participant GetClient(string ipAddress, int port) { - foreach (Participant c in clients) + foreach (Participant c in others) { if (c.ipAddress == ipAddress && c.port == port) return c; } return null; } - static public List clients = new List(); + public List others = new List(); - public Participant(UdpClient udpClient, int port) + #region Init + + public Participant() { + this.dataStream = new EchoStream(); + others.Add(this); + } + + public Participant(UdpClient udpClient, int port) : this() { this.udpClient = udpClient; this.ipAddress = null; this.port = port; - this.dataStream = new EchoStream(); - clients.Add(this); + //this.dataStream = new EchoStream(); + //clients.Add(this); } - public bool SendBuffer() - { - if (this.ipAddress == null) - return false; + #endregion Init - //UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}"); - this.udpClient.Send(this.buffer, this.buffer.Length, this.ipAddress, this.port); + #region Update + + private static readonly float publishInterval = 3.0f; + private float nextPublishMe = 0; + + + public virtual void Update(float currentTime) { + if (currentTime > this.nextPublishMe) { + ClientMsg.Publish(this, this.networkId); + this.nextPublishMe = currentTime + Participant.publishInterval; + } + + for (int ix = 0; ix < this.others.Count; ix++) { + Participant client = this.others[ix]; + if (client == null) + continue; + + client.ProcessMessages(); + } + Thing.UpdateAll(currentTime); + } + + #endregion Update + + #region Send + + public bool SendBuffer(int bufferSize) + { + //if (this.ipAddress == null) + // return false; + + UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}"); + //this.udpClient.Send(this.buffer, bufferSize, this.ipAddress, this.port); + this.udpClient.Send(this.buffer, bufferSize, this.endPoint); return true; } - public bool PublishBuffer() + public bool PublishBuffer(int bufferSize) { if (this.broadcastIpAddress == null) return false; - this.udpClient.Send(this.buffer, this.buffer.Length, this.broadcastIpAddress, this.port); + this.udpClient.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port); return true; } + #endregion + + #region Receive + + public async Task ReceiveData() { + while (true) { + byte packetSize = (byte)this.dataStream.ReadByte(); + if (packetSize != 0xFF) + await ReceiveData(this.dataStream, this, packetSize); + // else timeout + } + } + + public static async Task ReceiveData(Stream dataStream, Participant client, byte packetSize) { + byte msgId = (byte)dataStream.ReadByte(); + if (msgId == 0xFF) { + // Timeout + return; + } + + //UnityEngine.Debug.Log($"R {msgId} from {client.ipAddress}"); + bool result = false; + switch (msgId) { + case ClientMsg.Id: // 0xA0 / 160 + result = await ClientMsg.Receive(dataStream, client, packetSize); + break; + case NetworkIdMsg.Id: // 0xA1 / 161 + result = await NetworkIdMsg.Receive(dataStream, client, packetSize); + break; + case InvestigateMsg.Id: // 0x81 + result = await InvestigateMsg.Receive(dataStream, client, packetSize); + break; + case ThingMsg.id: // 0x80 / 128 + result = await ThingMsg.Receive(dataStream, client, packetSize); + break; + case NameMsg.Id: // 0x91 / 145 + result = await NameMsg.Receive(dataStream, client, packetSize); + break; + case ModelUrlMsg.Id: // 0x90 / 144 + result = await ModelUrlMsg.Receive(dataStream, client, packetSize); + break; + case PoseMsg.Id: // 0x10 / 16 + result = await PoseMsg.Receive(dataStream, client, packetSize); + break; + case CustomMsg.Id: // 0xB1 / 177 + result = await CustomMsg.Receive(dataStream, client, packetSize); + break; + case TextMsg.Id: // 0xB0 / 176 + result = await TextMsg.Receive(dataStream, client, packetSize); + break; + case DestroyMsg.Id: // 0x20 / 32 + result = await DestroyMsg.Receive(dataStream, client, packetSize); + break; + default: + break; + } + if (result == false) { + packetSize = msgId; // skip 1 byte, msgId is possibly a packet size byte + await ReceiveData(dataStream, client, packetSize); + } + } + + #endregion + + #region Process + public virtual void ProcessMessages() { while (this.messageQueue.TryDequeue(out IMessage msg)) @@ -129,7 +234,7 @@ namespace Passer.Control private void ForwardMessage(IMessage msg) { - foreach (Participant client in Participant.clients) + foreach (Participant client in others) { if (client == this) continue; @@ -137,5 +242,7 @@ namespace Passer.Control IMessage.SendMsg(client, msg); } } + + #endregion } } \ No newline at end of file diff --git a/SiteServer.cs b/SiteServer.cs index eec2163..7986caa 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -1,7 +1,7 @@ using System.IO; using System.Threading.Tasks; -namespace Passer.Control { +namespace Passer.Control.Core { public static class SiteServer { @@ -33,7 +33,7 @@ namespace Passer.Control { case InvestigateMsg.Id: // 0x81 result = await InvestigateMsg.Receive(dataStream, client, packetSize); break; - case ThingMsg.Id: // 0x80 / 128 + case ThingMsg.id: // 0x80 / 128 result = await ThingMsg.Receive(dataStream, client, packetSize); break; case NameMsg.Id: // 0x91 / 145 diff --git a/Thing.cs b/Thing.cs index 21fa3c6..356cf2f 100644 --- a/Thing.cs +++ b/Thing.cs @@ -1,40 +1,189 @@ using System.Collections.Generic; +using Passer.LinearAlgebra; -namespace Passer.Control -{ - public class CoreThing - { +namespace Passer.Control.Core { + + public class Thing { public Participant participant; + + public delegate void ChangeHandler(); + public delegate void SphericalHandler(Spherical v); + public byte networkId; public byte id; - public CoreThing parent; - public List children; - public byte type; - public string name; - public string modelUrl; - //protected Sensor sensor; - protected virtual void Init() - { + public event ChangeHandler OnParentChanged; + private Thing _parent; + public Thing parent { + get => _parent; + set { + if (_parent == value) + return; + + if (value == null) { + _parent.RemoveChild(this); + _parent = null; + } else { + value.AddChild(this); + OnParentChanged?.Invoke(); + } + } } - public CoreThing(Participant client, byte networkId, byte thingId, byte thingType = 0) - { + public void AddChild(Thing child) { + if (children.Find(thing => thing == child) != null) + return; + child._parent = this; + children.Add(child); + } + public void RemoveChild(Thing child) { + children.Remove(child); + } + + [System.NonSerialized] + public List children = new List(); + public byte type; + + public event ChangeHandler OnNameChanged; + private string _name; + public string name { + get => _name; + set { + if (_name != value) { + _name = value; + OnNameChanged?.Invoke(); + } + } + } + + public string modelUrl; + + public byte poseUpdated = 0x00; + public event ChangeHandler OnPositionChanged; + private Spherical _position; + public Spherical position { + get { return _position; } + set { + if (_position != value) { + _position = value; + OnPositionChanged?.Invoke(); + } + } + } + + public event ChangeHandler OnOrientationChanged; + private SwingTwist _orientation; + public SwingTwist orientation { + get { return _orientation; } + set { + if (_orientation != value) { + _orientation = value; + OnOrientationChanged?.Invoke(); + } + } + } + + public event SphericalHandler OnLinearVelocityChanged; + private Spherical _linearVelocity; + public Spherical linearVelocity { + get => _linearVelocity; + set { + if (_linearVelocity != value) { + _linearVelocity = value; + OnLinearVelocityChanged?.Invoke(_linearVelocity); + } + } + } + public Spherical angularVelocity; + + public virtual void Init(bool invokeEvent = true) { + Thing.Add(this, invokeEvent); + } + + public Thing(bool initialize = true) { + if (initialize) { + //this.Init(); + Thing.Add(this); + } + } + public Thing(Participant client, byte networkId, byte thingId, byte thingType = 0) { this.participant = client; this.id = thingId; this.type = thingType; this.networkId = networkId; this.Init(); - allThings.Add(this); + Thing.Add(this); } - private static readonly List allThings = new(); + public virtual void Update(float currentTime) { + // should recurse over children... + } - public static CoreThing Get(byte networkId, byte thingId) - { - CoreThing thing = allThings.Find(aThing => aThing.networkId == networkId && aThing.id == thingId); + public virtual void ProcessBytes(byte[] bytes) { + //if (sensor != null) + // sensor.ProcessBytes(bytes); + } + + // Experimental + + public float stressLevel = 0; + + protected delegate void ReceptorFunc(Sensor sensor); + protected void SetupReceptor(Sensor sensor, ReceptorFunc receptor) { + sensor.Signaller += (sensor => Receptor(receptor, sensor)); + } + protected void Receptor(ReceptorFunc receptor, Sensor sensor) { + if (sensor.signalStrength <= stressLevel) + return; + + receptor(sensor); + } + + //---------- All Things + + private static readonly List allThings = new(); + + public delegate void ThingHandler(Thing t); + public static event ThingHandler OnNewThing; + + public static void Add(Thing thing, bool invokeEvent = true) { + Thing foundThing = Get(thing.networkId, thing.id); + + if (foundThing == null) { + if (thing.id == 0) + thing.id = (byte)(allThings.Count + 1); + allThings.Add(thing); + if (invokeEvent) + OnNewThing?.Invoke(thing); + UnityEngine.Debug.Log($"Add thing [{thing.networkId}/{thing.id}] {thing.name}"); + } + } + + public static Thing Get(byte networkId, byte thingId) { + Thing thing = allThings.Find(aThing => IsThing(aThing, networkId, thingId)); return thing; } + private static bool IsThing(Thing thing, byte networkId, byte thingId) { + if (thing == null) + return false; + return (thing.networkId == networkId ) && (thing.id == thingId); + } + + public static void Remove(byte networkId, byte thingId) { + allThings.RemoveAll(t => t.networkId == networkId && t.id == thingId); + } + + public static Thing[] GetAllThings() { + return allThings.ToArray(); + } + + public static void UpdateAll(float currentTime) { + foreach (Thing thing in allThings) { + if (thing.parent == null) // update only root things + thing.Update(currentTime); + } + } + } } diff --git a/Thing.cs.meta b/Thing.cs.meta index 84adfb3..0d02170 100644 --- a/Thing.cs.meta +++ b/Thing.cs.meta @@ -1,5 +1,5 @@ fileFormatVersion: 2 -guid: 0e8b48bc91446304eaaccbfdde4cc4af +guid: 7429282ee0e367445bd1e2111631b27d MonoImporter: externalObjects: {} serializedVersion: 2