Pascal Serrarens de5bf3a10e Tested with VR
2024-11-19 17:32:51 +01:00

619 lines
22 KiB
C#

using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.IO;
using System.Net;
using System.Net.Sockets;
using UnityEngine;
using System.Runtime.Serialization;
#if hNW_ROBOID
using Passer.LinearAlgebra;
namespace Passer.Humanoid {
public partial class HumanoidPlayer : MonoBehaviour, IHumanoidNetworking {
[SerializeField]
protected float _sendRate = 25;
public float sendRate {
get { return _sendRate; }
}
[SerializeField]
protected bool _createLocalRemotes = false;
public bool createLocalRemotes {
get { return _createLocalRemotes; }
set { _createLocalRemotes = value; }
}
public int nssPort = 7681;
protected UdpClient udpClient;
protected IPEndPoint endPoint;
protected object obj;
protected AsyncCallback receiveCallback;
ConcurrentQueue<HumanoidNetworking.IMessage> messageQueue = new ConcurrentQueue<HumanoidNetworking.IMessage>();
public byte networkId;
#region Dummy Interface
public void Send(bool b) { }
public void Send(byte b) { }
public void Send(int x) { }
public void Send(float f) { }
public void Send(Vector3 v) { }
public void Send(Quaternion q) { }
public bool ReceiveBool() {
return false;
}
public byte ReceiveByte() {
return 0;
}
public int ReceiveInt() {
return 0;
}
public float ReceiveFloat() {
return 0;
}
public Vector3 ReceiveVector3() {
return Vector3.zero;
}
public Quaternion ReceiveQuaternion() {
return Quaternion.identity;
}
public ulong GetObjectIdentity(GameObject obj) {
return 0;
}
public GameObject GetGameObject(ulong objIdentity) {
return this.gameObject;
}
public void InstantiateHumanoid(HumanoidControl humanoid) {
if (debug <= HumanoidNetworking.DebugLevel.Info)
DebugLog("Send Instantiate Humanoid " + humanoid.humanoidId);
HumanoidNetworking.InstantiateHumanoid instantiateHumanoid = new HumanoidNetworking.InstantiateHumanoid(humanoid);
if (createLocalRemotes)
this.Receive(instantiateHumanoid);
}
public void DestroyHumanoid(HumanoidControl humanoid) { }
public void Grab(HandTarget handTarget, GameObject obj, bool rangeCheck, HandTarget.GrabType grabType = HandTarget.GrabType.HandGrab) { }
public void LetGo(HandTarget handTarget) { }
public void ChangeAvatar(HumanoidControl humanoid, string avatarPrefabName, string possessionLocation = null) {
if (debug <= HumanoidNetworking.DebugLevel.Info)
Debug.Log(humanoid.nwId + ": Change Avatar: " + avatarPrefabName);
HumanoidNetworking.ChangeAvatar changeAvatar = new HumanoidNetworking.ChangeAvatar(humanoid, avatarPrefabName, possessionLocation);
if (createLocalRemotes)
this.Receive(changeAvatar);
}
public void SyncTrackingSpace(HumanoidControl humanoid) { }
public void ReenableNetworkSync(GameObject obj) { }
public void DisableNetworkSync(GameObject obj) { }
public void DebugLog(string s) {
Debug.Log(s);
}
public void DebugWarning(string s) {
Debug.LogWarning(s);
}
public void DebugError(string s) {
Debug.LogError(s);
}
public HumanoidNetworking.Smoothing smoothing => HumanoidNetworking.Smoothing.None;
private bool _isLocal;
public bool isLocal => _isLocal; // may need to implement this
public ulong nwId => 0;
public HumanoidNetworking.HumanoidPose lastHumanoidPose { get => null; set { return; } }
public List<HumanoidControl> humanoids { get; set; }
#endregion Dummy Interface
protected virtual void Awake() {
mInstance = this;
GameObject.DontDestroyOnLoad(this.gameObject);
humanoids = HumanoidNetworking.FindLocalHumanoids();
for (int i = 0; i < humanoids.Count; i++) {
HumanoidControl humanoid = humanoids[i];
if (humanoid.isRemote)
continue;
humanoid.humanoidNetworking = this;
}
endPoint = new IPEndPoint(IPAddress.Any, nssPort + 3);
udpClient = new UdpClient(endPoint);
receiveCallback = new AsyncCallback(Receiver);
udpClient.BeginReceive(receiveCallback, obj);
}
protected void OnApplicationQuit() {
if (udpClient != null) {
udpClient.Client.Close();
udpClient.Close();
udpClient = null;
}
}
#region Update
float lastSend;
protected virtual void LateUpdate() {
while (messageQueue.TryDequeue(out HumanoidNetworking.IMessage msg)) {
ProcessMessage(msg);
}
if (Time.time > lastSend + 1 / sendRate) {
if (humanoids != null) {
foreach (HumanoidControl humanoid in humanoids) {
if (!humanoid.isRemote) {
UpdateHumanoidPose(humanoid);
}
}
}
lastSend = Time.time;
}
}
protected virtual void ProcessMessage(HumanoidNetworking.IMessage msg) {
switch (msg) {
case NetworkIdMsg networkId:
ProcessNetworkId(networkId);
break;
}
}
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));
ReceivePacket(data);
udpClient.BeginReceive(receiveCallback, obj);
}
void ReceivePacket(byte[] data) {
byte msgId = data[0];
switch (msgId) {
case 0xA1: // (161) Network Id
this.ReceiveNetworkId(data); break;
}
}
#endregion Update
#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);
this.networkId = msg.networkId;
messageQueue.Enqueue(msg);
}
private void ProcessNetworkId(NetworkIdMsg msg) {
GameObject networkingObj = this.GetGameObject(msg.networkId);
if (networkingObj == null) {
if (this.debug <= HumanoidNetworking.DebugLevel.Error)
this.DebugLog("Could not find Networking for Instantiate Humanoid " + msg.networkId + "/0");
return;
}
HumanoidControl remoteHumanoid = humanoids[0]; //FindRemoteHumanoid(humanoids, 0);
if (remoteHumanoid == null) {
if (debug <= HumanoidNetworking.DebugLevel.Warning)
DebugLog("Remote Humanoid " + msg.networkId + "/" + 0 + "not found");
return;
}
SendThing(remoteHumanoid);
SendName(remoteHumanoid, 0, "Humanoid");
SendModel(remoteHumanoid, "https://gitlab.passervr.com/passer/models/humanoid/-/raw/main/MakeHumanPasserMedium.glb?ref_type=heads&inline=false");
SendSubThing(remoteHumanoid, remoteHumanoid.hipsTarget.hips);
SendSubThing(remoteHumanoid, remoteHumanoid.hipsTarget.spine);
SendSubThing(remoteHumanoid, remoteHumanoid.hipsTarget.chest);
SendSubThing(remoteHumanoid, remoteHumanoid.headTarget.neck);
SendSubThing(remoteHumanoid, remoteHumanoid.headTarget.head);
SendSubThing(remoteHumanoid, remoteHumanoid.leftHandTarget.upperArm);
SendSubThing(remoteHumanoid, remoteHumanoid.leftHandTarget.forearm);
SendSubThing(remoteHumanoid, remoteHumanoid.leftHandTarget.hand);
SendSubThing(remoteHumanoid, remoteHumanoid.rightHandTarget.upperArm);
SendSubThing(remoteHumanoid, remoteHumanoid.rightHandTarget.forearm);
SendSubThing(remoteHumanoid, remoteHumanoid.rightHandTarget.hand);
SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.upperLeg);
SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.lowerLeg);
SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.foot);
SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.toes);
SendSubThing(remoteHumanoid, remoteHumanoid.leftFootTarget.upperLeg);
SendSubThing(remoteHumanoid, remoteHumanoid.leftFootTarget.lowerLeg);
SendSubThing(remoteHumanoid, remoteHumanoid.leftFootTarget.foot);
SendSubThing(remoteHumanoid, remoteHumanoid.leftFootTarget.toes);
SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.upperLeg);
SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.lowerLeg);
SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.foot);
SendSubThing(remoteHumanoid, remoteHumanoid.rightFootTarget.toes);
}
#endregion
#region Thing
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 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) {
if (debug <= HumanoidNetworking.DebugLevel.Debug)
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);
}
}
#endregion Thing
#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;
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;
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);
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
#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 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);
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);
}
}
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;
if (udpClient != null) {
byte[] data = nameMsg.Serialize();
udpClient.Send(data, data.Length, "127.0.0.1", nssPort);
}
}
#endregion
#region Model
class ModelUrlMsg : HumanoidNetworking.IMessage {
public const byte msgId = 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 override byte[] Serialize() {
MemoryStream ms = new();
BinaryWriter bw = new(ms);
bw.Write(msgId); // 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);
SendFloat16(bw, 1.0f);
bw.Write((byte)url.Length);
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 = 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 void SendModel(HumanoidControl humanoid, string url) {
if (debug <= HumanoidNetworking.DebugLevel.Debug)
Debug.Log("Send Name " + humanoid.humanoidId + " nwId: " + humanoid.nwId);
ModelUrlMsg msg = new(url); // humanoid.name;
if (udpClient != null) {
byte[] data = msg.Serialize();
udpClient.Send(data, data.Length, "127.0.0.1", nssPort);
}
}
#endregion Model
#region Pose
[Serializable]
public class RoboidBonePose : HumanoidNetworking.HumanoidPose {
private readonly byte msgId = 0x10;
readonly byte boneId;
readonly Quat32 boneOrientation;
public RoboidBonePose(HumanoidTarget.TargetedBone targetedBone, bool isRoot = false) {
this.boneId = (byte)targetedBone.boneId;
if (isRoot) {
this.boneOrientation = new Quat32(targetedBone.bone.transform.rotation);
}
else
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
data[7] = boneOrientation.qx;
data[8] = boneOrientation.qy;
data[9] = boneOrientation.qz;
data[10] = boneOrientation.qw;
return data;
}
}
//[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);
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 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);
}
#endregion Pose
#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 SendFloat16(BinaryWriter bw, float f) {
bw.Write((byte)0x3C); // Dummy value 1
bw.Write((byte)0x00);
}
#endregion Send
}
}
#endif