RoboidControl/RoboidControl-csharp
RoboidControl/RoboidControl-csharp
1
0
Fork 0
Code Issues Pull Requests Actions 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 d8fc41f1c4
commit a48ae12fc2
2 changed files with 80 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
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
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++];