diff --git a/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/LowLevelMessages.cs b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/LowLevelMessages.cs index a976ddf..f279dbc 100644 --- a/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/LowLevelMessages.cs +++ b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/LowLevelMessages.cs @@ -1,6 +1,13 @@ 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); + } + public static Spherical ReceiveSpherical(byte[] data, ref uint ix) { float horizontal = ReceiveAngle8(data, ref ix); float vertical = ReceiveAngle8(data, ref ix); @@ -9,6 +16,23 @@ public class LowLevelMessages { return v; } + public static void SendQuat32(byte[] buffer, ref uint ix, Quat32 q) { + int qx = (int)(q.x * 127 + 128); + int qy = (int)(q.y * 127 + 128); + int qz = (int)(q.z * 127 + 128); + int qw = (int)(q.w * 255); + if (q.w < 0) { + qx = -qx; + qy = -qy; + qz = -qz; + qw = -qw; + } + + buffer[ix++] = (byte)qx; + buffer[ix++] = (byte)qy; + buffer[ix++] = (byte)qz; + buffer[ix++] = (byte)qw; + } public static Quat32 ReceiveQuat32(byte[] data, ref uint ix) { Quat32 q = new( (data[ix++] - 128.0F) / 127.0F, @@ -18,11 +42,26 @@ public class LowLevelMessages { return q; } + public static void SendAngle8(byte[] buffer, ref uint ix, float angle) { + // Normalize angle + while (angle >= 180) + angle -= 360; + while (angle < -180) + angle += 360; + buffer[ix++] = (byte)((angle / 360.0f) * 256.0f); + } + public static float ReceiveAngle8(byte[] data, ref uint ix) { float value = (data[ix++] * 180) / 128.0F; return value; } + public static void SendFloat16(byte[] data, ref uint ix, float16 f) { + ushort binary = f.GetBinary(); + data[ix++] = (byte)(binary >> 8); + data[ix++] = (byte)(binary & 255); + } + public static float ReceiveFloat16(byte[] data, ref uint ix) { ushort value = (ushort)(data[ix++] << 8 | data[ix++]); float16 f16 = new(); @@ -30,6 +69,4 @@ public class LowLevelMessages { float f = f16.toFloat(); return f; } - - } diff --git a/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/Messages.cs b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/Messages.cs index acbecad..b9c7e2b 100644 --- a/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/Messages.cs +++ b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/Messages.cs @@ -62,6 +62,18 @@ namespace Passer.Control { 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()); + } + public static bool PublishMsg(Client client, byte[] data) { + if (client == null) + return false; + + client.udpClient.Send(data, data.Length, "127.0.0.1", 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); @@ -86,7 +98,13 @@ namespace Passer.Control { } public ClientMsg(byte[] data) : base(data) { } - public override void Deserialize(byte[] data) { + public override byte[] Serialize() { + byte[] buffer = new byte[ClientMsg.length]; + buffer[0] = ClientMsg.Id; + buffer[1] = networkId; + return buffer; + } + public override void Deserialize(byte[] data) { base.Deserialize(data); uint ix = 0; networkId = data[ix]; @@ -96,6 +114,10 @@ namespace Passer.Control { 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; @@ -212,7 +234,7 @@ namespace Passer.Control { #region Thing public class ThingMsg : IMessage { - public const byte length = 4; + public const byte length = 5; public const byte Id = 0x80; public byte objectId; public byte objectType; @@ -227,7 +249,7 @@ namespace Passer.Control { } public static bool Send(Client client, byte thingId, byte thingType, byte parentId) { - byte[] data = new byte[4]; + byte[] data = new byte[ThingMsg.length]; data[0] = ThingMsg.Id; data[1] = client.networkId; data[2] = thingId; @@ -370,12 +392,27 @@ namespace Passer.Control { 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 PoseMsg(byte[] data) : base(data) { } - public override void Deserialize(byte[] data) { + public override byte[] Serialize() { + byte[] buffer = new byte[PoseMsg.length]; + uint ix = 0; + buffer[ix++] = PoseMsg.Id; + buffer[ix++] = this.thingId; + buffer[ix++] = this.poseType; + + LowLevelMessages.SendSpherical(buffer, ref ix, position); + LowLevelMessages.SendQuat32(buffer, ref ix, orientation); + return buffer; + } + public override void Deserialize(byte[] data) { uint ix = 0; thingId = data[ix++]; poseType = data[ix++]; diff --git a/Runtime/HumanoidControl/Scripts/Networking/Roboid/HumanoidPlayerRoboid.cs b/Runtime/HumanoidControl/Scripts/Networking/Roboid/HumanoidPlayerRoboid.cs index 231d94f..141418c 100644 --- a/Runtime/HumanoidControl/Scripts/Networking/Roboid/HumanoidPlayerRoboid.cs +++ b/Runtime/HumanoidControl/Scripts/Networking/Roboid/HumanoidPlayerRoboid.cs @@ -188,8 +188,8 @@ namespace Passer.Humanoid { #region Update float lastSend; - float lastClientMsg; const float clientMsgInterval = 3; + float lastClientMsg = -clientMsgInterval; protected virtual void LateUpdate() { foreach (Client client in Client.clients) { @@ -211,7 +211,7 @@ namespace Passer.Humanoid { } lastSend = Time.time; } - if (Time.time + clientMsgInterval > lastClientMsg) { + if (Time.time > lastClientMsg + clientMsgInterval) { SendClientMsg(humanoids[0]); // We just need it for the networkId lastClientMsg = Time.time; } @@ -283,7 +283,7 @@ namespace Passer.Humanoid { Debug.Log("Send ClientMsg " + humanoid.nwId); //ClientMsg clientMsg = new((byte)humanoid.nwId); //SendMsg(clientMsg); - ClientMsg.Send(client, (byte)humanoid.nwId); + ClientMsg.Publish(client, (byte)humanoid.nwId); } #endregion Client @@ -375,6 +375,7 @@ namespace Passer.Humanoid { return; this.networkId = msg.networkId; + client.networkId = msg.networkId; GameObject networkingObj = this.GetGameObject(msg.networkId); if (networkingObj == null) { if (this.debug <= HumanoidNetworking.DebugLevel.Error) @@ -387,6 +388,7 @@ namespace Passer.Humanoid { DebugLog("Remote Humanoid " + msg.networkId + "/" + 0 + "not found"); return; } + remoteHumanoid.nwId = msg.networkId; SendThing(client, remoteHumanoid); SendName(remoteHumanoid, 0, "Humanoid"); @@ -512,8 +514,11 @@ namespace Passer.Humanoid { if (debug <= HumanoidNetworking.DebugLevel.Debug) Debug.Log("Send SubThing " + humanoid.humanoidId + " nwId: " + humanoid.nwId); - ThingMsg.Send(client, (byte)bone.boneId, 0x00, (byte)bone.parent.boneId); - + if (bone.parent != null) + ThingMsg.Send(client, (byte)bone.boneId, 0x00, (byte)bone.parent.boneId); + else + ThingMsg.Send(client, (byte)bone.boneId, 0x00, 0); + //SubThingMsg thingMsg; //if (bone.parent != null) // thingMsg = new((byte)bone.boneId, (byte)bone.parent.boneId, bone.bone.transform.position);