RoboidControl/RoboidControl-csharp
RoboidControl/RoboidControl-csharp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Ant is hopping around (pose does not look right yet...)

Browse Source
This commit is contained in:
Pascal Serrarens 2024-12-13 13:37:53 +01:00
parent 0d022c26ef
commit 17fa48a266
6 changed files with 99 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
Client.cs
View File

@ -11,6 +11,7 @@ namespace Passer.Control
//public ConnectionMethod connection; //public ConnectionMethod connection;
public UdpClient udpClient; public UdpClient udpClient;
public string ipAddress; public string ipAddress;
public string broadcastIpAddress = "255.255.255.255";
public int port; public int port;
public Stream dataStream; public Stream dataStream;
@ -40,6 +41,25 @@ namespace Passer.Control
clients.Add(this); clients.Add(this);
} }
public bool SendBuffer()
{
if (this.ipAddress == null)
return false;
//UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}");
this.udpClient.Send(this.buffer, this.buffer.Length, this.ipAddress, this.port);
return true;
}
public bool PublishBuffer()
{
if (this.broadcastIpAddress == null)
return false;
this.udpClient.Send(this.buffer, this.buffer.Length, this.broadcastIpAddress, this.port);
return true;
}
public virtual void ProcessMessages() public virtual void ProcessMessages()
{ {
while (this.messageQueue.TryDequeue(out IMessage msg)) while (this.messageQueue.TryDequeue(out IMessage msg))

32
LowLevelMessages.cs
View File

@ -1,14 +1,17 @@
using Passer; using Passer.LinearAlgebra;
public class LowLevelMessages { public class LowLevelMessages
{
public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v) { public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v)
{
SendAngle8(buffer, ref ix, v.horizontal); SendAngle8(buffer, ref ix, v.horizontal);
SendAngle8(buffer, ref ix, v.vertical); SendAngle8(buffer, ref ix, v.vertical);
SendFloat16(buffer, ref ix, new float16(v.distance)); SendFloat16(buffer, ref ix, new float16(v.distance));
} }
public static Spherical ReceiveSpherical(byte[] data, ref byte ix) { public static Spherical ReceiveSpherical(byte[] data, ref byte ix)
{
float horizontal = ReceiveAngle8(data, ref ix); float horizontal = ReceiveAngle8(data, ref ix);
float vertical = ReceiveAngle8(data, ref ix); float vertical = ReceiveAngle8(data, ref ix);
float distance = ReceiveFloat16(data, ref ix); float distance = ReceiveFloat16(data, ref ix);
@ -16,12 +19,14 @@ public class LowLevelMessages {
return v; return v;
} }
public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q) { public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q)
{
int qx = (int)(q.x * 127 + 128); int qx = (int)(q.x * 127 + 128);
int qy = (int)(q.y * 127 + 128); int qy = (int)(q.y * 127 + 128);
int qz = (int)(q.z * 127 + 128); int qz = (int)(q.z * 127 + 128);
int qw = (int)(q.w * 255); int qw = (int)(q.w * 255);
if (q.w < 0) { if (q.w < 0)
{
qx = -qx; qx = -qx;
qy = -qy; qy = -qy;
qz = -qz; qz = -qz;
@ -33,7 +38,8 @@ public class LowLevelMessages {
buffer[ix++] = (byte)qz; buffer[ix++] = (byte)qz;
buffer[ix++] = (byte)qw; buffer[ix++] = (byte)qw;
} }
public static Quat32 ReceiveQuat32(byte[] data, ref byte ix) { public static Quat32 ReceiveQuat32(byte[] data, ref byte ix)
{
Quat32 q = new( Quat32 q = new(
(data[ix++] - 128.0F) / 127.0F, (data[ix++] - 128.0F) / 127.0F,
(data[ix++] - 128.0F) / 127.0F, (data[ix++] - 128.0F) / 127.0F,
@ -42,7 +48,8 @@ public class LowLevelMessages {
return q; return q;
} }
public static void SendAngle8(byte[] buffer, ref byte ix, float angle) { public static void SendAngle8(byte[] buffer, ref byte ix, float angle)
{
// Normalize angle // Normalize angle
while (angle >= 180) while (angle >= 180)
angle -= 360; angle -= 360;
@ -51,18 +58,21 @@ public class LowLevelMessages {
buffer[ix++] = (byte)((angle / 360.0f) * 256.0f); buffer[ix++] = (byte)((angle / 360.0f) * 256.0f);
} }
public static float ReceiveAngle8(byte[] data, ref byte ix) { public static float ReceiveAngle8(byte[] data, ref byte ix)
{
float value = (data[ix++] * 180) / 128.0F; float value = (data[ix++] * 180) / 128.0F;
return value; return value;
} }
public static void SendFloat16(byte[] data, ref byte ix, float16 f) { public static void SendFloat16(byte[] data, ref byte ix, float16 f)
{
ushort binary = f.GetBinary(); ushort binary = f.GetBinary();
data[ix++] = (byte)(binary >> 8); data[ix++] = (byte)(binary >> 8);
data[ix++] = (byte)(binary & 255); data[ix++] = (byte)(binary & 255);
} }
public static float ReceiveFloat16(byte[] data, ref byte ix) { public static float ReceiveFloat16(byte[] data, ref byte ix)
{
ushort value = (ushort)(data[ix++] << 8 | data[ix++]); ushort value = (ushort)(data[ix++] << 8 | data[ix++]);
float16 f16 = new(); float16 f16 = new();
f16.SetBinary(value); f16.SetBinary(value);

93
Messages.cs
View File

@ -1,6 +1,7 @@
using System.IO; using System.IO;
using System.Threading.Tasks; using System.Threading.Tasks;
using Passer.LinearAlgebra;
namespace Passer.Control namespace Passer.Control
{ {
@ -12,34 +13,19 @@ namespace Passer.Control
Deserialize(buffer); Deserialize(buffer);
} }
public virtual byte[] Serialize() { return null; } public virtual byte Serialize(ref byte[] buffer) { return 0; }
public virtual void Deserialize(byte[] buffer) { } public virtual void Deserialize(byte[] buffer) { }
public static bool SendMsg(Client client, IMessage msg) public static bool SendMsg(Client client, IMessage msg)
{ {
return SendMsg(client, msg.Serialize()); msg.Serialize(ref client.buffer);
} return client.SendBuffer();
public static bool SendMsg(Client client, byte[] buffer)
{
if (client == null || client.ipAddress == null)
return false;
//UnityEngine.Debug.Log($"Send msg {buffer[0]} to {client.ipAddress}");
client.udpClient.Send(buffer, buffer.Length, client.ipAddress, client.port);
return true;
} }
public static bool PublishMsg(Client client, IMessage msg) public static bool PublishMsg(Client client, IMessage msg)
{ {
return PublishMsg(client, msg.Serialize()); msg.Serialize(ref client.buffer);
} return client.PublishBuffer();
public static bool PublishMsg(Client client, byte[] buffer)
{
if (client == null)
return false;
client.udpClient.Send(buffer, buffer.Length, "127.0.0.1", client.port);
return true;
} }
public static async Task<byte[]> Receive(Stream dataStream, byte packetSize) public static async Task<byte[]> Receive(Stream dataStream, byte packetSize)
@ -70,12 +56,12 @@ namespace Passer.Control
} }
public ClientMsg(byte[] buffer) : base(buffer) { } public ClientMsg(byte[] buffer) : base(buffer) { }
public override byte[] Serialize() public override byte Serialize(ref byte[] buffer)
{ {
byte[] buffer = new byte[ClientMsg.length]; byte ix = 0;
buffer[0] = ClientMsg.Id; buffer[ix++] = ClientMsg.Id;
buffer[1] = networkId; buffer[ix++] = networkId;
return buffer; return ix;
} }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer)
{ {
@ -134,12 +120,12 @@ namespace Passer.Control
} }
NetworkIdMsg(byte[] buffer) : base(buffer) { } NetworkIdMsg(byte[] buffer) : base(buffer) { }
public override byte[] Serialize() public override byte Serialize(ref byte[] buffer)
{ {
byte[] buffer = new byte[NetworkIdMsg.length]; byte ix = 0;
buffer[0] = NetworkIdMsg.Id; buffer[ix++] = NetworkIdMsg.Id;
buffer[1] = this.networkId; buffer[ix++] = this.networkId;
return buffer; return ix;
} }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer)
{ {
@ -182,13 +168,13 @@ namespace Passer.Control
} }
public InvestigateMsg(byte[] buffer) : base(buffer) { } public InvestigateMsg(byte[] buffer) : base(buffer) { }
public override byte[] Serialize() public override byte Serialize(ref byte[] buffer)
{ {
byte[] buffer = new byte[InvestigateMsg.length]; byte ix = 0;
buffer[0] = InvestigateMsg.Id; buffer[ix++] = InvestigateMsg.Id;
buffer[1] = this.networkId; buffer[ix++] = this.networkId;
buffer[2] = this.thingId; buffer[ix++] = this.thingId;
return buffer; return ix;
} }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer)
{ {
@ -240,16 +226,15 @@ namespace Passer.Control
} }
public ThingMsg(byte[] buffer) : base(buffer) { } public ThingMsg(byte[] buffer) : base(buffer) { }
public override byte[] Serialize() public override byte Serialize(ref byte[] buffer)
{ {
byte[] buffer = new byte[ThingMsg.length];
byte ix = 0; byte ix = 0;
buffer[ix++] = ThingMsg.Id; buffer[ix++] = ThingMsg.Id;
buffer[ix++] = this.networkId; buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId; buffer[ix++] = this.thingId;
buffer[ix++] = this.thingType; buffer[ix++] = this.thingType;
buffer[ix] = this.parentId; buffer[ix++] = this.parentId;
return buffer; return ix;
} }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer)
{ {
@ -305,17 +290,16 @@ namespace Passer.Control
} }
public NameMsg(byte[] buffer) : base(buffer) { } public NameMsg(byte[] buffer) : base(buffer) { }
public override byte[] Serialize() public override byte Serialize(ref byte[] buffer)
{ {
byte[] buffer = new byte[length + this.name.Length];
byte ix = 0; byte ix = 0;
buffer[ix++] = NameMsg.Id; buffer[ix++] = NameMsg.Id;
buffer[ix++] = this.networkId; buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId; buffer[ix++] = this.thingId;
buffer[ix++] = (byte)this.name.Length; buffer[ix++] = (byte)this.name.Length;
for (int nameIx = 0; nameIx < this.name.Length; nameIx++, ix++) for (int nameIx = 0; nameIx < this.name.Length; nameIx++)
buffer[ix] = (byte)this.name[nameIx]; buffer[ix++] = (byte)this.name[nameIx];
return buffer; return ix;
} }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer)
{ {
@ -364,9 +348,8 @@ namespace Passer.Control
} }
public ModelUrlMsg(byte[] buffer) : base(buffer) { } public ModelUrlMsg(byte[] buffer) : base(buffer) { }
public override byte[] Serialize() public override byte Serialize(ref byte[] buffer)
{ {
byte[] buffer = new byte[this.url.Length + 6];
byte ix = 0; byte ix = 0;
buffer[ix++] = ModelUrlMsg.Id; buffer[ix++] = ModelUrlMsg.Id;
buffer[ix++] = this.networkId; buffer[ix++] = this.networkId;
@ -374,9 +357,9 @@ namespace Passer.Control
LowLevelMessages.SendFloat16(buffer, ref ix, new float16(1.0f)); LowLevelMessages.SendFloat16(buffer, ref ix, new float16(1.0f));
buffer[ix++] = (byte)url.Length; buffer[ix++] = (byte)url.Length;
for (int urlIx = 0; urlIx < this.url.Length; urlIx++, ix++) for (int urlIx = 0; urlIx < this.url.Length; urlIx++)
buffer[ix] = (byte)url[urlIx]; buffer[ix++] = (byte)url[urlIx];
return buffer; return ix;
} }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer)
{ {
@ -441,9 +424,8 @@ namespace Passer.Control
} }
public PoseMsg(byte[] buffer) : base(buffer) { } public PoseMsg(byte[] buffer) : base(buffer) { }
public override byte[] Serialize() public override byte Serialize(ref byte[] buffer)
{ {
byte[] buffer = new byte[PoseMsg.length];
byte ix = 0; byte ix = 0;
buffer[ix++] = PoseMsg.Id; buffer[ix++] = PoseMsg.Id;
buffer[ix++] = this.networkId; buffer[ix++] = this.networkId;
@ -452,7 +434,7 @@ namespace Passer.Control
LowLevelMessages.SendSpherical(buffer, ref ix, this.position); LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation); LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation);
return buffer; return ix;
} }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer)
{ {
@ -508,9 +490,8 @@ namespace Passer.Control
this.bytes = bytes; this.bytes = bytes;
} }
public override byte[] Serialize() public override byte Serialize(ref byte[] buffer)
{ {
byte[] buffer = new byte[3 + this.bytes.Length];
byte ix = 0; byte ix = 0;
buffer[ix++] = CustomMsg.Id; buffer[ix++] = CustomMsg.Id;
buffer[ix++] = this.networkId; buffer[ix++] = this.networkId;
@ -519,7 +500,7 @@ namespace Passer.Control
foreach (byte b in bytes) foreach (byte b in bytes)
buffer[ix++] = b; buffer[ix++] = b;
return buffer; return ix;
} }
public override void Deserialize(byte[] buffer) public override void Deserialize(byte[] buffer)
{ {

11
Quat32.cs
View File

@ -1,15 +1,18 @@
namespace Passer { namespace Passer.LinearAlgebra
public class Quat32 { {
public class Quat32
{
public float x; public float x;
public float y; public float y;
public float z; public float z;
public float w; public float w;
public Quat32(float x, float y, float z, float w) { public Quat32(float x, float y, float z, float w)
{
this.x = x; this.x = x;
this.y = y; this.y = y;
this.z = z; this.z = z;
this.w = w; this.w = w;
} }
} }
} }

9
Spherical.cs
View File

@ -1,12 +1,15 @@
namespace Passer { namespace Passer.LinearAlgebra
public class Spherical { {
public class Spherical
{
public float distance; public float distance;
public float horizontal; public float horizontal;
public float vertical; public float vertical;
public static Spherical zero = new(0, 0, 0); public static Spherical zero = new(0, 0, 0);
public Spherical(float distance, float horizontal, float vertical) { public Spherical(float distance, float horizontal, float vertical)
{
this.distance = distance; this.distance = distance;
this.horizontal = horizontal; this.horizontal = horizontal;
this.vertical = vertical; this.vertical = vertical;

16
Thing.cs
View File

@ -3,12 +3,12 @@ using System.Collections.Generic;
namespace Passer.Control namespace Passer.Control
{ {
public class Thing public class CoreThing
{ {
public Client client; public Client client;
public byte networkId; public byte networkId;
public byte id; public byte id;
public Thing parent; public CoreThing parent;
public byte type; public byte type;
public string name; public string name;
public string modelUrl; public string modelUrl;
@ -18,21 +18,21 @@ namespace Passer.Control
{ {
} }
public Thing(Client client, byte networkId, byte objId, byte objType) public CoreThing(Client client, byte networkId, byte thingId, byte thingType = 0)
{ {
this.client = client; this.client = client;
this.id = objId; this.id = thingId;
this.type = objType; this.type = thingType;
this.networkId = networkId; this.networkId = networkId;
this.Init(); this.Init();
allThings.Add(this); allThings.Add(this);
} }
public static List<Thing> allThings = new(); private static readonly List<CoreThing> allThings = new();
public static Thing Get(byte networkId, byte thingId) public static CoreThing Get(byte networkId, byte thingId)
{ {
Thing thing = allThings.Find(aThing => aThing.networkId == networkId && aThing.id == thingId); CoreThing thing = allThings.Find(aThing => aThing.networkId == networkId && aThing.id == thingId);
return thing; return thing;
} }