HumanoidControl/HumanoidControl_Free
HumanoidControl/HumanoidControl_Free
2
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Pose & scene sync are working

Browse Source
This commit is contained in:
Pascal Serrarens 2024-11-21 12:23:56 +01:00
parent de5bf3a10e
commit 11b61d4d73
5 changed files with 446 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

221
Runtime/HumanoidControl/Scripts/Networking/Roboid/HumanoidPlayerRoboid.cs
View File

@ -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<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,11 +583,13 @@ 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);
@ -516,9 +599,18 @@ namespace Passer.Humanoid {
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

18
Runtime/HumanoidControl/Scripts/Networking/Roboid/Spherical.cs
View File

@ -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);
}
}

258
Runtime/HumanoidControl/Scripts/Networking/Roboid/float16.cs Normal file
View File

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

11
Runtime/HumanoidControl/Scripts/Networking/Roboid/float16.cs.meta Normal file
View File

@ -0,0 +1,11 @@
fileFormatVersion: 2
guid: ebe91dca635b85f4f94fb2469970b423
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

4
Runtime/PasserVR.HumanoidControl.asmdef
View File

@ -1,5 +1,6 @@
{
"name": "PasserVR.HumanoidControl",
"rootNamespace": "",
"references": [
"UnityEditor.SpatialTracking",
"UnityEngine.SpatialTracking",
@ -13,7 +14,8 @@
"LeapMotion.LeapCSharp",
"Ultraleap.Tracking.Core",
"SteamVR",
"Unity.XR.OpenVR"
"Unity.XR.OpenVR",
"glTFast"
],
"includePlatforms": [],
"excludePlatforms": [],