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

Merge commit '7e80a360274abb6d8f8299cdd2f6ada26bb5ce3c' into pascalV3

Browse Source
This commit is contained in:
Pascal Serrarens 2024-12-12 15:46:57 +01:00
commit 87fbddb2b5
8 changed files with 934 additions and 412 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
Client.cs Normal file
View File

@ -0,0 +1,109 @@
using System.Collections.Generic;
using System.Collections.Concurrent;
using System.Net.Sockets;
using System.IO;
namespace Passer.Control {
public class Client {
//public ConnectionMethod connection;
public UdpClient udpClient;
public string ipAddress;
public int port;
public Stream dataStream;
public byte networkId = 0;
public readonly ConcurrentQueue<IMessage> messageQueue = new();
public static Client GetClient(string ipAddress, int port) {
foreach (Client c in clients) {
if (c.ipAddress == ipAddress && c.port == port)
return c;
}
return null;
}
static public List<Client> clients = new List<Client>();
public Client(UdpClient udpClient, int port) {
this.udpClient = udpClient;
this.ipAddress = null;
this.port = port;
this.dataStream = new EchoStream();
clients.Add(this);
}
public virtual void ProcessMessages() {
while (this.messageQueue.TryDequeue(out IMessage msg))
ProcessMessage(msg);
}
public void ProcessMessage(IMessage msg) {
switch (msg) {
case ClientMsg clientMsg:
ProcessClient(clientMsg);
break;
case NetworkIdMsg networkId:
ProcessNetworkId(networkId);
break;
case InvestigateMsg investigate:
ProcessInvestigate(investigate);
break;
case ThingMsg thing:
ProcessThing(thing);
break;
case NameMsg name:
//UnityEngine.Debug.Log($"Name [{name.networkId}/{name.thingId}] {name.name}");
ProcessName(name);
break;
case ModelUrlMsg modelUrl:
ProcessModelUrl(modelUrl);
break;
case PoseMsg pose:
ProcessPose(pose);
break;
case CustomMsg custom:
ProcessCustom(custom);
break;
case TextMsg text:
ProcessText(text);
break;
case DestroyMsg destroy:
ProcessDestroy(destroy);
break;
default:
return;
}
ForwardMessage(msg);
}
protected virtual void ProcessClient(ClientMsg client) { }
protected virtual void ProcessNetworkId(NetworkIdMsg networkId) { }
protected virtual void ProcessInvestigate(InvestigateMsg investigate) { }
protected virtual void ProcessThing(ThingMsg thing) { }
protected virtual void ProcessName(NameMsg name) { }
protected virtual void ProcessModelUrl(ModelUrlMsg modelUrl) { }
protected virtual void ProcessPose(PoseMsg pose) { }
protected virtual void ProcessCustom(CustomMsg custom) { }
protected virtual void ProcessText(TextMsg text) { }
protected virtual void ProcessDestroy(DestroyMsg destroy) { }
private void ForwardMessage(IMessage msg) {
foreach (Client client in Client.clients) {
if (client == this)
continue;
//UnityEngine.Debug.Log($"---> {client.ipAddress}");
IMessage.SendMsg(client, msg);
}
}
}
}

2
Client.cs.meta Normal file
View File

@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: db9cd79cff119a9438110ead000031c3

173
EchoStream.cs Normal file
View File

