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

Merge commit 'dcccef221b999eb9cabe377b803fa8d92536ef63' into V2

Browse Source
This commit is contained in:
Pascal Serrarens 2025-02-19 12:26:33 +01:00
commit 6d37d8cec9
21 changed files with 95 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ControlCore.csproj
View File

@ -3,17 +3,12 @@
<PropertyGroup>
<GenerateAssemblyInfo>false</GenerateAssemblyInfo>
<GenerateTargetFrameworkAttribute>false</GenerateTargetFrameworkAttribute>
<TargetFramework>net9.0</TargetFramework>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>enable</Nullable>
<TargetFramework>net5.0</TargetFramework>
</PropertyGroup>
<ItemGroup>
<PackageReference Include="coverlet.collector" Version="6.0.2" />
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.12.0" />
<PackageReference Include="NUnit" Version="3.13.3" />
<PackageReference Include="NUnit.Analyzers" Version="4.3.0" />
<PackageReference Include="NUnit3TestAdapter" Version="4.6.0" />
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.13.0" />
<PackageReference Include="NUnit" Version="3.13.2" />
</ItemGroup>
</Project>

12
LinearAlgebra/Direction.cs
View File

@ -14,12 +14,12 @@ namespace Passer.LinearAlgebra {
//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 readonly static Direction forward = new Direction(0, 0);
public readonly static Direction backward = new Direction(-180, 0);
public readonly static Direction up = new Direction(0, 90);
public readonly static Direction down = new Direction(0, -90);
public readonly static Direction left = new Direction(-90, 0);
public readonly static Direction right = new Direction(90, 0);
public void Normalize() {
if (this.vertical > 90 || this.vertical < -90) {

2
LinearAlgebra/Quat32.cs
View File

@ -42,7 +42,7 @@ namespace Passer.LinearAlgebra
float yawOver2 = yaw * 0.5f;
float sinYawOver2 = (float)Math.Sin((float)yawOver2);
float cosYawOver2 = (float)Math.Cos((float)yawOver2);
Quat32 result = new()
Quat32 result = new Quat32()
{
w = cosYawOver2 * cosPitchOver2 * cosRollOver2 +
sinYawOver2 * sinPitchOver2 * sinRollOver2,

4
LinearAlgebra/Spherical.cs
View File

@ -8,8 +8,8 @@ namespace Passer.LinearAlgebra {
public float distance;
public Direction direction;
public static Spherical zero = new(0, 0, 0);
public static Spherical forward = new(1, 0, 0);
public static Spherical zero = new Spherical(0, 0, 0);
public static Spherical forward = new Spherical(1, 0, 0);
public Spherical(float distance, float horizontal, float vertical) {
this.distance = distance;

4
LinearAlgebra/SwingTwist.cs
View File

@ -6,7 +6,7 @@ namespace Passer.LinearAlgebra
public Direction swing;
public float twist;
public static readonly SwingTwist zero = new(0, 0, 0);
public static readonly SwingTwist zero = new SwingTwist(0, 0, 0);
public SwingTwist(Direction swing, float twist)
{
@ -24,7 +24,7 @@ namespace Passer.LinearAlgebra
// UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w);
// SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z);
q32.ToAngles(out float right, out float up, out float forward);
SwingTwist r = new(up, right, forward);
SwingTwist r = new SwingTwist(up, right, forward);
return r;
}
}

8
LinearAlgebra/Vector3.cs
View File

@ -1,3 +1,5 @@
#if !UNITY_5_3_OR_NEWER
using System;
#if !UNITY_5_3_OR_NEWER
namespace Passer.LinearAlgebra {
@ -17,9 +19,11 @@ namespace Passer.LinearAlgebra {
// }
}
public class Vector3Int(int x, int y, int z) : Vector3Of<int>(x, y, z) {
public class Vector3Int : Vector3Of<int> {
public Vector3Int(int x, int y, int z) : base(x, y, z) { }
}
public class Vector3Float(float x, float y, float z) : Vector3Of<float>(x, y, z) {
public class Vector3Float : Vector3Of<float> {
public Vector3Float(float x, float y, float z) : base(x, y, z) { }
public float magnitude {
get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z);

1
Messages/CustomMsg.cs
View File

@ -1,4 +1,3 @@
#nullable enable
namespace Passer.Control.Core {
public class CustomMsg : IMessage {

2
Messages/DestroyMsg.cs
View File

@ -21,7 +21,7 @@ namespace Passer.Control.Core {
return false;
byte[] buffer = await Receive(dataStream, packetSize);
DestroyMsg msg = new(buffer);
DestroyMsg msg = new DestroyMsg(buffer);
client.messageQueue.Enqueue(msg);
return true;

2
Messages/InvestigateMsg.cs
View File

@ -37,7 +37,7 @@ namespace Passer.Control.Core {
return false;
byte[] buffer = await Receive(dataStream, packetSize);
InvestigateMsg msg = new(buffer);
InvestigateMsg msg = new InvestigateMsg(buffer);
//UnityEngine.Debug.Log($"Receive investigate [{msg.networkId}/{msg.thingId}]");
client.messageQueue.Enqueue(msg);

8
Messages/LowLevelMessages.cs
View File

@ -17,7 +17,7 @@ namespace Passer.Control.Core
float distance = ReceiveFloat16(data, ref ix);
float horizontal = ReceiveAngle8(data, ref ix);
float vertical = ReceiveAngle8(data, ref ix);
Spherical v = new(distance, horizontal, vertical);
Spherical v = new Spherical(distance, horizontal, vertical);
return v;
}
@ -48,7 +48,7 @@ namespace Passer.Control.Core
}
public static Quat32 ReceiveQuat32(byte[] data, ref byte ix)
{
Quat32 q = new(
Quat32 q = new Quat32(
(data[ix++] - 128.0F) / 127.0F,
(data[ix++] - 128.0F) / 127.0F,
(data[ix++] - 128.0F) / 127.0F,
@ -84,7 +84,7 @@ namespace Passer.Control.Core
public static void SendFloat16(byte[] data, ref byte ix, float f)
{
float16 f16 = new(f);
float16 f16 = new float16(f);
ushort binary = f16.GetBinary();
data[ix++] = (byte)(binary >> 8);
data[ix++] = (byte)(binary & 255);
@ -99,7 +99,7 @@ namespace Passer.Control.Core
public static float ReceiveFloat16(byte[] data, ref byte ix)
{
ushort value = (ushort)(data[ix++] << 8 | data[ix++]);
float16 f16 = new();
float16 f16 = new float16();
f16.SetBinary(value);
float f = f16.toFloat();
return f;

2
Messages/NameMsg.cs
View File

@ -1,5 +1,3 @@
#nullable enable
namespace Passer.Control.Core {
public class NameMsg : IMessage {

4
Messages/PoseMsg.cs
View File

@ -70,12 +70,12 @@ namespace Passer.Control.Core {
}
public static bool Send(Participant client, byte thingId, Spherical position, SwingTwist orientation) {
PoseMsg msg = new(client.networkId, thingId, position, orientation);
PoseMsg msg = new PoseMsg(client.networkId, thingId, position, orientation);
return SendMsg(client, msg);
}
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
byte[] buffer = await Receive(dataStream, packetSize);
PoseMsg msg = new(buffer);
PoseMsg msg = new PoseMsg(buffer);
// Do no process poses with nwid == 0 (== local)
if (msg.networkId == 0)

2
Messages/TextMsg.cs
View File

@ -16,7 +16,7 @@ namespace Passer.Control.Core {
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
byte[] buffer = await Receive(dataStream, packetSize);
TextMsg msg = new(buffer);
TextMsg msg = new TextMsg(buffer);
client.messageQueue.Enqueue(msg);
return true;

4
ModelUrlMsg.cs
View File

@ -1,5 +1,3 @@
#nullable enable
namespace Passer.Control.Core {
public class ModelUrlMsg : IMessage {
@ -7,7 +5,7 @@ namespace Passer.Control.Core {
public const byte length = 4;
public byte networkId;
public byte thingId;
public string? url = null;
public string url = null;
public ModelUrlMsg(byte networkId, Thing thing) {
this.networkId = networkId;

35
Participant.cs
View File

@ -1,4 +1,3 @@
#nullable enable
using System;
using System.Collections.Generic;
using System.Collections.Concurrent;
@ -14,11 +13,11 @@ namespace Passer.Control.Core {
//public byte networkId = 0;
public string name = "Participant";
public IPEndPoint? endPoint = null;
public UdpClient? udpClient = null;
public IPEndPoint endPoint = null;
public UdpClient udpClient = null;
public string broadcastIpAddress = "255.255.255.255";
public readonly ConcurrentQueue<IMessage> messageQueue = new();
public readonly ConcurrentQueue<IMessage> messageQueue = new ConcurrentQueue<IMessage>();
#region Init
@ -46,10 +45,12 @@ namespace Passer.Control.Core {
this.ipAddress = ipAddress;
this.port = port;
this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending
this.udpClient = new UdpClient();
this.udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port)); // local port
// this.endPoint = new IPEndPoint(IPAddress.Parse(ipAddress), port); // for sending
this.udpClient = new UdpClient(port); // for receiving
this.udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port));
// this.udpClient = new UdpClient(port); // for receiving
// this.udpClient.Client.Bind(new IPEndPoint(IPAddress.Any, port));
this.udpClient.BeginReceive(new AsyncCallback(result => ReceiveUDP(result)), null);
}
@ -63,9 +64,9 @@ namespace Passer.Control.Core {
this.port = port;
}
public List<RemoteParticipant> senders = new();
public List<RemoteParticipant> senders = new List<RemoteParticipant>();
public RemoteParticipant? GetParticipant(string ipAddress, int port) {
public RemoteParticipant GetParticipant(string ipAddress, int port) {
//Console.WriteLine($"Get Participant {ipAddress}:{port}");
foreach (RemoteParticipant sender in senders) {
if (sender.ipAddress == ipAddress && sender.port == port)
@ -75,14 +76,14 @@ namespace Passer.Control.Core {
}
public RemoteParticipant AddParticipant(string ipAddress, int port) {
// Console.WriteLine($"New Participant {ipAddress}:{port}");
RemoteParticipant participant = new(ipAddress, port) {
RemoteParticipant participant = new RemoteParticipant(ipAddress, port) {
networkId = (byte)this.senders.Count
};
senders.Add(participant);
return participant;
}
protected readonly Dictionary<byte, Func<byte, byte, Thing?>> thingMsgProcessors = new();
protected readonly Dictionary<byte, Func<byte, byte, Thing>> thingMsgProcessors = new Dictionary<byte, Func<byte, byte, Thing>>();
public delegate Thing ThingConstructor(byte networkId, byte thingId);
public void Register(byte thingType, ThingConstructor constr) {
@ -94,7 +95,7 @@ namespace Passer.Control.Core {
}
public void Register<ThingClass>(byte thingType) where ThingClass : Thing {
thingMsgProcessors[thingType] = static (byte networkId, byte thingId) =>
thingMsgProcessors[thingType] = (byte networkId, byte thingId) =>
Activator.CreateInstance(typeof(ThingClass), networkId, thingId) as ThingClass;
Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}");
}
@ -115,13 +116,13 @@ namespace Passer.Control.Core {
// We can receive our own publish (broadcast) packages. How do we recognize them????
// It is hard to determine our source port
string ipAddress = this.endPoint.Address.ToString();
RemoteParticipant? remoteParticipant = GetParticipant(ipAddress, this.endPoint.Port);
RemoteParticipant remoteParticipant = GetParticipant(ipAddress, this.endPoint.Port);
if (remoteParticipant == null)
remoteParticipant = AddParticipant(ipAddress, this.endPoint.Port);
ReceiveData(data, remoteParticipant);
udpClient.BeginReceive(new AsyncCallback(result => ReceiveUDP(result)), null);
udpClient.BeginReceive(new AsyncCallback(callbackResult => ReceiveUDP(callbackResult)), null);
}
protected ulong nextPublishMe = 0;
@ -177,7 +178,7 @@ namespace Passer.Control.Core {
if (bufferSize <= 0)
return true;
IPEndPoint participantEndpoint = new(IPAddress.Parse(remoteParticipant.ipAddress), remoteParticipant.port);
IPEndPoint participantEndpoint = new IPEndPoint(IPAddress.Parse(remoteParticipant.ipAddress), remoteParticipant.port);
Console.WriteLine($"msg to {participantEndpoint.Address.ToString()} {participantEndpoint.Port}");
this.udpClient?.Send(this.buffer, bufferSize, participantEndpoint);
return true;
@ -289,7 +290,7 @@ namespace Passer.Control.Core {
protected virtual void Process(RemoteParticipant sender, NameMsg msg) {
// Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}");
Thing? thing = sender.Get(msg.networkId, msg.thingId);
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null)
thing.name = msg.name;
}
@ -302,7 +303,7 @@ namespace Passer.Control.Core {
protected virtual void Process(RemoteParticipant sender, CustomMsg msg) {
// Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]");
Thing? thing = sender.Get(msg.networkId, msg.thingId);
Thing thing = sender.Get(msg.networkId, msg.thingId);
thing?.ProcessBinary(msg.bytes);
}

8
RemoteParticipant.cs
View File

@ -18,10 +18,10 @@ namespace Passer.Control.Core {
this.port = port;
}
protected readonly List<Thing> things = new();
protected readonly List<Thing> things = new List<Thing>();
public Thing? Get(byte networkId, byte thingId) {
Thing? thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId));
public Thing Get(byte networkId, byte thingId) {
Thing thing = things.Find(aThing => Thing.IsThing(aThing, networkId, thingId));
return thing;
}
@ -30,7 +30,7 @@ namespace Passer.Control.Core {
public void Add(Thing thing, bool invokeEvent = true) {
// Console.WriteLine($"added thing [{thing.networkId}/{thing.id}]");
Thing? foundThing = Get(thing.networkId, thing.id);
Thing foundThing = Get(thing.networkId, thing.id);
if (foundThing == null) {
things.Add(thing);

11
SiteServer.cs
View File

@ -20,7 +20,7 @@ namespace Passer.Control.Core {
this.udpClient = new UdpClient(port); // for receiving
this.udpClient.BeginReceive(
new AsyncCallback(result => ReceiveUDP(result)),
new Tuple<UdpClient, IPEndPoint>(this.udpClient, new(IPAddress.Any, port)));
new Tuple<UdpClient, IPEndPoint>(this.udpClient, new IPEndPoint(IPAddress.Any, port)));
}
public void Close() {
@ -41,14 +41,15 @@ namespace Passer.Control.Core {
protected override void Process(RemoteParticipant sender, ThingMsg msg) {
//Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]");
Thing? thing = sender.Get(msg.networkId, msg.thingId);
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing == null) {
Thing? newThing = null;
if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<byte, byte, Thing?>? value)) {
Thing newThing = null;
if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<byte, byte, Thing> value)) {
if (value != null)
newThing = value(msg.networkId, msg.thingId);
}
newThing ??= new Thing(sender, msg.networkId, msg.thingId, msg.thingType);
if (newThing == null)
newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType);
sender.Add(newThing);
}

4
Thing.cs
View File

@ -43,8 +43,8 @@ namespace Passer.Control.Core {
public byte id;
public event ChangeHandler OnParentChanged = delegate {};
private Thing? _parent;
public Thing? parent {
private Thing _parent;
public Thing parent {
get => _parent;
set {
if (_parent == value)

55
float16.cs
View File

@ -1,10 +1,8 @@
using System;
namespace Passer.LinearAlgebra
{
namespace Passer.LinearAlgebra {
public class float16
{
public class float16 {
//
// FILE: float16.cpp
// AUTHOR: Rob Tillaart
@ -16,13 +14,11 @@ namespace Passer.LinearAlgebra
public float16() { _value = 0; }
public float16(float f)
{
public float16(float f) {
_value = f32tof16(f);
}
public float toFloat()
{
public float toFloat() {
return f16tof32(_value);
}
@ -156,8 +152,7 @@ namespace Passer.LinearAlgebra
//
// CORE CONVERSION
//
float f16tof32(ushort _value)
{
float f16tof32(ushort _value) {
//ushort sgn;
ushort man;
int exp;
@ -172,13 +167,11 @@ namespace Passer.LinearAlgebra
//Debug.Log($"{sgn} {exp} {man}");
// ZERO
if ((_value & 0x7FFF) == 0)
{
if ((_value & 0x7FFF) == 0) {
return sgn ? -0 : 0;
}
// NAN & INF
if (exp == 0x001F)
{
if (exp == 0x001F) {
if (man == 0)
return sgn ? float.NegativeInfinity : float.PositiveInfinity; //-INFINITY : INFINITY;
else
@ -192,44 +185,46 @@ namespace Passer.LinearAlgebra
f = 1;
// PROCESS MANTISSE
for (int i = 9; i >= 0; i--)
{
for (int i = 9; i >= 0; i--) {
f *= 2;
if ((man & (1 << i)) != 0)
f = f + 1;
}
//Debug.Log($"{f}");
f = f * (float)Math.Pow(2.0f, exp - 25);
if (exp == 0)
{
if (exp == 0) {
f = f * (float)Math.Pow(2.0f, -13); // 5.96046447754e-8;
}
//Debug.Log($"{f}");
return sgn ? -f : f;
}
ushort f32tof16(float f)
{
public static uint SingleToInt32Bits(float value) {
byte[] bytes = BitConverter.GetBytes(value);
if (BitConverter.IsLittleEndian)
Array.Reverse(bytes); // If the system is little-endian, reverse the byte order
return BitConverter.ToUInt32(bytes, 0);
}
ushort f32tof16(float f) {
//uint t = *(uint*)&f;
uint t = (uint)BitConverter.SingleToInt32Bits(f);
//uint t = (uint)BitConverter.SingleToInt32Bits(f);
uint t = SingleToInt32Bits(f);
// man bits = 10; but we keep 11 for rounding
ushort man = (ushort)((t & 0x007FFFFF) >> 12);
short exp = (short)((t & 0x7F800000) >> 23);
bool sgn = (t & 0x80000000) != 0;
// handle 0
if ((t & 0x7FFFFFFF) == 0)
{
if ((t & 0x7FFFFFFF) == 0) {
return sgn ? (ushort)0x8000 : (ushort)0x0000;
}
// denormalized float32 does not fit in float16
if (exp == 0x00)
{
if (exp == 0x00) {
return sgn ? (ushort)0x8000 : (ushort)0x0000;
}
// handle infinity & NAN
if (exp == 0x00FF)
{
if (exp == 0x00FF) {
if (man != 0)
return 0xFE00; // NAN
return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF
@ -238,13 +233,11 @@ namespace Passer.LinearAlgebra
// normal numbers
exp = (short)(exp - 127 + 15);
// overflow does not fit => INF
if (exp > 30)
{
if (exp > 30) {
return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF
}
// subnormal numbers
if (exp < -38)
{
if (exp < -38) {
return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ?
}
if (exp <= 0) // subnormal

14
test/UnitTest1.cs
View File

@ -13,7 +13,7 @@ namespace ControlCore.test {
[Test]
public void Test_Participant() {
Participant participant = new("127.0.0.1", 7681);
Participant participant = new Participant("127.0.0.1", 7682);
ulong milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
ulong startTime = milliseconds;
@ -29,7 +29,7 @@ namespace ControlCore.test {
[Test]
public void Test_SiteServer() {
SiteServer siteServer = new(7681);
SiteServer siteServer = new SiteServer(7681);
ulong milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
ulong startTime = milliseconds;
@ -45,8 +45,8 @@ namespace ControlCore.test {
[Test]
public void Test_SiteParticipant() {
SiteServer siteServer = new(7681);
Participant participant = new("127.0.0.1", 7681);
SiteServer siteServer = new SiteServer(7681);
Participant participant = new Participant("127.0.0.1", 7681);
ulong milliseconds = (ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
ulong startTime = milliseconds;
@ -63,9 +63,9 @@ namespace ControlCore.test {
[Test]
public void Test_ThingMsg() {
SiteServer siteServer = new();
Participant participant = new("127.0.0.1");
Thing thing = new(participant) {
SiteServer siteServer = new SiteServer();
Participant participant = new Participant("127.0.0.1");
Thing thing = new Thing(participant) {
name = "First Thing",
modelUrl = "https://passer.life/extras/ant.jpg"
};

17
test/test.csproj
View File

@ -1,24 +1,15 @@
<Project Sdk="Microsoft.NET.Sdk">
<PropertyGroup>
<TargetFramework>net9.0</TargetFramework>
<LangVersion>latest</LangVersion>
<ImplicitUsings>enable</ImplicitUsings>
<Nullable>enable</Nullable>
<TargetFramework>net5.0</TargetFramework>
<IsPackable>false</IsPackable>
<IsTestProject>true</IsTestProject>
</PropertyGroup>
<ItemGroup>
<PackageReference Include="coverlet.collector" Version="6.0.2" />
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.12.0" />
<PackageReference Include="NUnit" Version="3.13.3" />
<PackageReference Include="NUnit.Analyzers" Version="4.3.0" />
<PackageReference Include="NUnit3TestAdapter" Version="4.6.0" />
</ItemGroup>
<ItemGroup>
<Using Include="NUnit.Framework" />
<PackageReference Include="Microsoft.NET.Test.Sdk" Version="17.13.0" />
<PackageReference Include="NUnit" Version="3.13.2" />
<PackageReference Include="NUnit3TestAdapter" Version="3.17.0" />
</ItemGroup>
<ItemGroup>