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

Make the local things work (somewhat)

Browse Source
This commit is contained in:
Pascal Serrarens 2024-12-30 15:35:06 +01:00
parent e532f31236
commit 1429f0a9d6
15 changed files with 548 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
LinearAlgebra.meta Normal file
View File

@ -0,0 +1,8 @@
fileFormatVersion: 2
guid: 028f10636fdaa594a9a9969924e5ff18
folderAsset: yes
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

32
LinearAlgebra/Direction.cs Normal file
View File

@ -0,0 +1,32 @@
namespace Passer.LinearAlgebra {
public class Direction {
public float horizontal;
public float vertical;
public Direction() {
horizontal = 0;
vertical = 0;
}
public Direction(float horizontal, float vertical) {
this.horizontal = horizontal;
this.vertical = vertical;
//Normalize();
}
public readonly static Direction forward = new(0, 0);
public readonly static Direction backward = new(-180, 0);
public readonly static Direction up = new(0, 90);
public readonly static Direction down = new(0, -90);
public readonly static Direction left = new(-90, 0);
public readonly static Direction right = new(90, 0);
public void Normalize() {
if (this.vertical > 90 || this.vertical < -90) {
this.horizontal += 180;
this.vertical = 180 - this.vertical;
}
}
}
}

11
LinearAlgebra/Direction.cs.meta Normal file
View File

@ -0,0 +1,11 @@
fileFormatVersion: 2
guid: 9975e4702de32624e8d3deaf84669f2f
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

0
Quat32.cs → LinearAlgebra/Quat32.cs
View File

0
Quat32.cs.meta → LinearAlgebra/Quat32.cs.meta
View File

10
Spherical.cs → LinearAlgebra/Spherical.cs
View File

@ -3,16 +3,18 @@ namespace Passer.LinearAlgebra
public class Spherical
{
public float distance;
public float horizontal;
public float vertical;
public Direction direction;
public static Spherical zero = new(0, 0, 0);
public Spherical(float distance, float horizontal, float vertical)
{
this.distance = distance;
this.horizontal = horizontal;
this.vertical = vertical;
this.direction = new Direction(horizontal, vertical);
}
public Spherical(float distance, Direction direction) {
this.distance = distance;
this.direction = direction;
}
}
}

0
Spherical.cs.meta → LinearAlgebra/Spherical.cs.meta
View File

20
LinearAlgebra/SwingTwist.cs Normal file
View File

@ -0,0 +1,20 @@
namespace Passer.LinearAlgebra {
public class SwingTwist {
public Direction swing;
public float twist;
public static readonly SwingTwist zero = new(0, 0, 0);
public SwingTwist(Direction swing, float twist) {
this.swing = swing;
this.twist = twist;
}
public SwingTwist(float horizontalSwing, float verticalSwing, float twist) {
this.swing = new Direction(horizontalSwing, verticalSwing);
this.swing.Normalize();
this.twist = twist;
}
}
}

11
LinearAlgebra/SwingTwist.cs.meta Normal file
View File

@ -0,0 +1,11 @@
fileFormatVersion: 2
guid: 003a6ec763b101642b0589486f087aef
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

47
LowLevelMessages.cs
View File