@ -0,0 +1,173 @@
using System;
using System.IO;
using System.Threading.Tasks;
using System.Threading;
using System.Collections.Concurrent;
public class EchoStream : Stream {
public override bool CanTimeout { get; } = true;
public override int ReadTimeout { get; set; } = Timeout.Infinite;
public override int WriteTimeout { get; set; } = Timeout.Infinite;
public override bool CanRead { get; } = true;
public override bool CanSeek { get; } = false;
public override bool CanWrite { get; } = true;
public bool CopyBufferOnWrite { get; set; } = false;
private readonly object _lock = new object();
// Default underlying mechanism for BlockingCollection is ConcurrentQueue<T>, which is what we want
private readonly BlockingCollection<byte[]> _Buffers;
private int _maxQueueDepth = 10;
private byte[] m_buffer = null;
private int m_offset = 0;
private int m_count = 0;
private bool m_Closed = false;
private bool m_FinalZero = false; //after the stream is closed, set to true after returning a 0 for read()
public override void Close() {
m_Closed = true;
// release any waiting writes
_Buffers.CompleteAdding();
}
public bool DataAvailable {
get {
return _Buffers.Count > 0;
}
}
private long _Length = 0L;
public override long Length {
get {
return _Length;
}
}
private long _Position = 0L;
public override long Position {
get {
return _Position;
}
set {
throw new NotImplementedException();
}
}
public EchoStream() : this(10) {
}
public EchoStream(int maxQueueDepth) {
_maxQueueDepth = maxQueueDepth;
_Buffers = new BlockingCollection<byte[]>(_maxQueueDepth);
}
// we override the xxxxAsync functions because the default base class shares state between ReadAsync and WriteAsync, which causes a hang if both are called at once
public new Task WriteAsync(byte[] buffer, int offset, int count) {
return Task.Run(() => Write(buffer, offset, count));
}
// we override the xxxxAsync functions because the default base class shares state between ReadAsync and WriteAsync, which causes a hang if both are called at once
public new Task<int> ReadAsync(byte[] buffer, int offset, int count) {
return Task.Run(() => {
return Read(buffer, offset, count);
});
}
public override void Write(byte[] buffer, int offset, int count) {
if (m_Closed || buffer.Length - offset < count || count <= 0)
return;
byte[] newBuffer;
if (!CopyBufferOnWrite && offset == 0 && count == buffer.Length)
newBuffer = buffer;
else {
newBuffer = new byte[count];
System.Buffer.BlockCopy(buffer, offset, newBuffer, 0, count);
}
if (!_Buffers.TryAdd(newBuffer, WriteTimeout))
throw new TimeoutException("EchoStream Write() Timeout");
_Length += count;
}
public override int Read(byte[] buffer, int offset, int count) {
if (count == 0)
return 0;
lock (_lock) {
if (m_count == 0 && _Buffers.Count == 0) {
if (m_Closed) {
if (!m_FinalZero) {
m_FinalZero = true;
return 0;
}
else {
return -1;
}
}
if (_Buffers.TryTake(out m_buffer, ReadTimeout)) {
m_offset = 0;
m_count = m_buffer.Length;
}
else {
if (m_Closed) {
if (!m_FinalZero) {
m_FinalZero = true;
return 0;
}
else {
return -1;
}
}
else {
return 0;
}
}
}
int returnBytes = 0;
while (count > 0) {
if (m_count == 0) {
if (_Buffers.TryTake(out m_buffer, 0)) {
m_offset = 0;
m_count = m_buffer.Length;
}
else
break;
}
var bytesToCopy = (count < m_count) ? count : m_count;
System.Buffer.BlockCopy(m_buffer, m_offset, buffer, offset, bytesToCopy);
m_offset += bytesToCopy;
m_count -= bytesToCopy;
offset += bytesToCopy;
count -= bytesToCopy;
returnBytes += bytesToCopy;
}
_Position += returnBytes;
return returnBytes;
}
}
public override int ReadByte() {
byte[] returnValue = new byte[1];
return (Read(returnValue, 0, 1) <= 0 ? -1 : (int)returnValue[0]);
}
public override void Flush() {
}
public override long Seek(long offset, SeekOrigin origin) {
throw new NotImplementedException();
}
public override void SetLength(long value) {
throw new NotImplementedException();
}
}

2
EchoStream.cs.meta Normal file
View File

@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: 422516a56cbf14d46aaa0b1bc09115bf

49
LowLevelMessages.cs
View File

@ -1,7 +1,14 @@
using Passer; using Passer;
public class LowLevelMessages { public class LowLevelMessages {
public static Spherical ReceiveSpherical(byte[] data, ref uint ix) {
public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v) {
SendAngle8(buffer, ref ix, v.horizontal);
SendAngle8(buffer, ref ix, v.vertical);
SendFloat16(buffer, ref ix, new float16(v.distance));
}
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);
@ -9,7 +16,24 @@ public class LowLevelMessages {
return v; return v;
} }
public static Quat32 ReceiveQuat32(byte[] data, ref uint ix) { public static void SendQuat32(byte[] buffer, ref byte 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 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,
@ -18,18 +42,31 @@ public class LowLevelMessages {
return q; return q;
} }
public static float ReceiveAngle8(byte[] data, ref uint ix) { public static void SendAngle8(byte[] buffer, ref byte 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 byte ix) {
float value = (data[ix++] * 180) / 128.0F; float value = (data[ix++] * 180) / 128.0F;
return value; return value;
} }
public static float ReceiveFloat16(byte[] data, ref uint ix) { public static void SendFloat16(byte[] data, ref byte ix, float16 f) {
ushort binary = f.GetBinary();
data[ix++] = (byte)(binary >> 8);
data[ix++] = (byte)(binary & 255);
}
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);
float f = f16.toFloat(); float f = f16.toFloat();
return f; return f;
} }
} }

