diff --git a/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore.meta b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore.meta new file mode 100644 index 0000000..b6e25b9 --- /dev/null +++ b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 98ee056958e257046a58cddcbcb661e3 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/EchoStream.cs b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/EchoStream.cs new file mode 100644 index 0000000..c9ae949 --- /dev/null +++ b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/EchoStream.cs @@ -0,0 +1,173 @@ +using System; +using System.IO; +using System.Threading.Tasks; +using System.Threading; +using System.Collections.Concurrent; + +public class EchoStream : Stream { + public override bool CanTimeout { get; } = true; + public override int ReadTimeout { get; set; } = Timeout.Infinite; + public override int WriteTimeout { get; set; } = Timeout.Infinite; + public override bool CanRead { get; } = true; + public override bool CanSeek { get; } = false; + public override bool CanWrite { get; } = true; + + public bool CopyBufferOnWrite { get; set; } = false; + + private readonly object _lock = new object(); + + // Default underlying mechanism for BlockingCollection is ConcurrentQueue, which is what we want + private readonly BlockingCollection _Buffers; + private int _maxQueueDepth = 10; + + private byte[] m_buffer = null; + private int m_offset = 0; + private int m_count = 0; + + private bool m_Closed = false; + private bool m_FinalZero = false; //after the stream is closed, set to true after returning a 0 for read() + public override void Close() { + m_Closed = true; + + // release any waiting writes + _Buffers.CompleteAdding(); + } + + public bool DataAvailable { + get { + return _Buffers.Count > 0; + } + } + + private long _Length = 0L; + public override long Length { + get { + return _Length; + } + } + + private long _Position = 0L; + public override long Position { + get { + return _Position; + } + set { + throw new NotImplementedException(); + } + } + + public EchoStream() : this(10) { + } + + public EchoStream(int maxQueueDepth) { + _maxQueueDepth = maxQueueDepth; + _Buffers = new BlockingCollection(_maxQueueDepth); + } + + // we override the xxxxAsync functions because the default base class shares state between ReadAsync and WriteAsync, which causes a hang if both are called at once + public new Task WriteAsync(byte[] buffer, int offset, int count) { + return Task.Run(() => Write(buffer, offset, count)); + } + + // we override the xxxxAsync functions because the default base class shares state between ReadAsync and WriteAsync, which causes a hang if both are called at once + public new Task ReadAsync(byte[] buffer, int offset, int count) { + return Task.Run(() => { + return Read(buffer, offset, count); + }); + } + + public override void Write(byte[] buffer, int offset, int count) { + if (m_Closed || buffer.Length - offset < count || count <= 0) + return; + + byte[] newBuffer; + if (!CopyBufferOnWrite && offset == 0 && count == buffer.Length) + newBuffer = buffer; + else { + newBuffer = new byte[count]; + System.Buffer.BlockCopy(buffer, offset, newBuffer, 0, count); + } + if (!_Buffers.TryAdd(newBuffer, WriteTimeout)) + throw new TimeoutException("EchoStream Write() Timeout"); + + _Length += count; + } + + public override int Read(byte[] buffer, int offset, int count) { + if (count == 0) + return 0; + lock (_lock) { + if (m_count == 0 && _Buffers.Count == 0) { + if (m_Closed) { + if (!m_FinalZero) { + m_FinalZero = true; + return 0; + } + else { + return -1; + } + } + + if (_Buffers.TryTake(out m_buffer, ReadTimeout)) { + m_offset = 0; + m_count = m_buffer.Length; + } + else { + if (m_Closed) { + if (!m_FinalZero) { + m_FinalZero = true; + return 0; + } + else { + return -1; + } + } + else { + return 0; + } + } + } + + int returnBytes = 0; + while (count > 0) { + if (m_count == 0) { + if (_Buffers.TryTake(out m_buffer, 0)) { + m_offset = 0; + m_count = m_buffer.Length; + } + else + break; + } + + var bytesToCopy = (count < m_count) ? count : m_count; + System.Buffer.BlockCopy(m_buffer, m_offset, buffer, offset, bytesToCopy); + m_offset += bytesToCopy; + m_count -= bytesToCopy; + offset += bytesToCopy; + count -= bytesToCopy; + + returnBytes += bytesToCopy; + } + + _Position += returnBytes; + + return returnBytes; + } + } + + public override int ReadByte() { + byte[] returnValue = new byte[1]; + return (Read(returnValue, 0, 1) <= 0 ? -1 : (int)returnValue[0]); + } + + public override void Flush() { + } + + public override long Seek(long offset, SeekOrigin origin) { + throw new NotImplementedException(); + } + + public override void SetLength(long value) { + throw new NotImplementedException(); + } +} \ No newline at end of file diff --git a/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/EchoStream.cs.meta b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/EchoStream.cs.meta new file mode 100644 index 0000000..063a4f2 --- /dev/null +++ b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/EchoStream.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 422516a56cbf14d46aaa0b1bc09115bf \ No newline at end of file diff --git a/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/Messages.cs b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/Messages.cs index b517f4d..acbecad 100644 --- a/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/Messages.cs +++ b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/Messages.cs @@ -3,6 +3,7 @@ using System.Collections.Concurrent; using System.IO; using System.Net.Sockets; using System.Threading.Tasks; +using System; namespace Passer.Control { @@ -76,16 +77,25 @@ namespace Passer.Control { #region Client public class ClientMsg : IMessage { + public const byte Id = 0xA0; public const byte length = 2; - public byte clientId; + public byte networkId; + public ClientMsg(byte networkId) { + this.networkId = networkId; + } public ClientMsg(byte[] data) : base(data) { } + public override void Deserialize(byte[] data) { base.Deserialize(data); uint ix = 0; - clientId = data[ix++]; + networkId = data[ix]; } + public static bool Send(Client client, byte networkId) { + ClientMsg msg = new(networkId); + return SendMsg(client, msg); + } public static async Task Receive(Stream dataStream, Client client, byte packetSize) { if (packetSize != length) return false; @@ -95,12 +105,12 @@ namespace Passer.Control { if (client.networkId == 0) { client.networkId = (byte)(Client.clients.Count); - NetworkIdMsg.Send(client); + NetworkIdMsg.Send(client, client.networkId); //if (string.IsNullOrEmpty(sceneUrl) == false) //SendModelUrl(client, sceneUrl); } - else if (msg.clientId == 0) { - NetworkIdMsg.Send(client); + else if (msg.networkId == 0) { + NetworkIdMsg.Send(client, client.networkId); //if (string.IsNullOrEmpty(sceneUrl) == false) //SendModelUrl(client, sceneUrl); } @@ -116,20 +126,92 @@ namespace Passer.Control { public class NetworkIdMsg : IMessage { public const byte Id = 0xA1; public const byte length = 2; + public byte networkId; - public static bool Send(Client client) { - byte[] data = new byte[NetworkIdMsg.length]; - data[0] = NetworkIdMsg.Id; - data[1] = client.networkId; - return SendMsg(client, data); + 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 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); + NetworkIdMsg msg = new(buffer); + client.messageQueue.Enqueue(msg); + return true; + } } - #endregion Network Id + #endregion Network Id - #region Thing + #region Investigate - public class ThingMsg : IMessage { + 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; @@ -144,13 +226,13 @@ namespace Passer.Control { parentId = data[ix]; } - public static bool Send(Client client, byte networkId, byte thingId, byte thingType) { + public static bool Send(Client client, byte thingId, byte thingType, byte parentId) { byte[] data = new byte[4]; data[0] = ThingMsg.Id; - data[1] = networkId; + data[1] = client.networkId; data[2] = thingId; data[3] = thingType; - data[4] = 0x00; // parent not supported yet + data[4] = parentId; return SendMsg(client, data); } public static async Task Receive(Stream dataStream, Client client, byte packetSize) { @@ -170,19 +252,39 @@ namespace Passer.Control { #region Name public class NameMsg : IMessage { + public const byte Id = 0x91; // 145 + public const byte length = 3; public byte networkId = 0; - public byte objectId; + public byte thingId; public byte len; public string name; + public NameMsg(byte thingId, string name) { + this.thingId = thingId; + this.name = name; + } public NameMsg(byte[] data) : base(data) { } - public override void Deserialize(byte[] data) { + + public override byte[] Serialize() { + byte[] data = new byte[length + this.name.Length]; + data[0] = NameMsg.Id; + data[1] = this.thingId; + data[2] = (byte)this.name.Length; + for (int ix = 0; ix < this.name.Length; ix++) + data[3 + ix] = (byte)this.name[ix]; + return data; + } + public override void Deserialize(byte[] data) { uint ix = 0; - this.objectId = data[ix++]; + 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 NameMsg(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); @@ -198,12 +300,13 @@ namespace Passer.Control { public class ModelUrlMsg : IMessage { public const byte Id = 0x90; // (144) Model URL - public byte objectId; + public byte thingId; public Spherical position; public float scale; public string url; - public ModelUrlMsg(string url, float scale = 1) { + public ModelUrlMsg(byte thingId, string url, float scale = 1) { + this.thingId = thingId; this.url = url; this.scale = scale; this.position = Spherical.zero; @@ -213,7 +316,7 @@ namespace Passer.Control { public override byte[] Serialize() { byte[] data = new byte[this.url.Length + 9]; data[0] = ModelUrlMsg.Id; - data[1] = 0x00; // Thing Id + data[1] = this.thingId; // Thing Id // data[2]..[5] == position 0, 0, 0 data[6] = 0x3C; // Dummy float16 value 1 data[7] = 0x00; @@ -225,15 +328,15 @@ namespace Passer.Control { } public override void Deserialize(byte[] data) { uint ix = 0; - this.objectId = data[ix++]; + 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, string modelUrl) { - ModelUrlMsg msg = new(modelUrl); + 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) { @@ -249,14 +352,28 @@ namespace Passer.Control { #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[] data) : base(data) { } + 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; + if (this.orientation != null) + this.poseType |= Pose_Orientation; + } + public PoseMsg(byte[] data) : base(data) { } public override void Deserialize(byte[] data) { uint ix = 0; @@ -269,6 +386,10 @@ namespace Passer.Control { 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; @@ -278,7 +399,6 @@ namespace Passer.Control { client.messageQueue.Enqueue(msg); return true; - } } @@ -339,6 +459,7 @@ namespace Passer.Control { #region Destroy public class DestroyMsg : IMessage { + public const byte Id = 0x20; public const byte length = 2; public byte objectId; diff --git a/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/SiteServer.cs b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/SiteServer.cs new file mode 100644 index 0000000..cd01666 --- /dev/null +++ b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/SiteServer.cs @@ -0,0 +1,85 @@ + +using System.IO; +using System.Threading.Tasks; + +namespace Passer.Control { + + public static class SiteServer { + public static async Task ReceiveData(Stream dataStream, Client client) { + while (true) { + byte packetSize = (byte)dataStream.ReadByte(); + if (packetSize == 0xFF) + //Debug.Log("Receive timeout"); + // Timeout + ; + else + await ReceiveData(dataStream, client, packetSize); + } + } + + public static async Task ReceiveData(Stream dataStream, Client client, byte packetSize) { + byte msgId = (byte)dataStream.ReadByte(); + if (msgId == 0xFF) { + // Timeout + return; + } + + bool result = false; + switch (msgId) { + case PoseMsg.Id: // Object Pose (16) + result = await PoseMsg.Receive(dataStream, client, packetSize); + break; + case DestroyMsg.Id: // Destroy object (32) + result = await DestroyMsg.Receive(dataStream, client, packetSize); + break; + case ThingMsg.Id: + result = await ThingMsg.Receive(dataStream, client, packetSize); + break; + case InvestigateMsg.Id: + result = await InvestigateMsg.Receive(dataStream, client, packetSize); + break; + case ModelUrlMsg.Id: // Model URL (144) + result = await BytesMsg.Receive(dataStream, client, packetSize); + break; + case NameMsg.Id: // Object Name (145) + result = await NameMsg.Receive(dataStream, client, packetSize); + break; + case ClientMsg.Id: + result = await ClientMsg.Receive(dataStream, client, packetSize); + break; + case NetworkIdMsg.Id: + result = await NetworkIdMsg.Receive(dataStream, client, packetSize); + break; + //case TextMsg.Id: // Text (176) + // result = await TextMsg.Receive(dataStream, client, packetSize); + // break; + case BytesMsg.Id: + result = await BytesMsg.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); + } + } + + public static void ProcessMessage(ISiteServer site, Client client, IMessage msg) { + switch (msg) { + case NetworkIdMsg networkId: + site.ProcessNetworkId(client, networkId); + break; + case ModelUrlMsg modelUrl: + site.ProcessModelUrl(client, modelUrl); + break; + } + } + } + + public interface ISiteServer { + + public void ProcessNetworkId(Client client, NetworkIdMsg networkId); + public void ProcessModelUrl(Client client, ModelUrlMsg modelUrl); + } +} \ No newline at end of file diff --git a/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/SiteServer.cs.meta b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/SiteServer.cs.meta new file mode 100644 index 0000000..02fd171 --- /dev/null +++ b/Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/SiteServer.cs.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: 53345abb9310d344baa67c19a8d8249f \ No newline at end of file diff --git a/Runtime/HumanoidControl/Scripts/Networking/Roboid/HumanoidPlayerRoboid.cs b/Runtime/HumanoidControl/Scripts/Networking/Roboid/HumanoidPlayerRoboid.cs index 3841488..231d94f 100644 --- a/Runtime/HumanoidControl/Scripts/Networking/Roboid/HumanoidPlayerRoboid.cs +++ b/Runtime/HumanoidControl/Scripts/Networking/Roboid/HumanoidPlayerRoboid.cs @@ -4,16 +4,18 @@ using System.Collections.Generic; using System.IO; using System.Net; using System.Net.Sockets; +using System.Threading.Tasks; using UnityEngine; using GLTFast; #if hNW_ROBOID +using Passer.Control; using Passer.LinearAlgebra; namespace Passer.Humanoid { - public partial class HumanoidPlayer : MonoBehaviour, IHumanoidNetworking { + public partial class HumanoidPlayer : MonoBehaviour, IHumanoidNetworking, ISiteServer { [SerializeField] protected float _sendRate = 25; @@ -33,7 +35,9 @@ namespace Passer.Humanoid { protected IPEndPoint endPoint; protected object obj; protected AsyncCallback receiveCallback; - ConcurrentQueue messageQueue = new ConcurrentQueue(); + protected Client client; + //ConcurrentQueue messageQueue = new ConcurrentQueue(); + protected Stream dataStream; public byte networkId; @@ -135,8 +139,42 @@ namespace Passer.Humanoid { endPoint = new IPEndPoint(IPAddress.Any, nssPort + 3); udpClient = new UdpClient(endPoint); - receiveCallback = new AsyncCallback(Receiver); + receiveCallback = new System.AsyncCallback(result => ReceiveUDP(client, result)); udpClient.BeginReceive(receiveCallback, obj); + client = Client.NewUDPClient(udpClient, endPoint.Address.ToString(), endPoint.Port); + + // + //endpoint = new IPEndPoint(IPAddress.Any, sitePort); + //udpClient = new UdpClient(sitePort); + + //DebugLog($"Start receiving on port {sitePort}"); + dataStream = new EchoStream(); + //client = newClient ? Client.NewUDPClient(udpClient, endpoint.Address.ToString(), endpoint.Port) : Client.clients[0]; + //udpClient.BeginReceive(new System.AsyncCallback(result => ReceiveUDP(client, result)), null); + Task task = Task.Run(() => SiteServer.ReceiveData(dataStream, client)); + } + + protected void ReceiveUDP(Client client, IAsyncResult result) { + if (udpClient == null) + return; + + if (udpClient.Client.Connected) { + byte[] data = udpClient.EndReceive(result, ref endPoint); + dataStream.WriteByte((byte)data.Length); + dataStream.Write(data, 0, data.Length); + if (client.ipAddress == null) { + client.ipAddress = endPoint.Address.ToString(); + client.port = endPoint.Port; + // I thing sending now is too soon, we are currenlty processing a message... + //SendNetworkId(client); + //if (string.IsNullOrEmpty(sceneUrl) == false) + // SendModelUrl(client, sceneUrl); + } + } + else { + Debug.Log("Client disconnected"); + } + udpClient.BeginReceive(new System.AsyncCallback(result => ReceiveUDP(client, result)), null); } protected void OnApplicationQuit() { @@ -154,10 +192,15 @@ namespace Passer.Humanoid { const float clientMsgInterval = 3; protected virtual void LateUpdate() { - while (messageQueue.TryDequeue(out HumanoidNetworking.IMessage msg)) { - ProcessMessage(msg); + foreach (Client client in Client.clients) { + while (client.messageQueue.TryDequeue(out Passer.Control.IMessage msg)) + SiteServer.ProcessMessage(this, client, msg); } + //while (messageQueue.TryDequeue(out HumanoidNetworking.IMessage msg)) { + // ProcessMessage(msg); + //} + if (Time.time > lastSend + 1 / sendRate) { if (humanoids != null) { foreach (HumanoidControl humanoid in humanoids) { @@ -174,109 +217,160 @@ namespace Passer.Humanoid { } } - protected virtual void ProcessMessage(HumanoidNetworking.IMessage msg) { - switch (msg) { - case NetworkIdMsg networkId: - ProcessNetworkId(networkId); - break; - case ModelUrlMsg modelUrl: - ProcessModelURL(modelUrl); - break; - } - } + //protected virtual void ProcessMessage(HumanoidNetworking.IMessage msg) { + // switch (msg) { + // case NetworkIdMsg networkId: + // ProcessNetworkId(networkId); + // break; + // case ModelUrlMsg modelUrl: + // ProcessModelURL(modelUrl); + // break; + // } + //} - void Receiver(IAsyncResult result) { - // Take care of buffered messages after the client has been closed - if (udpClient == null || udpClient.Client.Connected == false) - return; + //void Receiver(IAsyncResult result) { + // // Take care of buffered messages after the client has been closed + // if (udpClient == null || udpClient.Client.Connected == false) + // return; - byte[] data = (udpClient.EndReceive(result, ref endPoint)); + // byte[] data = (udpClient.EndReceive(result, ref endPoint)); - ReceivePacket(data); + // ReceivePacket(data); - udpClient.BeginReceive(receiveCallback, obj); - } + // udpClient.BeginReceive(receiveCallback, obj); + //} - void ReceivePacket(byte[] data) { - byte msgId = data[0]; - switch (msgId) { - case 0xA1: // (161) Network Id - this.ReceiveNetworkId(data); break; - case ModelUrlMsg.Id: - this.ReceiveModelUrl(data); break; - } + //void ReceivePacket(byte[] data) { + // byte msgId = data[0]; + // switch (msgId) { + // case 0xA1: // (161) Network Id + // this.ReceiveNetworkId(data); break; + // case ModelUrlMsg.Id: + // this.ReceiveModelUrl(data); break; + // } - } + //} #endregion Update #region Client - class ClientMsg : HumanoidNetworking.IMessage { - public const byte Id = 0xA0; // 160 - public const byte length = 2; - public byte networkId; + //class ClientMsg : HumanoidNetworking.IMessage { + // public const byte Id = 0xA0; // 160 + // public const byte length = 2; + // public byte networkId; - public ClientMsg(byte networkId) { - this.networkId = networkId; - } - public ClientMsg(byte[] data) : base(data) { } + // public ClientMsg(byte networkId) { + // this.networkId = networkId; + // } + // public ClientMsg(byte[] data) : base(data) { } - public override byte[] Serialize() { - byte[] data = new byte[ClientMsg.length]; - data[0] = ClientMsg.Id; - data[1] = this.networkId; - return data; - } - public override void Deserialize(byte[] data) { - base.Deserialize(data); - uint ix = 0; - networkId = data[ix++]; - } - } + // public override byte[] Serialize() { + // byte[] data = new byte[ClientMsg.length]; + // data[0] = ClientMsg.Id; + // data[1] = this.networkId; + // return data; + // } + // public override void Deserialize(byte[] data) { + // base.Deserialize(data); + // uint ix = 0; + // networkId = data[ix++]; + // } + //} protected void SendClientMsg(HumanoidControl humanoid) { if (debug <= HumanoidNetworking.DebugLevel.Debug) Debug.Log("Send ClientMsg " + humanoid.nwId); - ClientMsg clientMsg = new((byte)humanoid.nwId); - SendMsg(clientMsg); + //ClientMsg clientMsg = new((byte)humanoid.nwId); + //SendMsg(clientMsg); + ClientMsg.Send(client, (byte)humanoid.nwId); } #endregion Client - #region Messages + #region Pose - #region NetworkId + public virtual void UpdateHumanoidPose(HumanoidControl humanoid) { + if (debug <= HumanoidNetworking.DebugLevel.Debug) + Debug.Log("Send Pose Humanoid " + humanoid.humanoidId + " nwId: " + humanoid.nwId); - public class NetworkIdMsg : HumanoidNetworking.IMessage { - public const byte msgId = 0xA1; - public byte networkId = 0; + SendBone(humanoid.hipsTarget.hips, true); + SendBone(humanoid.hipsTarget.spine); + SendBone(humanoid.hipsTarget.chest); + SendBone(humanoid.headTarget.neck); + SendBone(humanoid.headTarget.head); - public NetworkIdMsg(byte networkId) { - this.networkId = networkId; - } - public NetworkIdMsg(byte[] data) : base(data) { } + SendBone(humanoid.leftHandTarget.upperArm); + SendBone(humanoid.leftHandTarget.forearm); + SendBone(humanoid.leftHandTarget.hand); - public override byte[] Serialize() { - MemoryStream ms = new(); - BinaryWriter bw = new(ms); - bw.Write(msgId); - bw.Write(this.networkId); // Thing Id - byte[] data = ms.ToArray(); - return data; - } - public override void Deserialize(byte[] data) { - base.Deserialize(data); - this.networkId = (byte)data[1]; - } + SendBone(humanoid.rightHandTarget.upperArm); + SendBone(humanoid.rightHandTarget.forearm); + SendBone(humanoid.rightHandTarget.hand); + + SendBone(humanoid.leftFootTarget.upperLeg); + SendBone(humanoid.leftFootTarget.lowerLeg); + SendBone(humanoid.leftFootTarget.foot); + + SendBone(humanoid.rightFootTarget.upperLeg); + SendBone(humanoid.rightFootTarget.lowerLeg); + SendBone(humanoid.rightFootTarget.foot); } - public void ReceiveNetworkId(byte[] data) { - NetworkIdMsg msg = new(data); - - messageQueue.Enqueue(msg); + private void SendBone(HumanoidTarget.TargetedBone bone, bool isRoot = false) { + //RoboidBonePose bonePose = new(bone, isRoot); + //byte[] data = bonePose.Serialize(); + //if (bonePose.bonePosition != null) { + // Debug.Log($"bone position {bone.bone.transform.position.x} {bone.bone.transform.position.y} {bone.bone.transform.position.z}"); + // Debug.Log($"bone pos {bonePose.bonePosition.horizontal} {bonePose.bonePosition.vertical} {bonePose.bonePosition.distance}"); + //} + //SendMsg(data); + Quaternion orientation = bone.bone.transform.rotation; + Quat32 boneOrientation = new(orientation.x, orientation.y, orientation.z, orientation.w); + if (isRoot) { + FromVector3(bone.bone.transform.position, out float distance, out float horizontal, out float vertical); + Spherical bonePosition = new(distance, horizontal, vertical); + PoseMsg.Send(client, (byte)bone.boneId, bonePosition, boneOrientation); + } + else + PoseMsg.Send(client, (byte)bone.boneId, null, boneOrientation); } - private void ProcessNetworkId(NetworkIdMsg msg) { + #endregion Pose + + //#region Messages + + //#region NetworkId + + //public class NetworkIdMsg : HumanoidNetworking.IMessage { + // public const byte msgId = 0xA1; + // public byte networkId = 0; + + // public NetworkIdMsg(byte networkId) { + // this.networkId = networkId; + // } + // public NetworkIdMsg(byte[] data) : base(data) { } + + // public override byte[] Serialize() { + // MemoryStream ms = new(); + // BinaryWriter bw = new(ms); + // bw.Write(msgId); + // bw.Write(this.networkId); // Thing Id + // byte[] data = ms.ToArray(); + // return data; + // } + // public override void Deserialize(byte[] data) { + // base.Deserialize(data); + // this.networkId = (byte)data[1]; + // } + //} + + //public void ReceiveNetworkId(byte[] data) { + // NetworkIdMsg msg = new(data); + + // messageQueue.Enqueue(msg); + //} + + void ISiteServer.ProcessNetworkId(Client client, NetworkIdMsg msg) { if (this.networkId == msg.networkId) return; @@ -294,7 +388,7 @@ namespace Passer.Humanoid { return; } - SendThing(remoteHumanoid); + SendThing(client, remoteHumanoid); SendName(remoteHumanoid, 0, "Humanoid"); SendModel(remoteHumanoid, "https://gitlab.passervr.com/passer/models/humanoid/-/raw/main/MakeHumanPasserMedium.glb?ref_type=heads&inline=false"); @@ -328,226 +422,231 @@ namespace Passer.Humanoid { SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.toes); } - #endregion + //#endregion - #region Thing + //#region Thing - protected class ThingMsg : HumanoidNetworking.IMessage { - private const byte msgId = 0x80; - public const byte length = 3; - public byte thingId; - public byte thingType; + //protected class ThingMsg : HumanoidNetworking.IMessage { + // private const byte msgId = 0x80; + // public const byte length = 3; + // public byte thingId; + // public byte thingType; - public ThingMsg(byte thingId, byte thingType) { - this.thingId = thingId; - this.thingType = thingType; - } - public ThingMsg(byte[] data) : base(data) { } + // public ThingMsg(byte thingId, byte thingType) { + // this.thingId = thingId; + // this.thingType = thingType; + // } + // public ThingMsg(byte[] data) : base(data) { } - public override byte[] Serialize() { - byte[] data = new byte[3]; - data[0] = msgId; - data[1] = thingId; - data[2] = thingType; - return data; - } - public override void Deserialize(byte[] data) { - try { - uint ix = 0; - thingId = data[ix++]; - thingType = data[ix++]; - } - catch { } - } - } + // public override byte[] Serialize() { + // byte[] data = new byte[3]; + // data[0] = msgId; + // data[1] = thingId; + // data[2] = thingType; + // return data; + // } + // public override void Deserialize(byte[] data) { + // try { + // uint ix = 0; + // thingId = data[ix++]; + // thingType = data[ix++]; + // } + // catch { } + // } + //} - public void SendThing(HumanoidControl humanoid) { + public void SendThing(Client client, HumanoidControl humanoid) { if (debug <= HumanoidNetworking.DebugLevel.Debug) Debug.Log("Send Thing " + humanoid.humanoidId + " nwId: " + humanoid.nwId); - ThingMsg thingMsg = new(0, 1); - SendMsg(thingMsg); + ThingMsg.Send(client, (byte)humanoid.nwId, 0, 1); + //ThingMsg thingMsg = new(0, 1); + //SendMsg(thingMsg); //if (udpClient != null) { // byte[] data = thingMsg.Serialize(); // udpClient.Send(data, data.Length, "127.0.0.1", nssPort); //} } - #endregion Thing + //#endregion Thing - #region SubThing + //#region SubThing - protected class SubThingMsg : HumanoidNetworking.IMessage { - private const byte msgId = 0x12; // (18) - public const byte length = 3 + 4; - public byte thingId; - public byte parentId; - readonly Vector3 position; + //protected class SubThingMsg : HumanoidNetworking.IMessage { + // private const byte msgId = 0x12; // (18) + // public const byte length = 3 + 4; + // public byte thingId; + // public byte parentId; + // readonly Vector3 position; - public SubThingMsg(byte thingId, byte parentId, Vector3 position) { - this.thingId = thingId; - this.parentId = parentId; - this.position = position; - } - public SubThingMsg(byte[] data) : base(data) { } + // public SubThingMsg(byte thingId, byte parentId, Vector3 position) { + // this.thingId = thingId; + // this.parentId = parentId; + // this.position = position; + // } + // public SubThingMsg(byte[] data) : base(data) { } - public override byte[] Serialize() { - byte[] data = new byte[7]; - data[0] = msgId; - data[1] = thingId; - data[2] = parentId; + // public override byte[] Serialize() { + // byte[] data = new byte[7]; + // data[0] = msgId; + // data[1] = thingId; + // data[2] = parentId; - data[3] = 0; - data[4] = 0; - data[5] = 0; - data[6] = 0; - return data; - } - //public override void Deserialize(byte[] data) { - // try { - // uint ix = 0; - // thingId = data[ix++]; - // thingType = data[ix++]; - // } - // catch { } - //} - } + // data[3] = 0; + // data[4] = 0; + // data[5] = 0; + // data[6] = 0; + // return data; + // } + // //public override void Deserialize(byte[] data) { + // // try { + // // uint ix = 0; + // // thingId = data[ix++]; + // // thingType = data[ix++]; + // // } + // // catch { } + // //} + //} public void SendSubThing(HumanoidControl humanoid, HumanoidTarget.TargetedBone bone) { if (debug <= HumanoidNetworking.DebugLevel.Debug) Debug.Log("Send SubThing " + humanoid.humanoidId + " nwId: " + humanoid.nwId); - SubThingMsg thingMsg; - if (bone.parent != null) - thingMsg = new((byte)bone.boneId, (byte)bone.parent.boneId, bone.bone.transform.position); - else - thingMsg = new((byte)bone.boneId, 0, bone.bone.transform.position); + ThingMsg.Send(client, (byte)bone.boneId, 0x00, (byte)bone.parent.boneId); + + //SubThingMsg thingMsg; + //if (bone.parent != null) + // thingMsg = new((byte)bone.boneId, (byte)bone.parent.boneId, bone.bone.transform.position); + //else + // thingMsg = new((byte)bone.boneId, 0, bone.bone.transform.position); - if (udpClient != null) { - byte[] data = thingMsg.Serialize(); - udpClient.Send(data, data.Length, "127.0.0.1", nssPort); - } + //if (udpClient != null) { + // byte[] data = thingMsg.Serialize(); + // udpClient.Send(data, data.Length, "127.0.0.1", nssPort); + //} SendName(humanoid, (byte)bone.boneId, bone.bone.transform.name); } - #endregion + //#endregion - #region Name + //#region Name - public class NameMsg : HumanoidNetworking.IMessage { - public const byte msgId = 0x91; // (145) Name - public byte networkId = 0; - public byte thingId; - public byte len; - public string name; + //public class NameMsg : HumanoidNetworking.IMessage { + // public const byte msgId = 0x91; // (145) Name + // public byte networkId = 0; + // public byte thingId; + // public byte len; + // public string name; - public NameMsg(byte thingId, string name) { - this.thingId = thingId; - this.name = name; - } - public NameMsg(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() { - MemoryStream ms = new(); - BinaryWriter bw = new(ms); - bw.Write(msgId); // 145 Name Msg - bw.Write((byte)thingId); // Thing Id - bw.Write((byte)name.Length); + // public override byte[] Serialize() { + // MemoryStream ms = new(); + // BinaryWriter bw = new(ms); + // bw.Write(msgId); // 145 Name Msg + // bw.Write((byte)thingId); // Thing Id + // bw.Write((byte)name.Length); - for (int ix = 0; ix < name.Length; ix++) - bw.Write(name[ix]); + // for (int ix = 0; ix < name.Length; ix++) + // bw.Write(name[ix]); - byte[] data = ms.ToArray(); - return data; - } - public override void Deserialize(byte[] data) { - uint ix = 0; - this.thingId = data[ix++]; - int strlen = data[ix++]; - this.name = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen); - } - } + // byte[] data = ms.ToArray(); + // return data; + // } + // public override void Deserialize(byte[] data) { + // uint ix = 0; + // this.thingId = data[ix++]; + // int strlen = data[ix++]; + // this.name = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen); + // } + //} public void SendName(HumanoidControl humanoid, byte thingId, string name) { if (debug <= HumanoidNetworking.DebugLevel.Debug) Debug.Log("Send Name " + humanoid.humanoidId + " nwId: " + humanoid.nwId); - NameMsg nameMsg = new(thingId, name); // humanoid.name; + NameMsg.Send(client, thingId, name); + //NameMsg nameMsg = new(thingId, name); // humanoid.name; - if (udpClient != null) { - byte[] data = nameMsg.Serialize(); - udpClient.Send(data, data.Length, "127.0.0.1", nssPort); - } + //if (udpClient != null) { + // byte[] data = nameMsg.Serialize(); + // udpClient.Send(data, data.Length, "127.0.0.1", nssPort); + //} } - #endregion + //#endregion - #region Model + //#region Model - protected class ModelUrlMsg : HumanoidNetworking.IMessage { - public const byte Id = 0x90; // (144) Model URL - public byte objectId; - public Vector3 position; - public float scale; - public string url; + //protected class ModelUrlMsg : HumanoidNetworking.IMessage { + // public const byte Id = 0x90; // (144) Model URL + // public byte objectId; + // public Vector3 position; + // public float scale; + // public string url; - public ModelUrlMsg(string url) { - this.url = url; - } - public ModelUrlMsg(byte[] data) : base(data) { } + // public ModelUrlMsg(string url) { + // this.url = url; + // } + // public ModelUrlMsg(byte[] data) : base(data) { } - public override byte[] Serialize() { - MemoryStream ms = new(); - BinaryWriter bw = new(ms); - bw.Write(ModelUrlMsg.Id); // 145 Name Msg - bw.Write((byte)0x00); // Thing Id + // public override byte[] Serialize() { + // MemoryStream ms = new(); + // BinaryWriter bw = new(ms); + // bw.Write(ModelUrlMsg.Id); // 145 Name Msg + // bw.Write((byte)0x00); // Thing Id - bw.Write((byte)0x00); // Dummy position - bw.Write((byte)0x00); - bw.Write((byte)0x00); - bw.Write((byte)0x00); + // bw.Write((byte)0x00); // Dummy position + // bw.Write((byte)0x00); + // bw.Write((byte)0x00); + // bw.Write((byte)0x00); - SendFloat16(bw, 1.0f); + // SendFloat16(bw, 1.0f); - bw.Write((byte)url.Length); + // bw.Write((byte)url.Length); - for (int ix = 0; ix < url.Length; ix++) - bw.Write(url[ix]); + // for (int ix = 0; ix < url.Length; ix++) + // bw.Write(url[ix]); - byte[] data = ms.ToArray(); - return data; - } - public override void Deserialize(byte[] data) { - uint ix = 1; // [0] = msgId - this.objectId = data[ix++]; - //this.position = ReceiveVector3(data, ref ix); - this.position = ReceiveSpherical(data, ref ix); - this.scale = ReceiveFloat16(data, ref ix); - int strlen = data[ix++]; - url = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen); - } - } + // byte[] data = ms.ToArray(); + // return data; + // } + // public override void Deserialize(byte[] data) { + // uint ix = 1; // [0] = msgId + // this.objectId = data[ix++]; + // //this.position = ReceiveVector3(data, ref ix); + // this.position = ReceiveSpherical(data, ref ix); + // this.scale = ReceiveFloat16(data, ref ix); + // int strlen = data[ix++]; + // url = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen); + // } + //} public void SendModel(HumanoidControl humanoid, string url) { if (debug <= HumanoidNetworking.DebugLevel.Debug) Debug.Log("Send URL " + humanoid.humanoidId + " nwId: " + humanoid.nwId); - ModelUrlMsg msg = new(url); // humanoid.name; + ModelUrlMsg.Send(client, 0, url); + //ModelUrlMsg msg = new(url); // humanoid.name; - if (udpClient != null) { - byte[] data = msg.Serialize(); - udpClient.Send(data, data.Length, "127.0.0.1", nssPort); - } + //if (udpClient != null) { + // byte[] data = msg.Serialize(); + // udpClient.Send(data, data.Length, "127.0.0.1", nssPort); + //} } - protected void ReceiveModelUrl(byte[] data) { - ModelUrlMsg msg = new(data); - messageQueue.Enqueue(msg); - } + //protected void ReceiveModelUrl(byte[] data) { + // ModelUrlMsg msg = new(data); + // messageQueue.Enqueue(msg); + //} bool loaded = false; - protected async void ProcessModelURL(ModelUrlMsg msg) { + protected async void ProcessModelURL(Client client, ModelUrlMsg msg) { if (loaded) return; @@ -564,7 +663,7 @@ namespace Passer.Humanoid { //Camera camera = FindObjectOfType(); // assuming just one camera per scene //if (camera != null) // camera.enabled = true; - Light[] lights = FindObjectsOfType(); + Light[] lights = FindObjectsByType(FindObjectsSortMode.None); foreach (Light light in lights) light.intensity = 1; // light.intensity / 1000; @@ -574,118 +673,290 @@ namespace Passer.Humanoid { } } - #endregion Model - #region Pose + void ISiteServer.ProcessModelUrl(Client client, ModelUrlMsg msg) { + int ix = msg.url.LastIndexOf("."); + if (ix < 0) + return; - [Serializable] - public class RoboidBonePose : HumanoidNetworking.HumanoidPose { - private readonly byte msgId = 0x10; - readonly byte boneId; - readonly Quat32 boneOrientation; - public readonly Spherical16 bonePosition; + string extension = msg.url.Substring(msg.url.LastIndexOf(".")); + string extension3 = extension.Substring(0, 4); + string extension4 = extension.Substring(0, 5); + if (extension4 == ".gltf" || extension3 == ".glb") { + ProcessGltfModel(msg); + } + } - public RoboidBonePose(HumanoidTarget.TargetedBone targetedBone, bool isRoot = false) { - this.boneId = (byte)targetedBone.boneId; - if (isRoot) { - this.boneOrientation = new Quat32(targetedBone.bone.transform.rotation); - this.bonePosition = Spherical16.FromVector3(targetedBone.bone.transform.position); + #region GLTF + + private bool loadingModel = false; + private Dictionary gltfCache = new(); + + private async void ProcessGltfModel(ModelUrlMsg msg) { + if (gltfCache.TryGetValue(msg.url, out GltfImport gltfImport)) { + Debug.Log("Found buffered GLTF for: " + msg.url); + Transform parentTransform = await InstantiateGltf(gltfImport, msg.thingId, msg.scale); + Renderer[] renderers = parentTransform.GetComponentsInChildren(); + foreach (Renderer renderer in renderers) { + MeshCollider c = renderer.gameObject.AddComponent(); + c.convex = true; } - else - this.boneOrientation = new Quat32(targetedBone.bone.transform.rotation); + } + else { + if (!loadingModel) { + loadingModel = true; - public override byte[] Serialize() { - byte[] data = new byte[11]; - data[0] = msgId; - data[1] = boneId; - - if (bonePosition != null) { - data[2] = 0x03; // Pose_position & Pose_orientation - data[3] = bonePosition.horizontal; - data[4] = bonePosition.vertical; - ushort distanceRaw = bonePosition.distance.GetBinary(); - data[5] = (byte)(distanceRaw >> 8); - data[6] = (byte)(distanceRaw & 0xFF); + Debug.Log("Loading GLTF model from: " + msg.url); + gltfImport = new(); + bool success = await gltfImport.Load(msg.url); + if (success) { + gltfCache.Add(msg.url, gltfImport); + loadingModel = false; + Transform parentTransform = await InstantiateGltf(gltfImport, msg.thingId, msg.scale); + ScanModelForThings(parentTransform); + } + else { + this.transform.localScale = Vector3.one * 1; + } } - else - data[2] = 0x02; // Pose_orientation - - - data[7] = boneOrientation.qx; - data[8] = boneOrientation.qy; - data[9] = boneOrientation.qz; - data[10] = boneOrientation.qw; - return data; } } - public virtual void UpdateHumanoidPose(HumanoidControl humanoid) { - if (debug <= HumanoidNetworking.DebugLevel.Debug) - Debug.Log("Send Pose Humanoid " + humanoid.humanoidId + " nwId: " + humanoid.nwId); - - SendBone(humanoid.hipsTarget.hips, true); - SendBone(humanoid.hipsTarget.spine); - SendBone(humanoid.hipsTarget.chest); - SendBone(humanoid.headTarget.neck); - SendBone(humanoid.headTarget.head); - - SendBone(humanoid.leftHandTarget.upperArm); - SendBone(humanoid.leftHandTarget.forearm); - SendBone(humanoid.leftHandTarget.hand); - - SendBone(humanoid.rightHandTarget.upperArm); - SendBone(humanoid.rightHandTarget.forearm); - SendBone(humanoid.rightHandTarget.hand); - - SendBone(humanoid.leftFootTarget.upperLeg); - SendBone(humanoid.leftFootTarget.lowerLeg); - SendBone(humanoid.leftFootTarget.foot); - - SendBone(humanoid.rightFootTarget.upperLeg); - SendBone(humanoid.rightFootTarget.lowerLeg); - SendBone(humanoid.rightFootTarget.foot); + private async Task InstantiateGltf(GltfImport gltfImport, byte objectId, float scale) { + Debug.Log("Instantiate GLTF model"); + Transform parentTransform = this.transform; + //Thing parentThing = allThings.Find(thing => thing.thingId == objectId); + //if (parentThing != null) + // parentTransform = parentThing.transform; + await gltfImport.InstantiateMainSceneAsync(parentTransform); + parentTransform.localScale = Vector3.one * scale; + return parentTransform; } - private void SendBone(HumanoidTarget.TargetedBone bone, bool isRoot = false) { - RoboidBonePose bonePose = new(bone, isRoot); - byte[] data = bonePose.Serialize(); - if (bonePose.bonePosition != null) { - Debug.Log($"bone position {bone.bone.transform.position.x} {bone.bone.transform.position.y} {bone.bone.transform.position.z}"); - Debug.Log($"bone pos {bonePose.bonePosition.horizontal} {bonePose.bonePosition.vertical} {bonePose.bonePosition.distance}"); + private void ScanModelForThings(Transform parentTransform) { + //Debug.Log("scanning model for things....."); + //SkinnedMeshRenderer[] meshRenderers = parentTransform.GetComponentsInChildren(); + //if (meshRenderers.Length > 0) { + // foreach (SkinnedMeshRenderer meshRenderer in meshRenderers) { + // if (meshRenderer.rootBone != null) { + // Debug.Log("Found a skinned mesh with bones"); + // //string meshRendererName = meshRenderer.transform.name; + // //Thing meshRendererThing = allThings.Find(thing => thing.name == meshRendererName); + // //SkinnedMeshRenderer thingMeshRenderer = meshRendererThing.gameObject.AddComponent(); + // //thingMeshRenderer.materials = meshRenderer.materials; + // //thingMeshRenderer.sharedMesh = meshRenderer.sharedMesh; + // //thingMeshRenderer.bones = GetSkinnedThings(meshRenderer); + + // //ReplaceSkinnedBones(meshRenderer); + // //string rootBoneName = meshRenderer.rootBone.name; + // //Thing rootBoneThing = allThings.Find(thing => thing.name == rootBoneName); + // //if (rootBoneName != null) + // // meshRenderer.rootBone = rootBoneThing.transform; + // //else + // // Debug.LogWarning("Could not find the root bone thing"); + // ScanForThings(meshRenderer.rootBone); + // //meshRenderer.rootBone = allThings.Find(thing => thing.thingId == 0).transform; + // break; + // } + // } + //} + //else { + // Renderer[] renderers = parentTransform.GetComponentsInChildren(); + // foreach (Renderer renderer in renderers) { + // MeshCollider c = renderer.gameObject.AddComponent(); + // c.convex = true; + // } + + // ScanForThings(parentTransform); + //} + + } + + //private void ReplaceSkinnedBones(SkinnedMeshRenderer meshRenderer) { + // Transform[] bones = meshRenderer.bones; + // //foreach(Transform bone in bones) { + // for (int ix = 0; ix < bones.Length; ix++) { + // Thing boneThing = allThings.Find(thing => thing.name == bones[ix].name); + // if (boneThing != null) { + // Debug.Log($"Replace {bones[ix].name}"); + // meshRenderer.bones[ix] = boneThing.transform; + // } + // } + //} + + //private Transform[] GetSkinnedThings(SkinnedMeshRenderer meshRenderer) { + // Transform[] bones = meshRenderer.bones; + // List thingBones = new(); + // //foreach(Transform bone in bones) { + // for (int ix = 0; ix < bones.Length; ix++) { + // Thing boneThing = allThings.Find(thing => thing.name == bones[ix].name); + // if (boneThing != null) { + // thingBones.Add(boneThing.transform); + // } + // } + // return thingBones.ToArray(); + //} + + //private void ScanForThings(Transform rootTransform) { + // Thing[] thingArray = allThings.ToArray(); + + // for (int thingIx = 0; thingIx < thingArray.Length; thingIx++) { + // Thing thing = thingArray[thingIx]; + // //Debug.Log($"find {thing.name}"); + // GameObject foundObj = FindThingByName(thing, rootTransform); + // if (foundObj != null) { + // Thing foundThing = foundObj.GetComponent(); + // if (foundObj != thing.gameObject) { + // if (foundThing == null) { + // allThings.Remove(thing); + // foundThing = thing.CopyTo(foundObj); + // allThings.Add(foundThing); + // } + // } + // if (thing.transform.childCount > foundObj.transform.childCount) { + // CopyChildren(thing, foundObj.transform); + // } + // //} else { + // // Debug.LogWarning($"Could not find thing {thing.name}"); + // } + // } + //} + + //private GameObject FindThingByName(Thing thing, Transform rootTransform) { + // if (rootTransform == null || thing == null) + // return null; + + // if (rootTransform.name == thing.name) + // return rootTransform.gameObject; + + // for (int childIx = 0; childIx < rootTransform.childCount; childIx++) { + // Transform child = rootTransform.GetChild(childIx); + // GameObject foundObj = FindThingByName(thing, child); + // if (foundObj != null) + // return foundObj; + // } + // return null; + //} + + //private void CopyChildren(Thing from, Transform to) { + // for (int childIx = 0; childIx < from.transform.childCount; childIx++) { + // Transform child = from.transform.GetChild(childIx); + // Debug.Log(child.name); + // bool found = false; + // foreach (Transform toChild in to) { + // if (child.name == toChild.name) { + // found = true; + // break; + // } + // } + // if (!found) { + // Debug.Log("...not found"); + // Thing childThing = child.GetComponent(); + // if (childThing != null) { + // allThings.Remove(childThing); + // GameObject newChildObj = new GameObject(child.name); + // newChildObj.transform.SetParent(to.transform); + // newChildObj.transform.SetLocalPositionAndRotation( + // child.transform.localPosition, child.transform.localRotation); + // newChildObj.transform.localScale = Vector3.one; + // Thing newThing = childThing.CopyTo(newChildObj); + // allThings.Add(newThing); + // } + // } + // } + //} + + #endregion GLTF + + + //#endregion Model + + //#region Pose + + //[Serializable] + //public class RoboidBonePose : HumanoidNetworking.HumanoidPose { + // private readonly byte msgId = 0x10; + // readonly byte boneId; + // readonly Quat32 boneOrientation; + // public readonly Spherical16 bonePosition; + + // public RoboidBonePose(HumanoidTarget.TargetedBone targetedBone, bool isRoot = false) { + // this.boneId = (byte)targetedBone.boneId; + // if (isRoot) { + // this.boneOrientation = new Quat32(targetedBone.bone.transform.rotation); + // this.bonePosition = Spherical16.FromVector3(targetedBone.bone.transform.position); + // } + // else + // this.boneOrientation = new Quat32(targetedBone.bone.transform.rotation); + // } + + // public override byte[] Serialize() { + // byte[] data = new byte[11]; + // data[0] = msgId; + // data[1] = boneId; + + // if (bonePosition != null) { + // data[2] = 0x03; // Pose_position & Pose_orientation + // data[3] = bonePosition.horizontal; + // data[4] = bonePosition.vertical; + // ushort distanceRaw = bonePosition.distance.GetBinary(); + // data[5] = (byte)(distanceRaw >> 8); + // data[6] = (byte)(distanceRaw & 0xFF); + // } + // else + // data[2] = 0x02; // Pose_orientation + + + // data[7] = boneOrientation.qx; + // data[8] = boneOrientation.qy; + // data[9] = boneOrientation.qz; + // data[10] = boneOrientation.qw; + // return data; + // } + //} + + private static void FromVector3(Vector3 v, out float distance, out float horizontal, out float vertical) { + distance = v.magnitude; + if (distance == 0.0f) { + horizontal = 0; + vertical = 0; + } + else { + vertical = (Mathf.PI / 2 - Mathf.Acos(v.y / distance)) * Mathf.Rad2Deg; + horizontal = Mathf.Atan2(v.x, v.z) * Mathf.Rad2Deg; } - SendMsg(data); } - #endregion Pose + //#endregion Pose - #endregion Messages + //#endregion Messages #region Send - static void SendQuat32(BinaryWriter bw, Quat32 q) { - bw.Write((byte)q.qx); - bw.Write((byte)q.qy); - bw.Write((byte)q.qz); - bw.Write((byte)q.qw); - } + //static void SendQuat32(BinaryWriter bw, Quat32 q) { + // bw.Write((byte)q.qx); + // bw.Write((byte)q.qy); + // bw.Write((byte)q.qz); + // bw.Write((byte)q.qw); + //} - static void SendFloat16(BinaryWriter bw, float f) { - bw.Write((byte)0x3C); // Dummy value 1 - bw.Write((byte)0x00); - } + //static void SendFloat16(BinaryWriter bw, float f) { + // bw.Write((byte)0x3C); // Dummy value 1 + // bw.Write((byte)0x00); + //} - protected void SendMsg(HumanoidNetworking.IMessage msg) { - SendMsg(msg.Serialize()); - } + //protected void SendMsg(HumanoidNetworking.IMessage msg) { + // SendMsg(msg.Serialize()); + //} - protected void SendMsg(byte[] data) { - if (udpClient == null) - return; + //protected void SendMsg(byte[] data) { + // if (udpClient == null) + // return; - udpClient.Send(data, data.Length, "127.0.0.1", nssPort); - } + // udpClient.Send(data, data.Length, "127.0.0.1", nssPort); + //} #endregion Send