@ -1,17 +1,14 @@
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) {
SendFloat16(buffer, ref ix, new float16(v.distance));
SendAngle8(buffer, ref ix, v.horizontal);
SendAngle8(buffer, ref ix, v.vertical);
SendAngle8(buffer, ref ix, v.direction.horizontal);
SendAngle8(buffer, ref ix, v.direction.horizontal);
}
public static Spherical ReceiveSpherical(byte[] data, ref byte ix)
{
public static Spherical ReceiveSpherical(byte[] data, ref byte ix) {
float distance = ReceiveFloat16(data, ref ix);
float horizontal = ReceiveAngle8(data, ref ix);
float vertical = ReceiveAngle8(data, ref ix);
@ -19,14 +16,18 @@ public class LowLevelMessages
return v;
}
public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q)
{
public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s) {
UnityEngine.Quaternion q = UnityEngine.Quaternion.Euler(-s.swing.vertical, s.swing.horizontal, s.twist);
Quat32 q32 = new(q.x, q.y, q.z, q.w);
SendQuat32(buffer, ref ix, q32);
}
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)
{
if (q.w < 0) {
qx = -qx;
qy = -qy;
qz = -qz;
@ -38,8 +39,7 @@ public class LowLevelMessages
buffer[ix++] = (byte)qz;
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(
(data[ix++] - 128.0F) / 127.0F,
(data[ix++] - 128.0F) / 127.0F,
@ -48,8 +48,14 @@ public class LowLevelMessages
return q;
}
public static void SendAngle8(byte[] buffer, ref byte ix, float angle)
{
public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix) {
Quat32 q32 = ReceiveQuat32(data, ref ix);
UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w);
SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z);
return r;
}
public static void SendAngle8(byte[] buffer, ref byte ix, float angle) {
// Normalize angle
while (angle >= 180)
angle -= 360;
@ -58,8 +64,7 @@ public class LowLevelMessages
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;
return value;
}
@ -70,15 +75,13 @@ public class LowLevelMessages
data[ix++] = (byte)(binary >> 8);
data[ix++] = (byte)(binary & 255);
}
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();
data[ix++] = (byte)(binary >> 8);
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++]);
float16 f16 = new();
f16.SetBinary(value);

164
Messages.cs
View File

