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

ControlCore mostly works (but I don't see a model on the site server yet)

Browse Source
This commit is contained in:
Pascal Serrarens 2024-12-06 16:30:24 +01:00
parent 32af8e8df6
commit ceda03d1e7
3 changed files with 90 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/LowLevelMessages.cs
View File

@ -1,6 +1,13 @@
using Passer;
public class LowLevelMessages {
public static void SendSpherical(byte[] buffer, ref uint ix, Spherical v) {
SendFloat16(buffer, ref ix, new float16(v.distance));
SendAngle8(buffer, ref ix, v.horizontal);
SendAngle8(buffer, ref ix, v.vertical);
}
public static Spherical ReceiveSpherical(byte[] data, ref uint ix) {
float horizontal = ReceiveAngle8(data, ref ix);
float vertical = ReceiveAngle8(data, ref ix);
@ -9,6 +16,23 @@ public class LowLevelMessages {
return v;
}
public static void SendQuat32(byte[] buffer, ref uint ix, Quat32 q) {
int qx = (int)(q.x * 127 + 128);
int qy = (int)(q.y * 127 + 128);
int qz = (int)(q.z * 127 + 128);
int qw = (int)(q.w * 255);
if (q.w < 0) {
qx = -qx;
qy = -qy;
qz = -qz;
qw = -qw;
}
buffer[ix++] = (byte)qx;
buffer[ix++] = (byte)qy;
buffer[ix++] = (byte)qz;
buffer[ix++] = (byte)qw;
}
public static Quat32 ReceiveQuat32(byte[] data, ref uint ix) {
Quat32 q = new(
(data[ix++] - 128.0F) / 127.0F,
@ -18,11 +42,26 @@ public class LowLevelMessages {
return q;
}
public static void SendAngle8(byte[] buffer, ref uint ix, float angle) {
// Normalize angle
while (angle >= 180)
angle -= 360;
while (angle < -180)
angle += 360;
buffer[ix++] = (byte)((angle / 360.0f) * 256.0f);
}
public static float ReceiveAngle8(byte[] data, ref uint ix) {
float value = (data[ix++] * 180) / 128.0F;
return value;
}
public static void SendFloat16(byte[] data, ref uint ix, float16 f) {
ushort binary = f.GetBinary();
data[ix++] = (byte)(binary >> 8);
data[ix++] = (byte)(binary & 255);
}
public static float ReceiveFloat16(byte[] data, ref uint ix) {
ushort value = (ushort)(data[ix++] << 8 | data[ix++]);
float16 f16 = new();
@ -30,6 +69,4 @@ public class LowLevelMessages {
float f = f16.toFloat();
return f;
}
}

45
Runtime/HumanoidControl/Scripts/Networking/Roboid/ControlCore/Messages.cs
View File

@ -62,6 +62,18 @@ namespace Passer.Control {
client.udpClient.Send(data, data.Length, client.ipAddress, client.port);
return true;
}
public static bool PublishMsg(Client client, IMessage msg) {
return PublishMsg(client, msg.Serialize());
}
public static bool PublishMsg(Client client, byte[] data) {
if (client == null)
return false;
client.udpClient.Send(data, data.Length, "127.0.0.1", client.port);
return true;
}
public static async Task<byte[]> Receive(Stream dataStream, byte packetSize) {
byte[] buffer = new byte[packetSize - 1]; // without msgId
int byteCount = dataStream.Read(buffer, 0, packetSize - 1);
@ -86,7 +98,13 @@ namespace Passer.Control {
}
public ClientMsg(byte[] data) : base(data) { }
public override void Deserialize(byte[] data) {
public override byte[] Serialize() {
byte[] buffer = new byte[ClientMsg.length];
buffer[0] = ClientMsg.Id;
buffer[1] = networkId;
return buffer;
}
public override void Deserialize(byte[] data) {
base.Deserialize(data);
uint ix = 0;
networkId = data[ix];
@ -96,6 +114,10 @@ namespace Passer.Control {
ClientMsg msg = new(networkId);
return SendMsg(client, msg);
}
public static bool Publish(Client client, byte networkId) {
ClientMsg msg = new(networkId);
return PublishMsg(client, msg);
}
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
if (packetSize != length)
return false;
@ -212,7 +234,7 @@ namespace Passer.Control {
#region Thing
public class ThingMsg : IMessage {
public const byte length = 4;
public const byte length = 5;
public const byte Id = 0x80;
public byte objectId;
public byte objectType;
@ -227,7 +249,7 @@ namespace Passer.Control {
}
public static bool Send(Client client, byte thingId, byte thingType, byte parentId) {
byte[] data = new byte[4];
byte[] data = new byte[ThingMsg.length];
data[0] = ThingMsg.Id;
data[1] = client.networkId;
data[2] = thingId;
@ -370,12 +392,27 @@ namespace Passer.Control {
this.poseType = 0;
if (this.position != null)
this.poseType |= Pose_Position;
else
this.position = new Spherical(0, 0, 0);
if (this.orientation != null)
this.poseType |= Pose_Orientation;
else
this.orientation = new Quat32(0, 0, 0, 1);
}
public PoseMsg(byte[] data) : base(data) { }
public override void Deserialize(byte[] data) {
public override byte[] Serialize() {
byte[] buffer = new byte[PoseMsg.length];
uint ix = 0;
buffer[ix++] = PoseMsg.Id;
buffer[ix++] = this.thingId;
buffer[ix++] = this.poseType;
LowLevelMessages.SendSpherical(buffer, ref ix, position);
LowLevelMessages.SendQuat32(buffer, ref ix, orientation);
return buffer;
}
public override void Deserialize(byte[] data) {
uint ix = 0;
thingId = data[ix++];
poseType = data[ix++];

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

@ -188,8 +188,8 @@ namespace Passer.Humanoid {
#region Update
float lastSend;
float lastClientMsg;
const float clientMsgInterval = 3;
float lastClientMsg = -clientMsgInterval;
protected virtual void LateUpdate() {
foreach (Client client in Client.clients) {
@ -211,7 +211,7 @@ namespace Passer.Humanoid {
}
lastSend = Time.time;
}
if (Time.time + clientMsgInterval > lastClientMsg) {
if (Time.time > lastClientMsg + clientMsgInterval) {
SendClientMsg(humanoids[0]); // We just need it for the networkId
lastClientMsg = Time.time;
}
@ -283,7 +283,7 @@ namespace Passer.Humanoid {
Debug.Log("Send ClientMsg " + humanoid.nwId);
//ClientMsg clientMsg = new((byte)humanoid.nwId);
//SendMsg(clientMsg);
ClientMsg.Send(client, (byte)humanoid.nwId);
ClientMsg.Publish(client, (byte)humanoid.nwId);
}
#endregion Client
@ -375,6 +375,7 @@ namespace Passer.Humanoid {
return;
this.networkId = msg.networkId;
client.networkId = msg.networkId;
GameObject networkingObj = this.GetGameObject(msg.networkId);
if (networkingObj == null) {
if (this.debug <= HumanoidNetworking.DebugLevel.Error)
@ -387,6 +388,7 @@ namespace Passer.Humanoid {
DebugLog("Remote Humanoid " + msg.networkId + "/" + 0 + "not found");
return;
}
remoteHumanoid.nwId = msg.networkId;
SendThing(client, remoteHumanoid);
SendName(remoteHumanoid, 0, "Humanoid");
@ -512,8 +514,11 @@ namespace Passer.Humanoid {
if (debug <= HumanoidNetworking.DebugLevel.Debug)
Debug.Log("Send SubThing " + humanoid.humanoidId + " nwId: " + humanoid.nwId);
ThingMsg.Send(client, (byte)bone.boneId, 0x00, (byte)bone.parent.boneId);
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);