943
Messages.cs
View File

@ -1,410 +1,541 @@
using System.Collections.Generic;
using System.Collections.Concurrent;
using System.IO; using System.IO;
using System.Net.Sockets;
using System.Threading.Tasks; using System.Threading.Tasks;
namespace Passer.Control namespace Passer.Control {
{
public partial class Client public class IMessage {
{ public IMessage() { }
//public ConnectionMethod connection; public IMessage(byte[] buffer) {
public UdpClient udpClient; Deserialize(buffer);
public string ipAddress; }
public int port;
public virtual byte[] Serialize() { return null; }
public byte networkId; public virtual void Deserialize(byte[] buffer) { }
public readonly ConcurrentQueue<IMessage> messageQueue = new(); public static bool SendMsg(Client client, IMessage msg) {
return SendMsg(client, msg.Serialize());
public static Client GetClient(string ipAddress, int port) }
{ public static bool SendMsg(Client client, byte[] buffer) {
foreach (Client c in clients) if (client == null || client.ipAddress == null)
{ return false;
if (c.ipAddress == ipAddress && c.port == port)
return c; //UnityEngine.Debug.Log($"Send msg {buffer[0]} to {client.ipAddress}");
} client.udpClient.Send(buffer, buffer.Length, client.ipAddress, client.port);
return null; return true;
} }
static public List<Client> clients = new List<Client>();
public static bool PublishMsg(Client client, IMessage msg) {
public static Client NewClient() return PublishMsg(client, msg.Serialize());
{ }
Client client = new(); public static bool PublishMsg(Client client, byte[] buffer) {
clients.Add(client); if (client == null)
client.networkId = 0; return false;
return client; client.udpClient.Send(buffer, buffer.Length, "127.0.0.1", client.port);
} return true;
}
public static Client NewUDPClient(UdpClient udpClient, string ipAddress, int port)
{ public static async Task<byte[]> Receive(Stream dataStream, byte packetSize) {
Client client = NewClient(); byte[] buffer = new byte[packetSize - 1]; // without msgId
client.ipAddress = null; int byteCount = dataStream.Read(buffer, 0, packetSize - 1);
client.port = port; while (byteCount < packetSize - 1) {
client.udpClient = udpClient; // not all bytes have been read, wait and try again
return client; await Task.Delay(1);
} byteCount += dataStream.Read(buffer, byteCount, packetSize - 1 - byteCount);
} }
return buffer;
public class IMessage }
{ }
public IMessage() { }
public IMessage(byte[] data) #region Client
{
Deserialize(data); public class ClientMsg : IMessage {
} public const byte Id = 0xA0;
public const byte length = 2;
public virtual byte[] Serialize() { return null; } public byte networkId;
public virtual void Deserialize(byte[] data) { }
public ClientMsg(byte networkId) {
public static bool SendMsg(Client client, IMessage msg) this.networkId = networkId;
{ }
return SendMsg(client, msg.Serialize()); public ClientMsg(byte[] buffer) : base(buffer) { }
}
public static bool SendMsg(Client client, byte[] data) public override byte[] Serialize() {
{ byte[] buffer = new byte[ClientMsg.length];
if (client == null || client.ipAddress == null) buffer[0] = ClientMsg.Id;
return false; buffer[1] = networkId;
return buffer;
client.udpClient.Send(data, data.Length, client.ipAddress, client.port); }
return true; public override void Deserialize(byte[] buffer) {
} base.Deserialize(buffer);
public static async Task<byte[]> Receive(Stream dataStream, byte packetSize) uint ix = 0;
{ networkId = buffer[ix];
byte[] buffer = new byte[packetSize - 1]; // without msgId }
int byteCount = dataStream.Read(buffer, 0, packetSize - 1);
while (byteCount < packetSize - 1) public static bool Send(Client client, byte networkId) {
{ ClientMsg msg = new(networkId);
// not all bytes have been read, wait and try again return SendMsg(client, msg);
await Task.Delay(1); }
byteCount += dataStream.Read(buffer, byteCount, packetSize - 1 - byteCount); public static bool Publish(Client client, byte networkId) {
} ClientMsg msg = new(networkId);
return buffer; return PublishMsg(client, msg);
} }
} public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
if (packetSize != length)
#region Client return false;
public class ClientMsg : IMessage byte[] buffer = await Receive(dataStream, packetSize);
{ ClientMsg msg = new(buffer);
public const byte Id = 0xA0;
public const byte length = 2; if (client.networkId == 0) {
public byte clientId; client.networkId = (byte)(Client.clients.Count);
NetworkIdMsg.Send(client, client.networkId);
public ClientMsg(byte[] data) : base(data) { } client.messageQueue.Enqueue(msg);
public override void Deserialize(byte[] data) }
{ else if (msg.networkId == 0) {
base.Deserialize(data); NetworkIdMsg.Send(client, client.networkId);
uint ix = 0; client.messageQueue.Enqueue(msg);
clientId = data[ix++]; }
}
return true;
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) }
{ }
if (packetSize != length)
return false; #endregion Client
byte[] buffer = await Receive(dataStream, packetSize); #region Network Id
ClientMsg msg = new(buffer);
public class NetworkIdMsg : IMessage {
if (client.networkId == 0) public const byte Id = 0xA1;
{ public const byte length = 2;
client.networkId = (byte)(Client.clients.Count); public byte networkId;
NetworkIdMsg.Send(client);
//if (string.IsNullOrEmpty(sceneUrl) == false) NetworkIdMsg(byte networkId) {
//SendModelUrl(client, sceneUrl); this.networkId = networkId;
} }
else if (msg.clientId == 0) NetworkIdMsg(byte[] buffer) : base(buffer) { }
{
NetworkIdMsg.Send(client); public override byte[] Serialize() {
//if (string.IsNullOrEmpty(sceneUrl) == false) byte[] buffer = new byte[NetworkIdMsg.length];
//SendModelUrl(client, sceneUrl); buffer[0] = NetworkIdMsg.Id;
} buffer[1] = this.networkId;
return buffer;
return true; }
} public override void Deserialize(byte[] buffer) {
} uint ix = 0;
this.networkId = buffer[ix];
#endregion Client }
#region Network Id public static bool Send(Client client, byte networkId) {
NetworkIdMsg msg = new(networkId);
public class NetworkIdMsg : IMessage return SendMsg(client, msg);
{ }
public const byte Id = 0xA1; public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
public const byte length = 2; if (packetSize != length)
return false;
public static bool Send(Client client)
{ byte[] buffer = await Receive(dataStream, packetSize);
byte[] data = new byte[NetworkIdMsg.length]; NetworkIdMsg msg = new(buffer);
data[0] = NetworkIdMsg.Id; client.messageQueue.Enqueue(msg);
data[1] = client.networkId; return true;
return SendMsg(client, data); }
} }
}
#endregion Network Id
#endregion Network Id
#region Investigate
#region Thing
public class InvestigateMsg : IMessage {
public class ThingMsg : IMessage public const byte Id = 0x81;
{ public const byte length = 3;
public const byte length = 4; public byte networkId;
public const byte Id = 0x80; public byte thingId;
public byte thingId;
public byte thingType; public InvestigateMsg(byte networkId, byte thingId) {
public byte parentId; this.networkId = networkId;
this.thingId = thingId;
public ThingMsg(byte[] data) : base(data) { } }
public override void Deserialize(byte[] data) public InvestigateMsg(byte[] buffer) : base(buffer) { }
{
uint ix = 0; public override byte[] Serialize() {
thingId = data[ix++]; byte[] buffer = new byte[InvestigateMsg.length];
thingType = data[ix++]; buffer[0] = InvestigateMsg.Id;
parentId = data[ix]; buffer[1] = this.networkId;
} buffer[2] = this.thingId;
return buffer;
public static bool Send(Client client, byte networkId, byte thingId, byte thingType) }
{ public override void Deserialize(byte[] buffer) {
byte[] data = new byte[4]; uint ix = 0;
data[0] = ThingMsg.Id; this.networkId = buffer[ix++];
data[1] = networkId; this.thingId = buffer[ix++];
data[2] = thingId; }
data[3] = thingType;
data[4] = 0x00; // parent not supported yet public static bool Send(Client client, byte networkId, byte thingId) {
return SendMsg(client, data); InvestigateMsg msg = new(networkId, thingId);
} //UnityEngine.Debug.Log($"Send investigate [{msg.networkId}/{msg.thingId}]");
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) return SendMsg(client, msg);
{ }
if (packetSize != length) public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
return false; if (packetSize != length)
return false;
byte[] buffer = await Receive(dataStream, packetSize);
ThingMsg msg = new(buffer); byte[] buffer = await Receive(dataStream, packetSize);
InvestigateMsg msg = new(buffer);
client.messageQueue.Enqueue(msg); //UnityEngine.Debug.Log($"Receive investigate [{msg.networkId}/{msg.thingId}]");
return true;
} client.messageQueue.Enqueue(msg);
} return true;
#endregion Thing }
}
#region Name
#endregion Investigate
public class NameMsg : IMessage
{ #region Thing
public byte networkId = 0;
public byte objectId; public class ThingMsg : IMessage {
public byte len; public const byte length = 5;
public string name; public const byte Id = 0x80;
public byte networkId;
public NameMsg(byte[] data) : base(data) { } public byte thingId;
public override void Deserialize(byte[] data) public byte thingType;
{ public byte parentId;
uint ix = 0;
this.objectId = data[ix++]; public ThingMsg(byte networkId, byte thingId, byte thingType, byte parentId) {
int strlen = data[ix++]; this.networkId = networkId;
this.name = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen); this.thingId = thingId;
} this.thingType = thingType;
this.parentId = parentId;
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) }
{ public ThingMsg(byte[] buffer) : base(buffer) { }
byte[] buffer = await Receive(dataStream, packetSize);
NameMsg msg = new(buffer); public override byte[] Serialize() {
byte[] buffer = new byte[ThingMsg.length];
client.messageQueue.Enqueue(msg); byte ix = 0;
return true; buffer[ix++] = ThingMsg.Id;
} buffer[ix++] = this.networkId;
} buffer[ix++] = this.thingId;
buffer[ix++] = this.thingType;
#endregion buffer[ix] = this.parentId;
return buffer;
#region Model URL }
public override void Deserialize(byte[] buffer) {
public class ModelUrlMsg : IMessage uint ix = 0;
{ this.networkId = buffer[ix++];
public const byte Id = 0x90; // (144) Model URL this.thingId = buffer[ix++];
public byte objectId; this.thingType = buffer[ix++];
public Spherical position; this.parentId = buffer[ix];
public float scale; }
public string url;
public static bool Send(Client client, byte networkId, byte thingId, byte thingType, byte parentId) {
public ModelUrlMsg(string url, float scale = 1) ThingMsg msg = new(networkId, thingId, thingType, parentId);
{ //UnityEngine.Debug.Log($"Send thing [{msg.networkId}/{msg.thingId}]");
this.url = url; return SendMsg(client, msg);
this.scale = scale; }
this.position = Spherical.zero; public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
} if (packetSize != length)
public ModelUrlMsg(byte[] data) : base(data) { } return false;
public override byte[] Serialize() byte[] buffer = await Receive(dataStream, packetSize);
{ ThingMsg msg = new(buffer);
byte[] data = new byte[this.url.Length + 9]; //UnityEngine.Debug.Log($"Receive thing [{msg.networkId}/{msg.thingId}]");
data[0] = ModelUrlMsg.Id;
data[1] = 0x00; // Thing Id // Do no process poses with nwid == 0 (== local)
// data[2]..[5] == position 0, 0, 0 //if (msg.networkId == 0)
data[6] = 0x3C; // Dummy float16 value 1 // return true;
data[7] = 0x00;
client.messageQueue.Enqueue(msg);
data[8] = (byte)url.Length; return true;
for (int ix = 0; ix < this.url.Length; ix++) }
data[9 + ix] = (byte)url[ix]; }
return data;
} #endregion Thing
public override void Deserialize(byte[] data)
{ #region Name
uint ix = 0;
this.objectId = data[ix++]; public class NameMsg : IMessage {
this.position = LowLevelMessages.ReceiveSpherical(data, ref ix); public const byte Id = 0x91; // 145
this.scale = LowLevelMessages.ReceiveFloat16(data, ref ix); public const byte length = 4;
int strlen = data[ix++]; public byte networkId;
url = System.Text.Encoding.UTF8.GetString(data, (int)ix, strlen); public byte thingId;
} public byte len;
public string name;
public static bool Send(Client client, string modelUrl)
{ public NameMsg(byte networkId, byte thingId, string name) {
ModelUrlMsg msg = new(modelUrl); this.networkId = networkId;
return SendMsg(client, msg); this.thingId = thingId;
} this.name = name;
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) }
{ public NameMsg(byte[] buffer) : base(buffer) { }
byte[] buffer = await Receive(dataStream, packetSize);
ModelUrlMsg msg = new(buffer); public override byte[] Serialize() {
client.messageQueue.Enqueue(msg); byte[] buffer = new byte[length + this.name.Length];
return true; byte ix = 0;
} buffer[ix++] = NameMsg.Id;
} buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
#endregion Model URL buffer[ix++] = (byte)this.name.Length;
for (int nameIx = 0; nameIx < this.name.Length; nameIx++, ix++)
#region Pose buffer[ix] = (byte)this.name[nameIx];
return buffer;
public class PoseMsg : IMessage }
{ public override void Deserialize(byte[] buffer) {
public const byte Id = 0x10; byte ix = 0;
public const byte length = 3 + 4 + 4; this.networkId = buffer[ix++];
public byte thingId; this.thingId = buffer[ix++];
public byte poseType; int strlen = buffer[ix++];
this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
public Spherical position; }
public Quat32 orientation;
public static bool Send(Client client, byte networkId, byte thingId, string name) {
public PoseMsg(byte[] data) : base(data) { } NameMsg msg = new(networkId, thingId, name);
return SendMsg(client, msg);
public override void Deserialize(byte[] data) }
{ public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
uint ix = 0; byte[] buffer = await Receive(dataStream, packetSize);
thingId = data[ix++]; NameMsg msg = new(buffer);
poseType = data[ix++];
client.messageQueue.Enqueue(msg);
//if ((poseType & Pose_Position) != 0) return true;
position = LowLevelMessages.ReceiveSpherical(data, ref ix); }
//if ((poseType & Pose_Orientation) != 0) { }
orientation = LowLevelMessages.ReceiveQuat32(data, ref ix);
} #endregion
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) #region Model URL
{
if (packetSize != length) public class ModelUrlMsg : IMessage {
return false; public const byte Id = 0x90; // (144) Model URL
public byte networkId;
byte[] buffer = await Receive(dataStream, packetSize); public byte thingId;
PoseMsg msg = new(buffer); public Spherical position;
public float scale;
client.messageQueue.Enqueue(msg); public string url;
return true;
public ModelUrlMsg(byte networkId, byte thingId, string url, float scale = 1) {
} this.networkId = networkId;
} this.thingId = thingId;
this.url = url;
#endregion Pose this.scale = scale;
this.position = Spherical.zero;
#region Bytes }
public ModelUrlMsg(byte[] buffer) : base(buffer) { }
public class BytesMsg : IMessage
{ public override byte[] Serialize() {
public const byte Id = 0xB1; byte[] buffer = new byte[this.url.Length + 6];
public byte networkId; byte ix = 0;
public byte thingId; buffer[ix++] = ModelUrlMsg.Id;
public byte[] bytes; buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId; // Thing Id
public BytesMsg(byte[] data) : base(data) { } LowLevelMessages.SendFloat16(buffer, ref ix, new float16(1.0f));
public BytesMsg(byte networkId, byte thingId, byte[] bytes) : base()
{ buffer[ix++] = (byte)url.Length;
this.networkId = networkId; for (int urlIx = 0; urlIx < this.url.Length; urlIx++, ix++)
this.thingId = thingId; buffer[ix] = (byte)url[urlIx];
this.bytes = bytes; return buffer;
} }
public override byte[] Serialize() public override void Deserialize(byte[] buffer) {
{ byte ix = 0;
byte[] buffer = new byte[4 + this.bytes.Length]; this.networkId = buffer[ix++];
int ix = 0; this.thingId = buffer[ix++];
buffer[ix++] = BytesMsg.Id; this.scale = LowLevelMessages.ReceiveFloat16(buffer, ref ix);
buffer[ix++] = this.networkId; int strlen = buffer[ix++];
buffer[ix++] = this.thingId; url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
buffer[ix++] = (byte)bytes.Length; }
foreach (byte b in bytes)
buffer[ix++] = b; public static bool Send(Client client, byte networkId, byte thingId, string modelUrl) {
ModelUrlMsg msg = new(networkId, thingId, modelUrl);
return buffer; return SendMsg(client, msg);
} }
public override void Deserialize(byte[] data) public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
{ byte[] buffer = await Receive(dataStream, packetSize);
//this.bytes = data; ModelUrlMsg msg = new(buffer);
uint ix = 0; client.messageQueue.Enqueue(msg);
this.thingId = data[ix++]; return true;
this.bytes = new byte[data.Length - ix]; }
for (uint bytesIx = 0; ix < data.Length; ix++, bytesIx++) }
this.bytes[bytesIx] = data[ix];
} #endregion Model URL
#region Pose
public static void Send(Client client, byte thingId, byte[] bytes)
{ public class PoseMsg : IMessage {
BytesMsg msg = new(client.networkId, thingId, bytes); public const byte Id = 0x10;
SendMsg(client, msg); public const byte length = 4 + 4 + 4;
} public byte networkId;
public byte thingId;
// received bytes
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) public byte poseType;
{ public const byte Pose_Position = 0x01;
byte[] buffer = await Receive(dataStream, packetSize); public const byte Pose_Orientation = 0x02;
BytesMsg msg = new(buffer);
client.messageQueue.Enqueue(msg); public Spherical position;
return true; public Quat32 orientation;
}
} public PoseMsg(byte networkId, byte thingId, Spherical position, Quat32 orientation) {
this.networkId = networkId;
#endregion Bytes this.thingId = thingId;
this.position = position;
#region Destroy this.orientation = orientation;
this.poseType = 0;
public class DestroyMsg : IMessage if (this.position != null)
{ this.poseType |= Pose_Position;
public const byte length = 2; else
public byte objectId; this.position = new Spherical(0, 0, 0);
if (this.orientation != null)
public DestroyMsg(byte[] data) : base(data) { } this.poseType |= Pose_Orientation;
else
public override void Deserialize(byte[] data) this.orientation = new Quat32(0, 0, 0, 1);
{ }
objectId = data[0]; public PoseMsg(byte[] buffer) : base(buffer) { }
}
public override byte[] Serialize() {
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) byte[] buffer = new byte[PoseMsg.length];
{ byte ix = 0;
if (packetSize != length) buffer[ix++] = PoseMsg.Id;
return false; buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
byte[] buffer = await Receive(dataStream, packetSize); buffer[ix++] = this.poseType;
DestroyMsg msg = new(buffer);
LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
client.messageQueue.Enqueue(msg); LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation);
return true; return buffer;
} }
} public override void Deserialize(byte[] buffer) {
byte ix = 0;
this.networkId = buffer[ix++];
#endregion Destroy this.thingId = buffer[ix++];
this.poseType = buffer[ix++];
//if ((poseType & Pose_Position) != 0)
this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
//if ((poseType & Pose_Orientation) != 0) {
this.orientation = LowLevelMessages.ReceiveQuat32(buffer, ref ix);
}
public static bool Send(Client client, byte thingId, Spherical position, Quat32 orientation) {
PoseMsg msg = new(client.networkId, thingId, position, orientation);
return SendMsg(client, msg);
}
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
if (packetSize != length)
return false;
byte[] buffer = await Receive(dataStream, packetSize);
PoseMsg msg = new(buffer);
// Do no process poses with nwid == 0 (== local)
if (msg.networkId == 0)
return true;
client.messageQueue.Enqueue(msg);
return true;
}
}
#endregion Pose
#region Custom
public class CustomMsg : IMessage {
public const byte Id = 0xB1;
public byte networkId;
public byte thingId;
public byte[] bytes;
public CustomMsg(byte[] buffer) : base(buffer) { }
public CustomMsg(byte networkId, byte thingId, byte[] bytes) : base() {
this.networkId = networkId;
this.thingId = thingId;
this.bytes = bytes;
}
public override byte[] Serialize() {
byte[] buffer = new byte[3 + this.bytes.Length];
byte ix = 0;
buffer[ix++] = CustomMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
//buffer[ix++] = (byte)bytes.Length;
foreach (byte b in bytes)
buffer[ix++] = b;
return buffer;
}
public override void Deserialize(byte[] buffer) {
byte ix = 0;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
byte length = (byte)(buffer.Length - ix);//buffer[ix++];
this.bytes = new byte[length];
for (uint bytesIx = 0; bytesIx < length; bytesIx++)
this.bytes[bytesIx] = buffer[ix++];
}
public static void Send(Client client, byte thingId, byte[] bytes) {
CustomMsg msg = new(client.networkId, thingId, bytes);
SendMsg(client, msg);
}
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
byte[] buffer = await Receive(dataStream, packetSize);
CustomMsg msg = new(buffer);
client.messageQueue.Enqueue(msg);
return true;
}
}
#endregion Custom
#region Text
public class TextMsg : IMessage {
public const byte Id = 0xB0;
public string text;
public TextMsg(byte[] buffer) : base(buffer) { }
public override void Deserialize(byte[] buffer) {
uint ix = 0;
uint strlen = buffer[ix++];
this.text = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, (int)strlen);
}
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
byte[] buffer = await Receive(dataStream, packetSize);
TextMsg msg = new(buffer);
client.messageQueue.Enqueue(msg);
return true;
}
}
#endregion
#region Destroy
public class DestroyMsg : IMessage {
public const byte Id = 0x20;
public const byte length = 2;
public byte networkId;
public byte thingId;
public DestroyMsg(byte[] buffer) : base(buffer) { }
public override void Deserialize(byte[] buffer) {
this.networkId = buffer[0];
this.thingId = buffer[1];
}
public static async Task<bool> Receive(Stream dataStream, Client client, byte packetSize) {
if (packetSize != length)
return false;
byte[] buffer = await Receive(dataStream, packetSize);
DestroyMsg msg = new(buffer);
client.messageQueue.Enqueue(msg);
return true;
}
}
#endregion Destroy
} }