@ -2,7 +2,7 @@ using System.IO;
using System.Threading.Tasks;
using Passer.LinearAlgebra;
namespace Passer.Control {
namespace Passer.Control.Core {
public class IMessage {
public IMessage() { }
@ -15,17 +15,17 @@ namespace Passer.Control {
public bool SendTo(Participant client) {
Serialize(ref client.buffer);
return client.SendBuffer();
return client.SendBuffer(client.buffer.Length);
}
public static bool SendMsg(Participant client, IMessage msg) {
msg.Serialize(ref client.buffer);
return client.SendBuffer();
return client.SendBuffer(client.buffer.Length);
}
public static bool PublishMsg(Participant client, IMessage msg) {
msg.Serialize(ref client.buffer);
return client.PublishBuffer();
return client.PublishBuffer(client.buffer.Length);
}
public static async Task<byte[]> Receive(Stream dataStream, byte packetSize) {
@ -68,10 +68,10 @@ namespace Passer.Control {
ClientMsg msg = new(networkId);
return SendMsg(client, msg);
}
public static bool Publish(Participant client, byte networkId) {
ClientMsg msg = new(networkId);
return PublishMsg(client, msg);
}
//public static bool Publish(Participant client, byte networkId) {
// ClientMsg msg = new(networkId);
// return PublishMsg(client, msg);
//}
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
if (packetSize != length)
return false;
@ -80,17 +80,38 @@ namespace Passer.Control {
ClientMsg msg = new(buffer);
if (client.networkId == 0) {
client.networkId = (byte)(Participant.clients.Count);
NetworkIdMsg.Send(client, client.networkId);
// client.networkId = (byte)(Participant.clients.Count);
// NetworkIdMsg.Send(client, client.networkId);
client.messageQueue.Enqueue(msg);
}
else if (msg.networkId == 0) {
NetworkIdMsg.Send(client, client.networkId);
// NetworkIdMsg.Send(client, client.networkId);
client.messageQueue.Enqueue(msg);
}
return true;
}
public static bool SendTo(Participant participant, byte networkId) {
if (participant == null)
return false;
byte ix = 0;
participant.buffer[ix++] = ClientMsg.Id;
participant.buffer[ix++] = networkId;
return participant.SendBuffer(ix);
}
public static bool Publish(Participant participant, byte networkId) {
if (participant == null)
return false;
byte ix = 0;
participant.buffer[ix++] = ClientMsg.Id;
participant.buffer[ix++] = networkId;
return participant.PublishBuffer(ix);
}
}
#endregion Client
@ -131,6 +152,18 @@ namespace Passer.Control {
client.messageQueue.Enqueue(msg);
return true;
}
public static bool SendTo(Participant participant, byte networkId) {
if (participant == null)
return false;
byte ix = 0;
participant.buffer[ix++] = NetworkIdMsg.Id;
participant.buffer[ix++] = networkId;
return participant.SendBuffer(ix);
}
}
#endregion Network Id
@ -186,7 +219,7 @@ namespace Passer.Control {
public class ThingMsg : IMessage {
public const byte length = 5;
public const byte Id = 0x80;
public const byte id = 0x80;
public byte networkId;
public byte thingId;
public byte thingType;
@ -202,7 +235,7 @@ namespace Passer.Control {
public override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = ThingMsg.Id;
buffer[ix++] = ThingMsg.id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
buffer[ix++] = this.thingType;
@ -217,7 +250,7 @@ namespace Passer.Control {
this.parentId = buffer[ix];
}
public static bool Send(Participant client, CoreThing thing) {
public static bool Send(Participant client, Thing thing) {
ThingMsg msg = new(thing.networkId, thing.id, thing.type, thing.parent.id);
return SendMsg(client, msg);
}
@ -242,6 +275,23 @@ namespace Passer.Control {
client.messageQueue.Enqueue(msg);
return true;
}
public static bool SendTo(Participant participant, Thing thing) {
if (participant == null || thing == null)
return false;
byte ix = 0;
participant.buffer[ix++] = ThingMsg.id;
participant.buffer[ix++] = participant.networkId;
participant.buffer[ix++] = thing.id;
participant.buffer[ix++] = thing.type;
if (thing.parent != null)
participant.buffer[ix++] = thing.parent.id;
else
participant.buffer[ix++] = 0;
return participant.SendBuffer(ix);
}
}
#endregion Thing
@ -281,7 +331,7 @@ namespace Passer.Control {
this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
}
public static bool Send(Participant client, CoreThing thing) {
public static bool Send(Participant client, Thing thing) {
if (string.IsNullOrEmpty(thing.name))
return true; // nothing sent, but still a success!
@ -300,6 +350,25 @@ namespace Passer.Control {
client.messageQueue.Enqueue(msg);
return true;
}
public static bool SendTo(Participant participant, Thing thing) {
if (participant == null || thing == null || thing.name == null)
return false;
byte nameLength = (byte)thing.name.Length;
if (participant.buffer.Length < 3 + nameLength)
return false;
byte ix = 0;
participant.buffer[ix++] = NameMsg.Id;
participant.buffer[ix++] = participant.networkId;
participant.buffer[ix++] = thing.id;
participant.buffer[ix++] = nameLength;
for (int nameIx = 0; nameIx < nameLength; nameIx++)
participant.buffer[ix++] = (byte)thing.name[nameIx];
return participant.SendBuffer(ix);
}
}
#endregion
@ -344,7 +413,7 @@ namespace Passer.Control {
url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
}
public static bool Send(Participant client, CoreThing thing) {
public static bool Send(Participant client, Thing thing) {
if (string.IsNullOrEmpty(thing.modelUrl))
return true; // nothing sent, but still a success!
@ -364,6 +433,27 @@ namespace Passer.Control {
client.messageQueue.Enqueue(msg);
return true;
}
public static bool SendTo(Participant participant, Thing thing) {
if (participant == null || thing == null || thing.modelUrl == null)
return false;
byte urlLength = (byte)thing.modelUrl.Length;
if (participant.buffer.Length < 3 + urlLength)
return false;
byte ix = 0;
participant.buffer[ix++] = ModelUrlMsg.Id;
participant.buffer[ix++] = participant.networkId;
participant.buffer[ix++] = thing.id; // Thing Id
LowLevelMessages.SendFloat16(participant.buffer, ref ix, new float16(1.0f));
participant.buffer[ix++] = urlLength;
for (int urlIx = 0; urlIx < urlLength; urlIx++)
participant.buffer[ix++] = (byte)thing.modelUrl[urlIx];
return participant.SendBuffer(ix);
}
}
#endregion Model URL
@ -383,11 +473,11 @@ namespace Passer.Control {
public const byte Pose_AngularVelocity = 0x08;
public Spherical position;
public Quat32 orientation;
public SwingTwist orientation;
public Spherical linearVelocity;
public Spherical angularVelocity;
public PoseMsg(byte networkId, byte thingId, Spherical position, Quat32 orientation) {
public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation) {
this.networkId = networkId;
this.thingId = thingId;
this.position = position;
@ -400,7 +490,7 @@ namespace Passer.Control {
if (this.orientation != null)
this.poseType |= Pose_Orientation;
else
this.orientation = new Quat32(0, 0, 0, 1);
this.orientation = SwingTwist.zero;
}
public PoseMsg(byte[] buffer) : base(buffer) { }
@ -411,8 +501,14 @@ namespace Passer.Control {
buffer[ix++] = this.thingId;
buffer[ix++] = this.poseType;
LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation);
if ((poseType & Pose_Position) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.position);
if ((poseType & Pose_Orientation) != 0)
LowLevelMessages.SendQuat32(buffer, ref ix, this.orientation);
if ((poseType & Pose_LinearVelocity) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.linearVelocity);
if ((poseType & Pose_AngularVelocity) != 0)
LowLevelMessages.SendSpherical(buffer, ref ix, this.angularVelocity);
return ix;
}
public override void Deserialize(byte[] buffer) {
@ -424,14 +520,14 @@ namespace Passer.Control {
if ((poseType & Pose_Position) != 0)
this.position = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
if ((poseType & Pose_Orientation) != 0)
this.orientation = LowLevelMessages.ReceiveQuat32(buffer, ref ix);
this.orientation = LowLevelMessages.ReceiveSwingTwist(buffer, ref ix);
if ((poseType & Pose_LinearVelocity) != 0)
this.linearVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
if ((poseType & Pose_AngularVelocity) != 0)
this.angularVelocity = LowLevelMessages.ReceiveSpherical(buffer, ref ix);
}
public static bool Send(Participant client, byte thingId, Spherical position, Quat32 orientation) {
public static bool Send(Participant client, byte thingId, Spherical position, SwingTwist orientation) {
PoseMsg msg = new(client.networkId, thingId, position, orientation);
return SendMsg(client, msg);
}
@ -446,6 +542,28 @@ namespace Passer.Control {
client.messageQueue.Enqueue(msg);
return true;
}
public static bool SendTo(Participant participant, Thing thing) {
if (participant == null || thing == null)
return false;
byte ix = 0;
participant.buffer[ix++] = PoseMsg.Id;
participant.buffer[ix++] = participant.networkId;
participant.buffer[ix++] = thing.id;
participant.buffer[ix++] = thing.poseUpdated;
if ((thing.poseUpdated & Pose_Position) != 0 && thing.position != null)
LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.position);
if ((thing.poseUpdated & Pose_Orientation) != 0 && thing.orientation != null)
LowLevelMessages.SendQuat32(participant.buffer, ref ix, thing.orientation);
if ((thing.poseUpdated & Pose_LinearVelocity) != 0 && thing.linearVelocity != null)
LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.linearVelocity);
if ((thing.poseUpdated & Pose_AngularVelocity) != 0 && thing.angularVelocity != null)
LowLevelMessages.SendSpherical(participant.buffer, ref ix, thing.angularVelocity);
return participant.SendBuffer(ix);
}
}
#endregion Pose

139
Participant.cs
View File

@ -1,14 +1,17 @@
using System.Collections.Generic;
using System.Collections.Concurrent;
using System.Net;
using System.Net.Sockets;
using System.IO;
using System.Threading.Tasks;
namespace Passer.Control
namespace Passer.Control.Core
{
public class Participant
{
//public ConnectionMethod connection;
public IPEndPoint endPoint;
public UdpClient udpClient;
public string ipAddress;
public string broadcastIpAddress = "255.255.255.255";
@ -21,45 +24,147 @@ namespace Passer.Control
public readonly ConcurrentQueue<IMessage> messageQueue = new();
public static Participant GetClient(string ipAddress, int port)
public Participant GetClient(string ipAddress, int port)
{
foreach (Participant c in clients)
foreach (Participant c in others)
{
if (c.ipAddress == ipAddress && c.port == port)
return c;
}
return null;
}
static public List<Participant> clients = new List<Participant>();
public List<Participant> others = new List<Participant>();
public Participant(UdpClient udpClient, int port)
#region Init
public Participant() {
this.dataStream = new EchoStream();
others.Add(this);
}
public Participant(UdpClient udpClient, int port) : this()
{
this.udpClient = udpClient;
this.ipAddress = null;
this.port = port;
this.dataStream = new EchoStream();
clients.Add(this);
//this.dataStream = new EchoStream();
//clients.Add(this);
}
public bool SendBuffer()
{
if (this.ipAddress == null)
return false;
#endregion Init
//UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}");
this.udpClient.Send(this.buffer, this.buffer.Length, this.ipAddress, this.port);
#region Update
private static readonly float publishInterval = 3.0f;
private float nextPublishMe = 0;
public virtual void Update(float currentTime) {
if (currentTime > this.nextPublishMe) {
ClientMsg.Publish(this, this.networkId);
this.nextPublishMe = currentTime + Participant.publishInterval;
}
for (int ix = 0; ix < this.others.Count; ix++) {
Participant client = this.others[ix];
if (client == null)
continue;
client.ProcessMessages();
}
Thing.UpdateAll(currentTime);
}
#endregion Update
#region Send
public bool SendBuffer(int bufferSize)
{
//if (this.ipAddress == null)
// return false;
UnityEngine.Debug.Log($"Send msg {buffer[0]} to {ipAddress}");
//this.udpClient.Send(this.buffer, bufferSize, this.ipAddress, this.port);
this.udpClient.Send(this.buffer, bufferSize, this.endPoint);
return true;
}
public bool PublishBuffer()
public bool PublishBuffer(int bufferSize)
{
if (this.broadcastIpAddress == null)
return false;
this.udpClient.Send(this.buffer, this.buffer.Length, this.broadcastIpAddress, this.port);
this.udpClient.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port);
return true;
}
#endregion
#region Receive
public async Task ReceiveData() {
while (true) {
byte packetSize = (byte)this.dataStream.ReadByte();
if (packetSize != 0xFF)
await ReceiveData(this.dataStream, this, packetSize);
// else timeout
}
}
public static async Task ReceiveData(Stream dataStream, Participant 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);
}
}
#endregion
#region Process
public virtual void ProcessMessages()
{
while (this.messageQueue.TryDequeue(out IMessage msg))
@ -129,7 +234,7 @@ namespace Passer.Control
private void ForwardMessage(IMessage msg)
{
foreach (Participant client in Participant.clients)
foreach (Participant client in others)
{
if (client == this)
continue;
@ -137,5 +242,7 @@ namespace Passer.Control
IMessage.SendMsg(client, msg);
}
}
#endregion
}
}

4
SiteServer.cs
View File

@ -1,7 +1,7 @@
using System.IO;
using System.Threading.Tasks;
namespace Passer.Control {
namespace Passer.Control.Core {
public static class SiteServer {
@ -33,7 +33,7 @@ namespace Passer.Control {
case InvestigateMsg.Id: // 0x81
result = await InvestigateMsg.Receive(dataStream, client, packetSize);
break;
case ThingMsg.Id: // 0x80 / 128
case ThingMsg.id: // 0x80 / 128
result = await ThingMsg.Receive(dataStream, client, packetSize);
break;
case NameMsg.Id: // 0x91 / 145

187
Thing.cs
View File

@ -1,40 +1,189 @@
using System.Collections.Generic;
using Passer.LinearAlgebra;
namespace Passer.Control
{
public class CoreThing
{
namespace Passer.Control.Core {
public class Thing {
public Participant participant;
public delegate void ChangeHandler();
public delegate void SphericalHandler(Spherical v);
public byte networkId;
public byte id;
public CoreThing parent;
public List<CoreThing> children;
public byte type;
public string name;
public string modelUrl;
//protected Sensor sensor;
protected virtual void Init()
{
public event ChangeHandler OnParentChanged;
private Thing _parent;
public Thing parent {
get => _parent;
set {
if (_parent == value)
return;
if (value == null) {
_parent.RemoveChild(this);
_parent = null;
} else {
value.AddChild(this);
OnParentChanged?.Invoke();
}
}
}
public CoreThing(Participant client, byte networkId, byte thingId, byte thingType = 0)
{
public void AddChild(Thing child) {
if (children.Find(thing => thing == child) != null)
return;
child._parent = this;
children.Add(child);
}
public void RemoveChild(Thing child) {
children.Remove(child);
}
[System.NonSerialized]
public List<Thing> children = new List<Thing>();
public byte type;
public event ChangeHandler OnNameChanged;
private string _name;
public string name {
get => _name;
set {
if (_name != value) {
_name = value;
OnNameChanged?.Invoke();
}
}
}
public string modelUrl;
public byte poseUpdated = 0x00;
public event ChangeHandler OnPositionChanged;
private Spherical _position;
public Spherical position {
get { return _position; }
set {
if (_position != value) {
_position = value;
OnPositionChanged?.Invoke();
}
}
}
public event ChangeHandler OnOrientationChanged;
private SwingTwist _orientation;
public SwingTwist orientation {
get { return _orientation; }
set {
if (_orientation != value) {
_orientation = value;
OnOrientationChanged?.Invoke();
}
}
}
public event SphericalHandler OnLinearVelocityChanged;
private Spherical _linearVelocity;
public Spherical linearVelocity {
get => _linearVelocity;
set {
if (_linearVelocity != value) {
_linearVelocity = value;
OnLinearVelocityChanged?.Invoke(_linearVelocity);
}
}
}
public Spherical angularVelocity;
public virtual void Init(bool invokeEvent = true) {
Thing.Add(this, invokeEvent);
}
public Thing(bool initialize = true) {
if (initialize) {
//this.Init();
Thing.Add(this);
}
}
public Thing(Participant client, byte networkId, byte thingId, byte thingType = 0) {
this.participant = client;
this.id = thingId;
this.type = thingType;
this.networkId = networkId;
this.Init();
allThings.Add(this);
Thing.Add(this);
}
private static readonly List<CoreThing> allThings = new();
public virtual void Update(float currentTime) {
// should recurse over children...
}
public static CoreThing Get(byte networkId, byte thingId)
{
CoreThing thing = allThings.Find(aThing => aThing.networkId == networkId && aThing.id == thingId);
public virtual void ProcessBytes(byte[] bytes) {
//if (sensor != null)
// sensor.ProcessBytes(bytes);
}
// Experimental
public float stressLevel = 0;
protected delegate void ReceptorFunc(Sensor sensor);
protected void SetupReceptor(Sensor sensor, ReceptorFunc receptor) {
sensor.Signaller += (sensor => Receptor(receptor, sensor));
}
protected void Receptor(ReceptorFunc receptor, Sensor sensor) {
if (sensor.signalStrength <= stressLevel)
return;
receptor(sensor);
}
//---------- All Things
private static readonly List<Thing> allThings = new();
public delegate void ThingHandler(Thing t);
public static event ThingHandler OnNewThing;
public static void Add(Thing thing, bool invokeEvent = true) {
Thing foundThing = Get(thing.networkId, thing.id);
if (foundThing == null) {
if (thing.id == 0)
thing.id = (byte)(allThings.Count + 1);
allThings.Add(thing);
if (invokeEvent)
OnNewThing?.Invoke(thing);
UnityEngine.Debug.Log($"Add thing [{thing.networkId}/{thing.id}] {thing.name}");
}
}
public static Thing Get(byte networkId, byte thingId) {
Thing thing = allThings.Find(aThing => IsThing(aThing, networkId, thingId));
return thing;
}
private static bool IsThing(Thing thing, byte networkId, byte thingId) {
if (thing == null)
return false;
return (thing.networkId == networkId ) && (thing.id == thingId);
}
public static void Remove(byte networkId, byte thingId) {
allThings.RemoveAll(t => t.networkId == networkId && t.id == thingId);
}
public static Thing[] GetAllThings() {
return allThings.ToArray();
}
public static void UpdateAll(float currentTime) {
foreach (Thing thing in allThings) {
if (thing.parent == null) // update only root things
thing.Update(currentTime);
}
}
}
}

2
Thing.cs.meta
View File

@ -1,5 +1,5 @@
fileFormatVersion: 2
guid: 0e8b48bc91446304eaaccbfdde4cc4af
guid: 7429282ee0e367445bd1e2111631b27d
MonoImporter:
externalObjects: {}
serializedVersion: 2