Pose & scene sync are working

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);
-
+            SendMsg(thingMsg);
-            if (udpClient != null) {
+            //if (udpClient != null) {
-                byte[] data = thingMsg.Serialize();
+            //    byte[] data = thingMsg.Serialize();
-                udpClient.Send(data, data.Length, "127.0.0.1", nssPort);
+            //    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 {
+        protected class ModelUrlMsg : HumanoidNetworking.IMessage {
-            public const byte msgId = 0x90; // (144) Model URL
+            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) {
+            public override void Deserialize(byte[] data) {
-            //    uint ix = 0;
+                uint ix = 1; // [0] = msgId
-            //    this.objectId = data[ix++];
+                this.objectId = data[ix++];
-            //    //this.position = ReceiveVector3(data, ref ix);
+                //this.position = ReceiveVector3(data, ref ix);
-            //    this.position = ReceiveSpherical(data, ref ix);
+                this.position = ReceiveSpherical(data, ref ix);
-            //    this.scale = ReceiveFloat16(data, ref ix);
+                this.scale = ReceiveFloat16(data, ref ix);
-            //    int strlen = data[ix++];
+                int strlen = data[ix++];
-            //    url = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen);
+                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<Camera>(); // assuming just one camera per scene
+                //if (camera != null)
+                //    camera.enabled = true;
+                Light[] lights = FindObjectsOfType<Light>();
+                foreach (Light light in lights)
+                    light.intensity = 1; // light.intensity / 1000;
+
+                Renderer[] renderers = parentTransform.GetComponentsInChildren<Renderer>();
+                foreach (Renderer renderer in renderers)
+                    renderer.gameObject.AddComponent<MeshCollider>();
+            }
+        }
+
         #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();
+            byte[] data = bonePose.Serialize();
-            udpClient.Send(buffer, buffer.Length, "127.0.0.1", nssPort);
+            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

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 float16 distance;
-        public float horizontal;
+        public byte horizontal;
-        public float vertical;
+        public byte vertical;
 
         public Spherical16(float distance, float horizontal = 0, float vertical = 0) {
-            this.distance = distance;
+            horizontal = Angle.Normalize(horizontal);
-            this.horizontal = horizontal;
+            vertical = Angle.Normalize(vertical);
-            this.vertical = 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);
             }
         }

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 --
+}

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: 

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": [],