1118 lines
46 KiB
C#
1118 lines
46 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.IO;
|
|
using System.Net;
|
|
using System.Net.Sockets;
|
|
using System.Threading.Tasks;
|
|
using UnityEngine;
|
|
using RoboidControl.Unity;
|
|
|
|
|
|
|
|
#if hNW_ROBOID
|
|
|
|
using RoboidControl;
|
|
using LinearAlgebra;
|
|
|
|
namespace Passer.Humanoid
|
|
{
|
|
|
|
public partial class HumanoidPlayer : MonoBehaviour, IHumanoidNetworking
|
|
{ //}, ISiteServer {
|
|
|
|
[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 sitePort = 7681;
|
|
public string siteAddress = "127.0.0.1";
|
|
// protected UdpClient udpClient;
|
|
// protected IPEndPoint endPoint;
|
|
// protected object obj;
|
|
// protected AsyncCallback receiveCallback;
|
|
//protected HumanoidClient client;
|
|
//protected Stream dataStream;
|
|
|
|
protected HumanoidParticipant participant;
|
|
// This should be one core thing per humanoid in the end
|
|
protected RoboidControl.Thing core;
|
|
|
|
protected HumanoidControl humanoid; // we only support one humanoid ATM
|
|
protected RoboidControl.Thing headThing;
|
|
|
|
|
|
//public byte networkId;
|
|
|
|
public const byte Pose_Position = 0x01;
|
|
public const byte Pose_Orientation = 0x02;
|
|
public const byte Pose_LinearVelocity = 0x04;
|
|
public const byte Pose_AngularVelocity = 0x08;
|
|
|
|
#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
|
|
|
|
public class HumanoidParticipant : RoboidControl.ParticipantUDP
|
|
{
|
|
readonly HumanoidPlayer player;
|
|
|
|
public HumanoidParticipant(string ipAddress, int port, HumanoidPlayer player) : base(ipAddress, port)
|
|
{
|
|
this.player = player;
|
|
}
|
|
|
|
/*
|
|
protected override void Process(Participant sender, NetworkIdMsg msg)
|
|
{
|
|
Debug.Log($"NetworkId [{msg.networkId}]");
|
|
if (this.networkId == msg.networkId)
|
|
return;
|
|
|
|
this.networkId = msg.networkId;
|
|
player.ProcessNetworkId(this, msg);
|
|
}
|
|
|
|
protected override void Process(Participant sender, InvestigateMsg msg) {
|
|
// Debug.Log($"Investigate [{msg.networkId}/{msg.thingId}]");
|
|
// Thing thing = Thing.GetThing(msg.networkId, msg.thingId);
|
|
// Thing parentThing = thing.transform.parent.GetComponentInParent<Thing>();
|
|
// if (parentThing != null)
|
|
// ThingMsg.Send(this, thing.networkId, thing.thingId, thing.objectType, parentThing.thingId);
|
|
// else
|
|
// ThingMsg.Send(this, thing.networkId, thing.thingId, thing.objectType, 0);
|
|
// NameMsg.Send(this, thing.networkId, thing.thingId, thing.name);
|
|
// if (thing.modelUrl != null)
|
|
// ModelUrlMsg.Send(this, thing.networkId, thing.thingId, thing.modelUrl);
|
|
}
|
|
|
|
protected override void Process(Participant sender, ThingMsg thing) {
|
|
player.ProcessThing(this, thing);
|
|
}
|
|
|
|
protected override void Process(Participant sender, NameMsg msg) {
|
|
Debug.Log($"Name [{msg.networkId}/{msg.thingId}] {msg.name}");
|
|
Thing thing = Thing.GetThing(msg.networkId, msg.thingId);
|
|
if (thing == null) {
|
|
Debug.LogWarning($"could not find thing [{msg.networkId}/{msg.thingId}] for name {msg.name}");
|
|
return;
|
|
}
|
|
thing.gameObject.name = msg.name;
|
|
}
|
|
|
|
protected override void Process(Participant sender, ModelUrlMsg msg) {
|
|
Debug.Log($"Model [{msg.networkId}/{msg.thingId}] {msg.url}");
|
|
// int ix = msg.url.LastIndexOf(".");
|
|
// if (ix < 0)
|
|
// return;
|
|
|
|
// 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") {
|
|
// player.ProcessGltfModel(msg);
|
|
// }
|
|
}
|
|
|
|
protected override void Process(Participant sender, PoseMsg msg) {
|
|
Thing thing = Thing.allThings.Find(thing => thing.networkId == msg.networkId && thing.thingId == msg.thingId);
|
|
if (thing == null) {
|
|
Debug.Log($"Pose for unknown thing [{msg.networkId}/{msg.thingId}] -> create thing");
|
|
thing = player.CreateThing(this, msg.networkId, msg.thingId, 0, 0);
|
|
InvestigateMsg.Send(this, msg.networkId, msg.thingId);
|
|
}
|
|
if (msg.thingId != 0)
|
|
Debug.Log($"Pose [{msg.networkId}/{msg.thingId}] {thing.transform.name} {ToQuaternion(msg.orientation).eulerAngles} ");
|
|
if ((msg.poseType & Pose_Position) != 0)
|
|
thing.transform.localPosition = ToVector3(msg.position);
|
|
if ((msg.poseType & Pose_Orientation) != 0) {
|
|
thing.transform.localRotation = ToQuaternion(msg.orientation);
|
|
}
|
|
}
|
|
*/
|
|
}
|
|
|
|
protected virtual void Awake()
|
|
{
|
|
mInstance = this;
|
|
|
|
Console.SetOut(new UnityLogWriter());
|
|
this.participant = new HumanoidParticipant(siteAddress, sitePort, this);
|
|
|
|
// endPoint = new IPEndPoint(IPAddress.Any, sitePort + 3);
|
|
// udpClient = new UdpClient(endPoint);
|
|
// receiveCallback = new System.AsyncCallback(result => ReceiveUDP(client, result));
|
|
// udpClient.BeginReceive(receiveCallback, obj);
|
|
// client = new HumanoidClient(udpClient, siteAddress, sitePort, this);
|
|
|
|
GameObject.DontDestroyOnLoad(this.gameObject);
|
|
humanoids = HumanoidNetworking.FindLocalHumanoids();
|
|
|
|
for (int i = 0; i < humanoids.Count; i++)
|
|
{
|
|
HumanoidControl aHumanoid = humanoids[i];
|
|
if (aHumanoid.isRemote)
|
|
continue;
|
|
|
|
aHumanoid.humanoidNetworking = this;
|
|
}
|
|
if (humanoids.Count != 1)
|
|
{
|
|
Debug.LogError("Roboid Networking only supports 1 humanoid");
|
|
return;
|
|
}
|
|
|
|
humanoid = humanoids[0];
|
|
headThing = new BoneThing(humanoid.headTarget.head);
|
|
// this.participant.Send(new ThingMsg((byte)humanoid.nwId, headThing));
|
|
//
|
|
//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) {
|
|
// try {
|
|
// 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);
|
|
// }
|
|
// }
|
|
// catch (Exception e) {
|
|
// Debug.Log("connection error " + e);
|
|
// }
|
|
// }
|
|
// else {
|
|
// Debug.Log("Client disconnected");
|
|
// }
|
|
// udpClient.BeginReceive(new System.AsyncCallback(result => ReceiveUDP(client, result)), null);
|
|
// }
|
|
|
|
// protected void OnApplicationQuit() {
|
|
// if (udpClient != null) {
|
|
// udpClient.Client.Close();
|
|
// udpClient.Close();
|
|
// udpClient = null;
|
|
// }
|
|
// }
|
|
|
|
#region Update
|
|
|
|
float lastSend;
|
|
const float clientMsgInterval = 3;
|
|
float lastClientMsg = -clientMsgInterval;
|
|
|
|
protected void Update()
|
|
{
|
|
while (this.participant.updateQueue.TryDequeue(out RoboidControl.Participant.UpdateEvent e))
|
|
HandleUpdateEvent(e);
|
|
}
|
|
|
|
private void HandleUpdateEvent(RoboidControl.Participant.UpdateEvent e)
|
|
{
|
|
switch (e.messageId)
|
|
{
|
|
case NetworkIdMsg.Id:
|
|
ProcessNetworkId();
|
|
break;
|
|
}
|
|
}
|
|
|
|
protected virtual void LateUpdate()
|
|
{
|
|
// foreach (Client client in Client.clients) {
|
|
// if (client is HumanoidParticipant humanoidClient) {
|
|
// while (humanoidClient.messageQueue.TryDequeue(out Passer.Control.IMessage msg))
|
|
// humanoidClient.ProcessMessage(msg);
|
|
// }
|
|
// }
|
|
|
|
if (Time.time > lastSend + 1 / sendRate)
|
|
{
|
|
if (humanoids != null)
|
|
{
|
|
foreach (HumanoidControl humanoid in humanoids)
|
|
{
|
|
if (!humanoid.isRemote)
|
|
{
|
|
UpdateHumanoidPose(humanoid);
|
|
}
|
|
}
|
|
}
|
|
lastSend = Time.time;
|
|
}
|
|
if (Time.time > lastClientMsg + clientMsgInterval)
|
|
{
|
|
//SendClientMsg(humanoids[0]); // We just need it for the networkId
|
|
this.participant.Send(new ParticipantMsg((byte)humanoid.nwId));
|
|
lastClientMsg = Time.time;
|
|
}
|
|
}
|
|
|
|
#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);
|
|
ClientMsg.Publish(client, (byte)humanoid.nwId);
|
|
}
|
|
*/
|
|
#endregion Client
|
|
|
|
#region Investigate
|
|
/*
|
|
protected void ProcessInvestigate(InvestigateMsg investigate) {
|
|
|
|
}
|
|
*/
|
|
#endregion Investigate
|
|
|
|
#region Thing
|
|
/*
|
|
protected virtual void Process(Participant client, ThingMsg msg) {
|
|
DebugLog($"Process Thing [{msg.networkId}/{msg.thingId}] parent:{msg.parentId} type: {msg.thingType}");
|
|
|
|
Thing thing = Thing.GetThing(msg.networkId, msg.thingId);
|
|
if (thing == null)
|
|
CreateThing(client, msg.networkId, msg.thingId, msg.thingType, msg.parentId);
|
|
|
|
else {
|
|
thing.objectType = msg.thingType;
|
|
if (msg.parentId != 0) {
|
|
Thing parentThing = Thing.GetThing(msg.networkId, msg.parentId);
|
|
if (parentThing != null) {
|
|
if (thing.transform.parent != parentThing.transform) {
|
|
Debug.Log($"Changed parent to {parentThing.gameObject.name} {parentThing.thingId}");
|
|
thing.transform.SetParent(parentThing.transform, true);
|
|
}
|
|
}
|
|
else {
|
|
Debug.LogWarning($"Could not find the parent {msg.parentId} for {msg.thingId}");
|
|
thing.transform.SetParent(this.transform);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
protected virtual Thing CreateThing(Participant client, byte networkId, byte thingId, byte thingType, byte parentId) {
|
|
DebugLog($"Create new thing [{networkId}/{thingId}]");
|
|
GameObject thingObject = new("Thing " + thingId);
|
|
Thing thing = Thing.Create(thingObject, client, networkId, thingId, thingType);
|
|
if (parentId != 0) {
|
|
Thing parentThing = Thing.allThings.Find(thing => thing.networkId == networkId && thing.thingId == parentId);
|
|
if (parentThing != null)
|
|
thing.transform.SetParent(parentThing.transform);
|
|
else {
|
|
Debug.LogWarning($"Could not find the parent {parentId} for {thingId}");
|
|
thing.transform.SetParent(this.transform);
|
|
}
|
|
}
|
|
|
|
return thing;
|
|
}
|
|
*/
|
|
#endregion Thing
|
|
|
|
#region Pose
|
|
|
|
public virtual void UpdateHumanoidPose(HumanoidControl humanoid)
|
|
{
|
|
if (debug <= HumanoidNetworking.DebugLevel.Debug)
|
|
Debug.Log("Send Pose Humanoid " + humanoid.humanoidId + " nwId: " + humanoid.nwId);
|
|
|
|
this.participant.Send(new PoseMsg((byte)humanoid.nwId, headThing));
|
|
// 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, false, true);
|
|
|
|
// SendBone(humanoid.rightHandTarget.upperArm);
|
|
// SendBone(humanoid.rightHandTarget.forearm);
|
|
// SendBone(humanoid.rightHandTarget.hand, false, true);
|
|
|
|
// 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, bool calculateOrientation = 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);
|
|
|
|
// if (isRoot)
|
|
// {
|
|
// Quaternion orientation = bone.bone.transform.rotation;
|
|
// Quat32 boneOrientation = new(orientation.x, orientation.y, orientation.z, orientation.w);
|
|
// 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);
|
|
// this.participant.Send(new PoseMsg(networkId, boneThing))
|
|
// }
|
|
// else
|
|
// {
|
|
// Quaternion orientation = bone.bone.transform.localRotation;
|
|
// if (calculateOrientation)
|
|
// orientation = Quaternion.Inverse(bone.parent.bone.transform.rotation) * bone.bone.transform.rotation;
|
|
// //orientation = Quaternion.Inverse(bone.bone.transform.parent.rotation) * bone.bone.transform.rotation;
|
|
// Quat32 boneOrientation = new(orientation.x, orientation.y, orientation.z, orientation.w);
|
|
// PoseMsg.Send(client, (byte)bone.boneId, null, boneOrientation);
|
|
// }
|
|
// }
|
|
|
|
#endregion Pose
|
|
|
|
//#region Messages
|
|
|
|
#region NetworkId
|
|
|
|
void ProcessNetworkId()
|
|
{
|
|
//this.networkId = msg.networkId;
|
|
GameObject networkingObj = this.GetGameObject(this.participant.networkId);
|
|
if (networkingObj == null)
|
|
{
|
|
if (this.debug <= HumanoidNetworking.DebugLevel.Error)
|
|
this.DebugLog("Could not find Networking for Instantiate Humanoid " + this.participant.networkId + "/0");
|
|
return;
|
|
}
|
|
HumanoidControl remoteHumanoid = humanoids[0]; //FindRemoteHumanoid(humanoids, 0);
|
|
if (remoteHumanoid == null)
|
|
{
|
|
if (debug <= HumanoidNetworking.DebugLevel.Warning)
|
|
DebugLog("Remote Humanoid " + this.participant.networkId + "/" + 0 + "not found");
|
|
return;
|
|
}
|
|
remoteHumanoid.nwId = this.participant.networkId;
|
|
|
|
this.participant.Send(new ThingMsg((byte)humanoid.nwId, headThing));
|
|
/*
|
|
//SendThing(client, remoteHumanoid);
|
|
ThingMsg.Send(client, msg.networkId, 0, 1, 0);
|
|
NameMsg.Send(client, msg.networkId, 0, "Humanoid");
|
|
//SendName(remoteHumanoid, 0, "Humanoid");
|
|
//SendModel(remoteHumanoid, "https://gitlab.passervr.com/passer/models/humanoid/-/raw/main/MakeHumanPasserMedium.glb?ref_type=heads&inline=false");
|
|
ModelUrlMsg.Send(client, msg.networkId, 0, "https://gitlab.passervr.com/passer/models/humanoid/-/raw/main/MakeHumanPasserMedium.glb?ref_type=heads&inline=false");
|
|
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.hipsTarget.hips);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.hipsTarget.spine);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.hipsTarget.chest);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.headTarget.neck);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.headTarget.head);
|
|
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.leftHandTarget.shoulder);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.leftHandTarget.upperArm);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.leftHandTarget.forearm);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.leftHandTarget.hand);
|
|
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.rightHandTarget.shoulder);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.rightHandTarget.upperArm);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.rightHandTarget.forearm);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.rightHandTarget.hand);
|
|
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.rightFootTarget.upperLeg);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.rightFootTarget.lowerLeg);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.rightFootTarget.foot);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.rightFootTarget.toes);
|
|
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.leftFootTarget.upperLeg);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.leftFootTarget.lowerLeg);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.leftFootTarget.foot);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.leftFootTarget.toes);
|
|
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.rightFootTarget.upperLeg);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.rightFootTarget.lowerLeg);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.rightFootTarget.foot);
|
|
InitBone(client, remoteHumanoid, remoteHumanoid.rightFootTarget.toes);
|
|
*/
|
|
}
|
|
|
|
// protected void InitBone(Client client, HumanoidControl humanoid, HumanoidTarget.TargetedBone bone)
|
|
// {
|
|
// Thing thing = Thing.Create(bone.bone.transform.gameObject, client, client.networkId, (byte)bone.boneId, 0x00);
|
|
// if (bone.parent != null)
|
|
// ThingMsg.Send(client, thing.networkId, thing.thingId, thing.objectType, (byte)bone.parent.boneId);
|
|
// else
|
|
// ThingMsg.Send(client, thing.networkId, thing.thingId, thing.objectType, 0);
|
|
// NameMsg.Send(client, thing.networkId, thing.thingId, bone.bone.transform.name);
|
|
// }
|
|
|
|
#endregion
|
|
|
|
//#region Thing
|
|
|
|
|
|
//public void SendThing(Client client, HumanoidControl humanoid) {
|
|
// if (debug <= HumanoidNetworking.DebugLevel.Debug)
|
|
// Debug.Log("Send Thing " + humanoid.humanoidId + " nwId: " + humanoid.nwId);
|
|
|
|
// ThingMsg.Send(client, 0, 1, 0);
|
|
// //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
|
|
|
|
//#region SubThing
|
|
|
|
//public void SendSubThing(HumanoidControl humanoid, HumanoidTarget.TargetedBone bone) {
|
|
// if (debug <= HumanoidNetworking.DebugLevel.Debug)
|
|
// Debug.Log("Send SubThing " + humanoid.humanoidId + " nwId: " + humanoid.nwId);
|
|
|
|
// if (bone.parent != null)
|
|
// ThingMsg.Send(client, (byte)bone.boneId, 0x00, (byte)bone.parent.boneId);
|
|
// else
|
|
// ThingMsg.Send(client, (byte)bone.boneId, 0x00, 0);
|
|
|
|
// //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 void SendName(HumanoidControl humanoid, byte thingId, string name) {
|
|
// if (debug <= HumanoidNetworking.DebugLevel.Debug)
|
|
// Debug.Log("Send Name " + humanoid.humanoidId + " nwId: " + humanoid.nwId);
|
|
|
|
// 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);
|
|
// //}
|
|
//}
|
|
|
|
//#endregion
|
|
|
|
//#region Model
|
|
|
|
//public void SendModel(HumanoidControl humanoid, string url) {
|
|
// if (debug <= HumanoidNetworking.DebugLevel.Debug)
|
|
// Debug.Log("Send URL " + humanoid.humanoidId + " nwId: " + humanoid.nwId);
|
|
|
|
// ModelUrlMsg.Send(client, 0, url);
|
|
//}
|
|
|
|
// bool loaded = false;
|
|
// protected async void ProcessModelURL(Client client, 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 = FindObjectsByType<Light>(FindObjectsSortMode.None);
|
|
// 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>();
|
|
// }
|
|
// }
|
|
|
|
|
|
//void ISiteServer.ProcessModelUrl(Client client, ModelUrlMsg msg) {
|
|
// int ix = msg.url.LastIndexOf(".");
|
|
// if (ix < 0)
|
|
// return;
|
|
|
|
// 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);
|
|
// }
|
|
//}
|
|
|
|
#region GLTF
|
|
|
|
private bool loadingModel = false;
|
|
//private Dictionary<string, GltfImport> gltfCache = new();
|
|
|
|
// private async void ProcessGltfModel(ModelUrlMsg msg) {
|
|
// Transform parentTransform = this.transform;
|
|
// Thing parentThing = Thing.GetThing(msg.networkId, msg.thingId);
|
|
// if (parentThing != null) {
|
|
// if (parentThing.modelUrl == msg.url) {
|
|
// // Thing already has this model
|
|
// Debug.Log($"thing [{msg.networkId}/{msg.thingId}] already has model {msg.url}");
|
|
// return;
|
|
// }
|
|
// else
|
|
// parentThing.modelUrl = msg.url;
|
|
// parentTransform = parentThing.transform;
|
|
// }
|
|
|
|
// if (gltfCache.TryGetValue(msg.url, out GltfImport gltfImport)) {
|
|
// Debug.Log($"Instantiate [{msg.networkId}/{msg.thingId}] ***** buffered GLTF: " + msg.url);
|
|
// await gltfImport.InstantiateMainSceneAsync(parentTransform);
|
|
// }
|
|
// else {
|
|
// Debug.Log("Loading GLTF model from: " + msg.url);
|
|
// gltfImport = new();
|
|
// bool success = await gltfImport.Load(msg.url);
|
|
// if (success == false)
|
|
// return;
|
|
|
|
// gltfCache.Add(msg.url, gltfImport);
|
|
// Debug.Log($"Instantiate [{msg.networkId}/{msg.thingId}] ***** loaded GLTF: " + msg.url);
|
|
// await gltfImport.InstantiateMainSceneAsync(parentTransform);
|
|
// }
|
|
// parentTransform.localScale = Vector3.one * msg.scale;
|
|
|
|
// ScanModelForThings(parentTransform);
|
|
|
|
// // Correct lights
|
|
// if (parentTransform != null) {
|
|
// Light[] lights = parentTransform.GetComponentsInChildren<Light>();
|
|
// foreach (Light light in lights)
|
|
// light.intensity = 1; // light.intensity / 1000;
|
|
// }
|
|
// }
|
|
|
|
//private async Task<Transform> 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 ScanModelForThings(Transform parentTransform)
|
|
// {
|
|
// if (parentTransform == null)
|
|
// return;
|
|
|
|
// Debug.Log("scanning model for things.....");
|
|
// SkinnedMeshRenderer[] meshRenderers = parentTransform.GetComponentsInChildren<SkinnedMeshRenderer>();
|
|
// if (meshRenderers.Length > 0)
|
|
// {
|
|
// foreach (SkinnedMeshRenderer meshRenderer in meshRenderers)
|
|
// {
|
|
// if (meshRenderer.rootBone != null)
|
|
// {
|
|
// Debug.Log("Found a skinned mesh with bones");
|
|
// ScanForThings(parentTransform);//meshRenderer.rootBone);
|
|
// break;
|
|
// }
|
|
// }
|
|
// }
|
|
// else
|
|
// {
|
|
// Renderer[] renderers = parentTransform.GetComponentsInChildren<Renderer>();
|
|
// foreach (Renderer renderer in renderers)
|
|
// {
|
|
// MeshCollider c = renderer.gameObject.AddComponent<MeshCollider>();
|
|
// c.convex = true;
|
|
// }
|
|
|
|
// ScanForThings(parentTransform);
|
|
// }
|
|
// }
|
|
|
|
// private void ScanForThings(Transform rootTransform)
|
|
// {
|
|
// Thing[] thingArray = Thing.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<Thing>();
|
|
// if (foundObj != thing.gameObject)
|
|
// {
|
|
// if (foundThing == null)
|
|
// {
|
|
// Debug.Log($"move thing [{thing.networkId}/{thing.thingId}] to {foundObj.name}");
|
|
// Thing.allThings.Remove(thing);
|
|
// foundThing = thing.CopyTo(foundObj);
|
|
// Thing.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<Thing>();
|
|
// if (childThing != null)
|
|
// {
|
|
// Thing.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);
|
|
// Thing.allThings.Add(newThing);
|
|
// }
|
|
// }
|
|
// }
|
|
// }
|
|
|
|
//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<Transform> 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<Thing>();
|
|
// 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<Thing>();
|
|
// 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;
|
|
// }
|
|
// }
|
|
|
|
// protected static Vector3 ToVector3(Spherical s)
|
|
// {
|
|
// Vector3 v = Quaternion.AngleAxis(s.horizontal, Vector3.up) * Quaternion.AngleAxis(s.vertical, Vector3.left) * (Vector3.forward * s.distance);
|
|
// return v;
|
|
// }
|
|
|
|
// protected static Quaternion ToQuaternion(Quat32 q32)
|
|
// {
|
|
// Quaternion q = new(q32.x, q32.y, q32.z, q32.w);
|
|
// return q;
|
|
// }
|
|
|
|
//#endregion Pose
|
|
|
|
//#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 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(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 |