66
SiteServer.cs Normal file
View File

@ -0,0 +1,66 @@
using System.IO;
using System.Threading.Tasks;
namespace Passer.Control {
public static class SiteServer {
public static async Task ReceiveData(Stream dataStream, Client client) {
while (true) {
byte packetSize = (byte)dataStream.ReadByte();
if (packetSize != 0xFF)
await ReceiveData(dataStream, client, packetSize);
// else timeout
}
}
public static async Task ReceiveData(Stream dataStream, Client client, byte packetSize) {
byte msgId = (byte)dataStream.ReadByte();
if (msgId == 0xFF) {
// Timeout
return;
}
//UnityEngine.Debug.Log($"R {msgId} from {client.ipAddress}");
bool result = false;
switch (msgId) {
case ClientMsg.Id: // 0xA0 / 160
result = await ClientMsg.Receive(dataStream, client, packetSize);
break;
case NetworkIdMsg.Id: // 0xA1 / 161
result = await NetworkIdMsg.Receive(dataStream, client, packetSize);
break;
case InvestigateMsg.Id: // 0x81
result = await InvestigateMsg.Receive(dataStream, client, packetSize);
break;
case ThingMsg.Id: // 0x80 / 128
result = await ThingMsg.Receive(dataStream, client, packetSize);
break;
case NameMsg.Id: // 0x91 / 145
result = await NameMsg.Receive(dataStream, client, packetSize);
break;
case ModelUrlMsg.Id: // 0x90 / 144
result = await ModelUrlMsg.Receive(dataStream, client, packetSize);
break;
case PoseMsg.Id: // 0x10 / 16
result = await PoseMsg.Receive(dataStream, client, packetSize);
break;
case CustomMsg.Id: // 0xB1 / 177
result = await CustomMsg.Receive(dataStream, client, packetSize);
break;
case TextMsg.Id: // 0xB0 / 176
result = await TextMsg.Receive(dataStream, client, packetSize);
break;
case DestroyMsg.Id: // 0x20 / 32
result = await DestroyMsg.Receive(dataStream, client, packetSize);
break;
default:
break;
}
if (result == false) {
packetSize = msgId; // skip 1 byte, msgId is possibly a packet size byte
await ReceiveData(dataStream, client, packetSize);
}
}
}
}

2
SiteServer.cs.meta Normal file
View File

@ -0,0 +1,2 @@
fileFormatVersion: 2
guid: 53345abb9310d344baa67c19a8d8249f