diff --git a/Runtime/HumanoidControl/Scripts/Networking/Roboid/HumanoidPlayerRoboid.cs b/Runtime/HumanoidControl/Scripts/Networking/Roboid/HumanoidPlayerRoboid.cs index 3cd3640..3841488 100644 --- a/Runtime/HumanoidControl/Scripts/Networking/Roboid/HumanoidPlayerRoboid.cs +++ b/Runtime/HumanoidControl/Scripts/Networking/Roboid/HumanoidPlayerRoboid.cs @@ -5,8 +5,7 @@ using System.IO; using System.Net; using System.Net.Sockets; using UnityEngine; -using System.Runtime.Serialization; - +using GLTFast; #if hNW_ROBOID @@ -151,6 +150,8 @@ namespace Passer.Humanoid { #region Update float lastSend; + float lastClientMsg; + const float clientMsgInterval = 3; protected virtual void LateUpdate() { while (messageQueue.TryDequeue(out HumanoidNetworking.IMessage msg)) { @@ -167,6 +168,10 @@ namespace Passer.Humanoid { } lastSend = Time.time; } + if (Time.time + clientMsgInterval > lastClientMsg) { + SendClientMsg(humanoids[0]); // We just need it for the networkId + lastClientMsg = Time.time; + } } protected virtual void ProcessMessage(HumanoidNetworking.IMessage msg) { @@ -174,6 +179,9 @@ namespace Passer.Humanoid { case NetworkIdMsg networkId: ProcessNetworkId(networkId); break; + case ModelUrlMsg modelUrl: + ProcessModelURL(modelUrl); + break; } } @@ -194,12 +202,49 @@ namespace Passer.Humanoid { 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; + + 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++]; + } + } + + protected void SendClientMsg(HumanoidControl humanoid) { + if (debug <= HumanoidNetworking.DebugLevel.Debug) + Debug.Log("Send ClientMsg " + humanoid.nwId); + ClientMsg clientMsg = new((byte)humanoid.nwId); + SendMsg(clientMsg); + } + #endregion Client + + #region Messages + #region NetworkId public class NetworkIdMsg : HumanoidNetworking.IMessage { @@ -227,12 +272,15 @@ namespace Passer.Humanoid { public void ReceiveNetworkId(byte[] data) { NetworkIdMsg msg = new(data); - this.networkId = msg.networkId; messageQueue.Enqueue(msg); } private void ProcessNetworkId(NetworkIdMsg msg) { + if (this.networkId == msg.networkId) + return; + + this.networkId = msg.networkId; GameObject networkingObj = this.GetGameObject(msg.networkId); if (networkingObj == null) { if (this.debug <= HumanoidNetworking.DebugLevel.Error) @@ -318,11 +366,11 @@ namespace Passer.Humanoid { Debug.Log("Send Thing " + humanoid.humanoidId + " nwId: " + humanoid.nwId); ThingMsg thingMsg = new(0, 1); - - if (udpClient != null) { - byte[] data = thingMsg.Serialize(); - udpClient.Send(data, data.Length, "127.0.0.1", nssPort); - } + SendMsg(thingMsg); + //if (udpClient != null) { + // byte[] data = thingMsg.Serialize(); + // udpClient.Send(data, data.Length, "127.0.0.1", nssPort); + //} } #endregion Thing @@ -437,8 +485,8 @@ namespace Passer.Humanoid { #region Model - class ModelUrlMsg : HumanoidNetworking.IMessage { - public const byte msgId = 0x90; // (144) Model URL + protected class ModelUrlMsg : HumanoidNetworking.IMessage { + public const byte Id = 0x90; // (144) Model URL public byte objectId; public Vector3 position; public float scale; @@ -452,7 +500,7 @@ namespace Passer.Humanoid { public override byte[] Serialize() { MemoryStream ms = new(); BinaryWriter bw = new(ms); - bw.Write(msgId); // 145 Name Msg + bw.Write(ModelUrlMsg.Id); // 145 Name Msg bw.Write((byte)0x00); // Thing Id bw.Write((byte)0x00); // Dummy position @@ -470,20 +518,20 @@ namespace Passer.Humanoid { byte[] data = ms.ToArray(); return data; } - //public override void Deserialize(byte[] data) { - // uint ix = 0; - // 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 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 Name " + humanoid.humanoidId + " nwId: " + humanoid.nwId); + Debug.Log("Send URL " + humanoid.humanoidId + " nwId: " + humanoid.nwId); ModelUrlMsg msg = new(url); // humanoid.name; @@ -493,6 +541,39 @@ namespace Passer.Humanoid { } } + protected void ReceiveModelUrl(byte[] data) { + ModelUrlMsg msg = new(data); + messageQueue.Enqueue(msg); + } + + bool loaded = false; + protected async void ProcessModelURL(ModelUrlMsg msg) { + if (loaded) + return; + + Debug.Log("Loading GLTF model from :" + msg.url); + GltfImport gltfImport = new GltfImport(); + loaded = true; + // for .gltf... + bool success = await gltfImport.Load(msg.url); + if (success) { + Transform parentTransform = this.transform; + await gltfImport.InstantiateMainSceneAsync(parentTransform); + if (!success) + return; + //Camera camera = FindObjectOfType(); // assuming just one camera per scene + //if (camera != null) + // camera.enabled = true; + Light[] lights = FindObjectsOfType(); + foreach (Light light in lights) + light.intensity = 1; // light.intensity / 1000; + + Renderer[] renderers = parentTransform.GetComponentsInChildren(); + foreach (Renderer renderer in renderers) + renderer.gameObject.AddComponent(); + } + } + #endregion Model #region Pose @@ -502,23 +583,34 @@ namespace Passer.Humanoid { 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); + this.boneOrientation = new Quat32(targetedBone.bone.transform.rotation); } - + public override byte[] Serialize() { byte[] data = new byte[11]; data[0] = msgId; data[1] = boneId; - data[2] = 0x03; // Pose_orientation - //data[3][4][5][6] are 0 + 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; @@ -528,46 +620,10 @@ namespace Passer.Humanoid { } } - - //[Serializable] - //public class RoboidPose : HumanoidNetworking.HumanoidPose { - // readonly Quat32 headOrientation; - - // public RoboidPose(HumanoidControl humanoid) { - // headOrientation = new Quat32(humanoid.headTarget.head.bone.transform.rotation); - // } - - // public override byte[] Serialize() { - // MemoryStream ms = new(); - // BinaryWriter bw = new(ms); - // bw.Write((byte)0x10); // PoseMsg - // bw.Write((byte)this.head.boneId); // Thing Id - // bw.Write((byte)0x03); // Pose_Orientation - - // bw.Write((byte)0x00); // Dummy data - // bw.Write((byte)0x00); - // bw.Write((byte)0x00); - // bw.Write((byte)0x00); - - // SendQuat32(bw, headOrientation); - - // byte[] data = ms.ToArray(); - // return data; - // } - //} - public virtual void UpdateHumanoidPose(HumanoidControl humanoid) { if (debug <= HumanoidNetworking.DebugLevel.Debug) Debug.Log("Send Pose Humanoid " + humanoid.humanoidId + " nwId: " + humanoid.nwId); - //RoboidPose humanoidPose = new(humanoid); - //if (createLocalRemotes) - // this.Receive(humanoidPose); - - //if (udpClient != null) { - // byte[] data = humanoidPose.Serialize(); - // udpClient.Send(data, data.Length, "127.0.0.1", nssPort); - //} SendBone(humanoid.hipsTarget.hips, true); SendBone(humanoid.hipsTarget.spine); SendBone(humanoid.hipsTarget.chest); @@ -593,12 +649,18 @@ namespace Passer.Humanoid { private void SendBone(HumanoidTarget.TargetedBone bone, bool isRoot = false) { RoboidBonePose bonePose = new(bone, isRoot); - byte[] buffer = bonePose.Serialize(); - udpClient.Send(buffer, buffer.Length, "127.0.0.1", nssPort); + 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); } #endregion Pose + #endregion Messages + #region Send static void SendQuat32(BinaryWriter bw, Quat32 q) { @@ -613,7 +675,44 @@ namespace Passer.Humanoid { bw.Write((byte)0x00); } + + protected void SendMsg(HumanoidNetworking.IMessage msg) { + SendMsg(msg.Serialize()); + } + + protected void SendMsg(byte[] data) { + if (udpClient == null) + return; + + udpClient.Send(data, data.Length, "127.0.0.1", nssPort); + } + #endregion Send + + #region Receive + + protected static float ReceiveAngle8(byte[] data, ref uint ix) { + float value = (data[ix++] * 180) / 128.0F; + return value; + } + + protected static Vector3 ReceiveSpherical(byte[] data, ref uint ix) { + float horizontal = ReceiveAngle8(data, ref ix); + float vertical = ReceiveAngle8(data, ref ix); + float distance = ReceiveFloat16(data, ref ix); + Vector3 v = Quaternion.AngleAxis(horizontal, Vector3.up) * Quaternion.AngleAxis(vertical, Vector3.left) * (Vector3.forward * distance); + return v; + } + + protected static float ReceiveFloat16(byte[] data, ref uint ix) { + ushort value = (ushort)(data[ix++] << 8 | data[ix++]); + float16 f16 = new float16(); + f16.SetBinary(value); + float f = f16.toFloat(); + return f; + } + + #endregion Receive } } #endif \ No newline at end of file diff --git a/Runtime/HumanoidControl/Scripts/Networking/Roboid/Spherical.cs b/Runtime/HumanoidControl/Scripts/Networking/Roboid/Spherical.cs index 19b246f..5a7204b 100644 --- a/Runtime/HumanoidControl/Scripts/Networking/Roboid/Spherical.cs +++ b/Runtime/HumanoidControl/Scripts/Networking/Roboid/Spherical.cs @@ -1,17 +1,20 @@ +using Passer.Humanoid.Tracking; using UnityEngine; namespace Passer.LinearAlgebra { public class Spherical16 { - public float distance; - public float horizontal; - public float vertical; + public float16 distance; + public byte horizontal; + public byte vertical; public Spherical16(float distance, float horizontal = 0, float vertical = 0) { - this.distance = distance; - this.horizontal = horizontal; - this.vertical = vertical; + horizontal = Angle.Normalize(horizontal); + vertical = Angle.Normalize(vertical); + this.distance = new float16(distance); + this.horizontal = (byte)(horizontal * 128.0f / 180.0f); + this.vertical = (byte)(vertical * 128.0f / 180.0f); } public static Spherical16 FromVector3(Vector3 v) { @@ -20,8 +23,9 @@ namespace Passer.LinearAlgebra { return new Spherical16(distance); } else { - float verticalAngle = Mathf.PI / 2 - Mathf.Acos(v.y / distance) * Mathf.Rad2Deg; + float verticalAngle = (Mathf.PI / 2 - Mathf.Acos(v.y / distance)) * Mathf.Rad2Deg; float horizontalAngle = Mathf.Atan2(v.x, v.z) * Mathf.Rad2Deg; + Debug.Log($"sph {horizontalAngle} {verticalAngle} {distance}"); return new Spherical16(distance, horizontalAngle, verticalAngle); } } diff --git a/Runtime/HumanoidControl/Scripts/Networking/Roboid/float16.cs b/Runtime/HumanoidControl/Scripts/Networking/Roboid/float16.cs new file mode 100644 index 0000000..d3081e3 --- /dev/null +++ b/Runtime/HumanoidControl/Scripts/Networking/Roboid/float16.cs @@ -0,0 +1,258 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +public class float16 { + // + // FILE: float16.cpp + // AUTHOR: Rob Tillaart + // VERSION: 0.1.8 + // PURPOSE: library for Float16s for Arduino + // URL: http://en.wikipedia.org/wiki/Half-precision_floating-point_format + + ushort _value; + + public float16() { _value = 0; } + + public float16(float f) { + _value = f32tof16(f); + } + + public float toFloat() { + return f16tof32(_value); + } + + public ushort GetBinary() { return _value; } + public void SetBinary(ushort value) { _value = value; } + + ////////////////////////////////////////////////////////// + // + // EQUALITIES + // + /* + bool float16::operator ==(const float16 &f) { return (_value == f._value); } + +bool float16::operator !=(const float16 &f) { return (_value != f._value); } + +bool float16::operator >(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value < f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value > f._value; +} + +bool float16::operator >=(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value <= f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value >= f._value; +} + +bool float16::operator <(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value > f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value < f._value; +} + +bool float16::operator <=(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value >= f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value <= f._value; +} + +////////////////////////////////////////////////////////// +// +// NEGATION +// +float16 float16::operator -() { + float16 f16; + f16.setBinary(_value ^ 0x8000); + return f16; +} + +////////////////////////////////////////////////////////// +// +// MATH +// +float16 float16::operator +(const float16 &f) { + return float16(this->toDouble() + f.toDouble()); +} + +float16 float16::operator -(const float16 &f) { + return float16(this->toDouble() - f.toDouble()); +} + +float16 float16::operator *(const float16 &f) { + return float16(this->toDouble() * f.toDouble()); +} + +float16 float16::operator /(const float16 &f) { + return float16(this->toDouble() / f.toDouble()); +} + +float16 & float16::operator+=(const float16 &f) { + *this = this->toDouble() + f.toDouble(); + return *this; +} + +float16 & float16::operator-=(const float16 &f) { + *this = this->toDouble() - f.toDouble(); + return *this; +} + +float16 & float16::operator*=(const float16 &f) { + *this = this->toDouble() * f.toDouble(); + return *this; +} + +float16 & float16::operator/=(const float16 &f) { + *this = this->toDouble() / f.toDouble(); + return *this; +} + +////////////////////////////////////////////////////////// +// +// MATH HELPER FUNCTIONS +// +int float16::sign() { + if (_value & 0x8000) + return -1; + if (_value & 0xFFFF) + return 1; + return 0; +} + +bool float16::isZero() { return ((_value & 0x7FFF) == 0x0000); } + +bool float16::isNaN() { + if ((_value & 0x7C00) != 0x7C00) + return false; + if ((_value & 0x03FF) == 0x0000) + return false; + return true; +} + +bool float16::isInf() { return ((_value == 0x7C00) || (_value == 0xFC00)); } + */ + ////////////////////////////////////////////////////////// + // + // CORE CONVERSION + // + float f16tof32(ushort _value) { + //ushort sgn; + ushort man; + int exp; + float f; + + //Debug.Log($"{_value}"); + + bool sgn = (_value & 0x8000) > 0; + exp = (_value & 0x7C00) >> 10; + man = (ushort)(_value & 0x03FF); + + //Debug.Log($"{sgn} {exp} {man}"); + + // ZERO + if ((_value & 0x7FFF) == 0) { + return sgn ? -0 : 0; + } + // NAN & INF + if (exp == 0x001F) { + if (man == 0) + return sgn ? float.NegativeInfinity : float.PositiveInfinity; //-INFINITY : INFINITY; + else + return float.NaN; // NAN; + } + + // SUBNORMAL/NORMAL + if (exp == 0) + f = 0; + else + f = 1; + + // PROCESS MANTISSE + for (int i = 9; i >= 0; i--) { + f *= 2; + if ((man & (1 << i)) != 0) + f = f + 1; + } + //Debug.Log($"{f}"); + f = f * Mathf.Pow(2.0f, exp - 25); + if (exp == 0) { + f = f * Mathf.Pow(2.0f, -13); // 5.96046447754e-8; + } + //Debug.Log($"{f}"); + return sgn ? -f : f; + } + + ushort f32tof16(float f) { + //uint t = *(uint*)&f; + uint t = (uint)BitConverter.SingleToInt32Bits(f); + // man bits = 10; but we keep 11 for rounding + ushort man = (ushort)((t & 0x007FFFFF) >> 12); + short exp = (short)((t & 0x7F800000) >> 23); + bool sgn = (t & 0x80000000) != 0; + + // handle 0 + if ((t & 0x7FFFFFFF) == 0) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // denormalized float32 does not fit in float16 + if (exp == 0x00) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // handle infinity & NAN + if (exp == 0x00FF) { + if (man != 0) + return 0xFE00; // NAN + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + + // normal numbers + exp = (short)(exp - 127 + 15); + // overflow does not fit => INF + if (exp > 30) { + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + // subnormal numbers + if (exp < -38) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ? + } + if (exp <= 0) // subnormal + { + man >>= (exp + 14); + // rounding + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | man); + return man; + } + + // normal + // TODO rounding + exp <<= 10; + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | exp | man); + return (ushort)(exp | man); + } + + // -- END OF FILE -- +} diff --git a/Runtime/HumanoidControl/Scripts/Networking/Roboid/float16.cs.meta b/Runtime/HumanoidControl/Scripts/Networking/Roboid/float16.cs.meta new file mode 100644 index 0000000..fc907e4 --- /dev/null +++ b/Runtime/HumanoidControl/Scripts/Networking/Roboid/float16.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ebe91dca635b85f4f94fb2469970b423 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Runtime/PasserVR.HumanoidControl.asmdef b/Runtime/PasserVR.HumanoidControl.asmdef index 5c491e9..2a20775 100644 --- a/Runtime/PasserVR.HumanoidControl.asmdef +++ b/Runtime/PasserVR.HumanoidControl.asmdef @@ -1,5 +1,6 @@ { "name": "PasserVR.HumanoidControl", + "rootNamespace": "", "references": [ "UnityEditor.SpatialTracking", "UnityEngine.SpatialTracking", @@ -11,9 +12,10 @@ "PhotonVoice.API", "LeapMotion", "LeapMotion.LeapCSharp", - "Ultraleap.Tracking.Core", + "Ultraleap.Tracking.Core", "SteamVR", - "Unity.XR.OpenVR" + "Unity.XR.OpenVR", + "glTFast" ], "includePlatforms": [], "excludePlatforms": [],