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

Merge commit '0f844f5fad764e47bcd3c5634239e30492405fce' into V2

Browse Source
This commit is contained in:
Pascal Serrarens 2025-02-19 11:04:38 +01:00
commit 062aafd19a
20 changed files with 468 additions and 459 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.editorconfig
View File

@ -10,6 +10,10 @@ end_of_line = crlf
charset = utf-8 charset = utf-8
trim_trailing_whitespace = false trim_trailing_whitespace = false
insert_final_newline = false insert_final_newline = false
max_line_length = 80
[*.cs] [*.cs]
csharp_new_line_before_open_brace = none csharp_new_line_before_open_brace = none
# Suppress warnings everywhere
dotnet_diagnostic.IDE1006.severity = none
dotnet_diagnostic.IDE0130.severity = none

12
LinearAlgebra/Angle.cs
View File

@ -1,7 +1,11 @@
using System; using System;
class Angle namespace Passer.LinearAlgebra {
{
public static float Rad2Deg = 360.0f / ((float)Math.PI * 2); public class Angle {
public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f; public const float pi = 3.1415927410125732421875F;
public static float Rad2Deg = 360.0f / ((float)Math.PI * 2);
public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f;
}
} }

318
LinearAlgebra/Matrix.cs
View File

@ -1,11 +1,10 @@
using System; using System;
using System.Diagnostics; using System.Diagnostics;
using Vector3 = UnityEngine.Vector3; using Passer.LinearAlgebra;
using Vector2 = UnityEngine.Vector2;
public readonly struct Slice { public readonly struct Slice {
public int start {get;} public int start { get; }
public int stop {get;} public int stop { get; }
public Slice(int start, int stop) { public Slice(int start, int stop) {
this.start = start; this.start = start;
this.stop = stop; this.stop = stop;
@ -50,7 +49,7 @@ public class Matrix2 {
return new Matrix2(resultData); return new Matrix2(resultData);
} }
public static Matrix2 SkewMatrix(Vector3 v) { public static Matrix2 SkewMatrix(Vector3Float v) {
float[,] result = new float[3, 3] { float[,] result = new float[3, 3] {
{0, -v.z, v.y}, {0, -v.z, v.y},
{v.z, 0, -v.x}, {v.z, 0, -v.x},
@ -137,12 +136,12 @@ public class Matrix2 {
return new Matrix1(result); return new Matrix1(result);
} }
public static Vector3 operator *(Matrix2 A, Vector3 v) { public static Vector3Float operator *(Matrix2 A, Vector3Float v) {
return new Vector3() { return new Vector3Float(
x = A.data[0, 0] * v.x + A.data[0, 1] * v.y + A.data[0, 2] * v.z, A.data[0, 0] * v.x + A.data[0, 1] * v.y + A.data[0, 2] * v.z,
y = A.data[1, 0] * v.x + A.data[1, 1] * v.y + A.data[1, 2] * v.z, A.data[1, 0] * v.x + A.data[1, 1] * v.y + A.data[1, 2] * v.z,
z = A.data[2, 0] * v.x + A.data[2, 1] * v.y + A.data[2, 2] * v.z A.data[2, 0] * v.x + A.data[2, 1] * v.y + A.data[2, 2] * v.z
}; );
} }
@ -263,14 +262,14 @@ public class Matrix1 {
return new Matrix1(magnitude); return new Matrix1(magnitude);
} }
public static Matrix1 FromVector2(Vector2 v) { public static Matrix1 FromVector2(Vector2Float v) {
float[] result = new float[2]; float[] result = new float[2];
result[0] = v.x; result[0] = v.x;
result[1] = v.y; result[1] = v.y;
return new Matrix1(result); return new Matrix1(result);
} }
public static Matrix1 FromVector3(Vector3 v) { public static Matrix1 FromVector3(Vector3Float v) {
float[] result = new float[3]; float[] result = new float[3];
result[0] = v.x; result[0] = v.x;
result[1] = v.y; result[1] = v.y;
@ -278,18 +277,18 @@ public class Matrix1 {
return new Matrix1(result); return new Matrix1(result);
} }
public Vector2 vector2 { public Vector2Float vector2 {
get { get {
if (this.magnitude != 2) if (this.magnitude != 2)
throw new System.ArgumentException("Matrix1 must be of size 2"); throw new System.ArgumentException("Matrix1 must be of size 2");
return new Vector2(this.data[0], this.data[1]); return new Vector2Float(this.data[0], this.data[1]);
} }
} }
public Vector3 vector3 { public Vector3Float vector3 {
get { get {
if (this.magnitude != 3) if (this.magnitude != 3)
throw new System.ArgumentException("Matrix1 must be of size 3"); throw new System.ArgumentException("Matrix1 must be of size 3");
return new Vector3(this.data[0], this.data[1], this.data[2]); return new Vector3Float(this.data[0], this.data[1], this.data[2]);
} }
} }
@ -345,178 +344,179 @@ public class Matrix1 {
} }
} }
public class Matrix { // public class Matrix {
private readonly uint rows = 0; // private readonly uint rows = 0;
private readonly uint cols = 0; // private readonly uint cols = 0;
private float[] data; // private float[] data;
public Matrix(uint rows, uint cols) { // public Matrix(uint rows, uint cols) {
this.rows = rows; // this.rows = rows;
this.cols = cols; // this.cols = cols;
} // }
public static float[,] Diagonal(float[] v) { // public static float[,] Diagonal(float[] v) {
float[,] r = new float[v.Length, v.Length]; // float[,] r = new float[v.Length, v.Length];
for (int i = 0; i < v.Length; i++) { // for (int i = 0; i < v.Length; i++) {
r[i, i] = v[i]; // r[i, i] = v[i];
} // }
return r; // return r;
} // }
public static float[,] Transpose(float[,] m) { // public static float[,] Transpose(float[,] m) {
int rows = m.GetLength(0); // int rows = m.GetLength(0);
int cols = m.GetLength(1); // int cols = m.GetLength(1);
float[,] r = new float[cols, rows]; // float[,] r = new float[cols, rows];
for (uint rowIx = 0; rowIx < rows; rowIx++) { // for (uint rowIx = 0; rowIx < rows; rowIx++) {
for (uint colIx = 0; colIx < cols; colIx++) // for (uint colIx = 0; colIx < cols; colIx++)
r[colIx, rowIx] = m[rowIx, colIx]; // r[colIx, rowIx] = m[rowIx, colIx];
} // }
return r; // return r;
// double checked code // // double checked code
} // }
public static void NegateColumn(float[,] m, uint colIx) { // public static void NegateColumn(float[,] m, uint colIx) {
for (uint rowIx = 0; rowIx < m.GetLength(0); rowIx++) { // for (uint rowIx = 0; rowIx < m.GetLength(0); rowIx++) {
m[rowIx, colIx] = -m[rowIx, colIx]; // m[rowIx, colIx] = -m[rowIx, colIx];
} // }
} // }
public static float Determinant(float[,] matrix) { // public static float Determinant(float[,] matrix) {
int n = matrix.GetLength(0); // int n = matrix.GetLength(0);
if (n != matrix.GetLength(1)) // if (n != matrix.GetLength(1))
throw new System.ArgumentException("Matrix must be square."); // throw new System.ArgumentException("Matrix must be square.");
if (n == 1) // if (n == 1)
return matrix[0, 0]; // Base case for 1x1 matrix // return matrix[0, 0]; // Base case for 1x1 matrix
if (n == 2) // Base case for 2x2 matrix // if (n == 2) // Base case for 2x2 matrix
return matrix[0, 0] * matrix[1, 1] - matrix[0, 1] * matrix[1, 0]; // return matrix[0, 0] * matrix[1, 1] - matrix[0, 1] * matrix[1, 0];
float det = 0; // float det = 0;
for (int col = 0; col < n; col++) // for (int col = 0; col < n; col++)
det += (col % 2 == 0 ? 1 : -1) * matrix[0, col] * Determinant(Minor(matrix, 0, col)); // det += (col % 2 == 0 ? 1 : -1) * matrix[0, col] * Determinant(Minor(matrix, 0, col));
return det; // return det;
} // }
// Helper function to compute the minor of a matrix // // Helper function to compute the minor of a matrix
private static float[,] Minor(float[,] matrix, int rowToRemove, int colToRemove) { // private static float[,] Minor(float[,] matrix, int rowToRemove, int colToRemove) {
int n = matrix.GetLength(0); // int n = matrix.GetLength(0);
float[,] minor = new float[n - 1, n - 1]; // float[,] minor = new float[n - 1, n - 1];
int r = 0, c = 0; // int r = 0, c = 0;
for (int i = 0; i < n; i++) { // for (int i = 0; i < n; i++) {
if (i == rowToRemove) continue; // if (i == rowToRemove) continue;
c = 0; // c = 0;
for (int j = 0; j < n; j++) { // for (int j = 0; j < n; j++) {
if (j == colToRemove) continue; // if (j == colToRemove) continue;
minor[r, c] = matrix[i, j]; // minor[r, c] = matrix[i, j];
c++; // c++;
} // }
r++; // r++;
} // }
return minor; // return minor;
} // }
public static float[,] MultiplyMatrices(float[,] A, float[,] B) { // public static float[,] MultiplyMatrices(float[,] A, float[,] B) {
int rowsA = A.GetLength(0); // int rowsA = A.GetLength(0);
int colsA = A.GetLength(1); // int colsA = A.GetLength(1);
int rowsB = B.GetLength(0); // int rowsB = B.GetLength(0);
int colsB = B.GetLength(1); // int colsB = B.GetLength(1);
if (colsA != rowsB) // if (colsA != rowsB)
throw new System.ArgumentException("Number of columns in A must match number of rows in B."); // throw new System.ArgumentException("Number of columns in A must match number of rows in B.");
float[,] result = new float[rowsA, colsB]; // float[,] result = new float[rowsA, colsB];
for (int i = 0; i < rowsA; i++) { // for (int i = 0; i < rowsA; i++) {
for (int j = 0; j < colsB; j++) { // for (int j = 0; j < colsB; j++) {
float sum = 0.0f; // float sum = 0.0f;
for (int k = 0; k < colsA; k++) // for (int k = 0; k < colsA; k++)
sum += A[i, k] * B[k, j]; // sum += A[i, k] * B[k, j];
result[i, j] = sum; // result[i, j] = sum;
} // }
} // }
return result; // return result;
// double checked code // // double checked code
} // }
public static float[] MultiplyMatrixVector(float[,] A, float[] v) { // public static float[] MultiplyMatrixVector(float[,] A, float[] v) {
int rows = A.GetLength(0); // int rows = A.GetLength(0);
int cols = A.GetLength(1); // int cols = A.GetLength(1);
float[] result = new float[rows]; // float[] result = new float[rows];
for (int i = 0; i < rows; i++) { // for (int i = 0; i < rows; i++) {
for (int j = 0; j < cols; j++) { // for (int j = 0; j < cols; j++) {
result[i] += A[i, j] * v[j]; // result[i] += A[i, j] * v[j];
} // }
} // }
return result; // return result;
} // }
// Vector-matrix multiplication // // Vector-matrix multiplication
public static Vector3 MultiplyMatrixVector3(float[,] A, Vector3 v) { // public static Vector3Float MultiplyMatrixVector3(float[,] A, Vector3Float v) {
return new Vector3() { // return new Vector3Float(
x = A[0, 0] * v.x + A[0, 1] * v.y + A[0, 2] * v.z, // A[0, 0] * v.x + A[0, 1] * v.y + A[0, 2] * v.z,
y = A[1, 0] * v.x + A[1, 1] * v.y + A[1, 2] * v.z, // A[1, 0] * v.x + A[1, 1] * v.y + A[1, 2] * v.z,
z = A[2, 0] * v.x + A[2, 1] * v.y + A[2, 2] * v.z // A[2, 0] * v.x + A[2, 1] * v.y + A[2, 2] * v.z
}; // );
} // }
public static float[,] MultiplyMatrixScalar(float[,] A, float s) { // public static float[,] MultiplyMatrixScalar(float[,] A, float s) {
int rows = A.GetLength(0); // int rows = A.GetLength(0);
int cols = A.GetLength(1); // int cols = A.GetLength(1);
float[,] result = new float[rows, cols]; // float[,] result = new float[rows, cols];
for (int i = 0; i < rows; i++) { // for (int i = 0; i < rows; i++) {
for (int j = 0; j < cols; j++) { // for (int j = 0; j < cols; j++) {
result[i, j] += A[i, j] * s; // result[i, j] += A[i, j] * s;
} // }
} // }
return result; // return result;
} // }
public static float[] GetColumn(float[,] M, int col) { // public static float[] GetColumn(float[,] M, int col) {
int rows = M.GetLength(0); // int rows = M.GetLength(0);
float[] column = new float[rows]; // float[] column = new float[rows];
for (int i = 0; i < rows; i++) { // for (int i = 0; i < rows; i++) {
column[i] = M[i, col]; // column[i] = M[i, col];
} // }
return column; // return column;
} // }
public static Vector3 GetRow3(float[,] M, int rowIx) { // public static Vector3Float GetRow3(float[,] M, int rowIx) {
int cols = M.GetLength(1); // int cols = M.GetLength(1);
Vector3 row = new(); // Vector3Float row = new(
row.x = M[rowIx, 0]; // M[rowIx, 0],
row.y = M[rowIx, 1]; // M[rowIx, 1],
row.z = M[rowIx, 2]; // M[rowIx, 2]
return row; // );
} // return row;
// }
public static void SetRow3(float[,] M, int rowIx, Vector3 v) { // public static void SetRow3(float[,] M, int rowIx, Vector3Float v) {
M[rowIx, 0] = v.x; // M[rowIx, 0] = v.x;
M[rowIx, 1] = v.y; // M[rowIx, 1] = v.y;
M[rowIx, 2] = v.z; // M[rowIx, 2] = v.z;
} // }
public static float Dot(float[] a, float[] b) { // public static float Dot(float[] a, float[] b) {
float sum = 0; // float sum = 0;
for (int i = 0; i < a.Length; i++) sum += a[i] * b[i]; // for (int i = 0; i < a.Length; i++) sum += a[i] * b[i];
return sum; // return sum;
} // }
public static float[,] IdentityMatrix(int size) { // public static float[,] IdentityMatrix(int size) {
float[,] I = new float[size, size]; // float[,] I = new float[size, size];
for (int i = 0; i < size; i++) I[i, i] = 1.0f; // for (int i = 0; i < size; i++) I[i, i] = 1.0f;
return I; // return I;
} // }
} // }

24
LinearAlgebra/Spherical.cs
View File

@ -1,5 +1,3 @@
using UnityEngine;
namespace Passer.LinearAlgebra { namespace Passer.LinearAlgebra {
public class Spherical { public class Spherical {
public float distance; public float distance;
@ -17,30 +15,30 @@ namespace Passer.LinearAlgebra {
this.direction = direction; this.direction = direction;
} }
public static Spherical FromVector3(Vector3 v) { public static Spherical FromVector3(Vector3Float v) {
float distance = v.magnitude; float distance = v.magnitude;
if (distance == 0.0f) if (distance == 0.0f)
return new Spherical(distance, 0, 0); return new Spherical(distance, 0, 0);
else { else {
float verticalAngle = (Mathf.PI / 2 - Mathf.Acos(v.y / distance)) * Mathf.Rad2Deg; float verticalAngle = (float)((Angle.pi / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg);
float horizontalAngle = Mathf.Atan2(v.x, v.z) * Mathf.Rad2Deg; float horizontalAngle = (float) Math.Atan2(v.x, v.z) * Angle.Rad2Deg;
return new Spherical(distance, horizontalAngle, verticalAngle); return new Spherical(distance, horizontalAngle, verticalAngle);
} }
} }
public Vector3 ToVector3() { public Vector3Float ToVector3() {
float verticalRad = (UnityEngine.Mathf.PI / 2) - this.direction.vertical * UnityEngine.Mathf.Deg2Rad; float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad;
float horizontalRad = this.direction.horizontal * UnityEngine.Mathf.Deg2Rad; float horizontalRad = this.direction.horizontal * Angle.Deg2Rad;
float cosVertical = UnityEngine.Mathf.Cos(verticalRad); float cosVertical = (float)Math.Cos(verticalRad);
float sinVertical = UnityEngine.Mathf.Sin(verticalRad); float sinVertical = (float)Math.Sin(verticalRad);
float cosHorizontal = UnityEngine.Mathf.Cos(horizontalRad); float cosHorizontal = (float)Math.Cos(horizontalRad);
float sinHorizontal = UnityEngine.Mathf.Sin(horizontalRad); float sinHorizontal = (float)Math.Sin(horizontalRad);
float x = this.distance * sinVertical * sinHorizontal; float x = this.distance * sinVertical * sinHorizontal;
float y = this.distance * cosVertical; float y = this.distance * cosVertical;
float z = this.distance * sinVertical * cosHorizontal; float z = this.distance * sinVertical * cosHorizontal;
Vector3 v = new Vector3(x, y, z); Vector3Float v = new Vector3Float(x, y, z);
return v; return v;
} }
} }

22
LinearAlgebra/Vector3.cs
View File

@ -1,3 +1,7 @@
#if UNITY_5_3_OR_NEWER
using Passer.LinearAlgebra.Vector3Float = UnityEngine.Vector3
#else
namespace Passer.LinearAlgebra { namespace Passer.LinearAlgebra {
public class Vector3Of<T> { public class Vector3Of<T> {
@ -10,12 +14,20 @@ namespace Passer.LinearAlgebra {
this.y = y; this.y = y;
this.z = z; this.z = z;
} }
// public uint magnitude {
// get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
// }
} }
public class Vector3Int : Vector3Of<int> { public class Vector3Int(int x, int y, int z) : Vector3Of<int>(x, y, z) {
public Vector3Int(int x, int y, int z) : base(x, y, z) { }
} }
public class Vector3Float : Vector3Of<float> { public class Vector3Float(float x, float y, float z) : Vector3Of<float>(x, y, z) {
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);
}
} }
} }
#endif

5
Messages/CustomMsg.cs
View File

@ -1,3 +1,4 @@
#nullable enable
namespace Passer.Control.Core { namespace Passer.Control.Core {
public class CustomMsg : IMessage { public class CustomMsg : IMessage {
@ -23,11 +24,13 @@ namespace Passer.Control.Core {
public CustomMsg(byte networkId, Thing thing) : base() { public CustomMsg(byte networkId, Thing thing) : base() {
this.networkId = networkId; this.networkId = networkId;
this.thingId = thing.id; this.thingId = thing.id;
this.bytes = [];
} }
public override byte Serialize(ref byte[] buffer) { public override byte Serialize(ref byte[] buffer) {
if (bytes == null) if (bytes.Length == 0)
return 0; return 0;
byte ix = 0; byte ix = 0;
buffer[ix++] = CustomMsg.Id; buffer[ix++] = CustomMsg.Id;
buffer[ix++] = this.networkId; buffer[ix++] = this.networkId;

29
Messages/DestroyMsg.cs Normal file
View File

@ -0,0 +1,29 @@
namespace Passer.Control.Core {
public class DestroyMsg : IMessage {
public const byte Id = 0x20;
public const byte length = 3;
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, Participant client, byte packetSize) {
if (packetSize != length)
return false;
byte[] buffer = await Receive(dataStream, packetSize);
DestroyMsg msg = new(buffer);
client.messageQueue.Enqueue(msg);
return true;
}
}
}

46
Messages/InvestigateMsg.cs Normal file
View File

@ -0,0 +1,46 @@
namespace Passer.Control.Core {
public class InvestigateMsg : IMessage {
public const byte Id = 0x81;
public const byte length = 3;
public byte networkId;
public byte thingId;
public InvestigateMsg(byte networkId, byte thingId) {
this.networkId = networkId;
this.thingId = thingId;
}
public InvestigateMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = InvestigateMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
return ix;
}
public override void Deserialize(byte[] buffer) {
uint ix = 0;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
}
//public static bool Send(Participant client, CoreThing thing) {
// InvestigateMsg msg = new(thing.networkId, thing.id);
// return SendMsg(client, msg);
//}
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
if (packetSize != length)
return false;
byte[] buffer = await Receive(dataStream, packetSize);
InvestigateMsg msg = new(buffer);
//UnityEngine.Debug.Log($"Receive investigate [{msg.networkId}/{msg.thingId}]");
client.messageQueue.Enqueue(msg);
return true;
}
}
}

214
Messages/Messages.cs
View File

@ -1,7 +1,3 @@
using System.IO;
using System.Threading.Tasks;
using Passer.LinearAlgebra;
namespace Passer.Control.Core { namespace Passer.Control.Core {
public class IMessage { public class IMessage {
@ -40,214 +36,4 @@ namespace Passer.Control.Core {
} }
} }
#region Investigate
public class InvestigateMsg : IMessage {
public const byte Id = 0x81;
public const byte length = 3;
public byte networkId;
public byte thingId;
public InvestigateMsg(byte networkId, byte thingId) {
this.networkId = networkId;
this.thingId = thingId;
}
public InvestigateMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = InvestigateMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
return ix;
}
public override void Deserialize(byte[] buffer) {
uint ix = 0;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
}
//public static bool Send(Participant client, CoreThing thing) {
// InvestigateMsg msg = new(thing.networkId, thing.id);
// return SendMsg(client, msg);
//}
public static async Task<bool> Receive(Stream dataStream, Participant client, byte packetSize) {
if (packetSize != length)
return false;
byte[] buffer = await Receive(dataStream, packetSize);
InvestigateMsg msg = new(buffer);
//UnityEngine.Debug.Log($"Receive investigate [{msg.networkId}/{msg.thingId}]");
client.messageQueue.Enqueue(msg);
return true;
}
}
#endregion Investigate
#region Pose
public class PoseMsg : IMessage {
public const byte Id = 0x10;
public const byte length = 4 + 4 + 4;
public byte networkId;
public byte thingId;
public byte poseType;
public const byte Pose_Position = 0x01;
public const byte Pose_Orientation = 0x02;
public const byte Pose_LinearVelocity = 0x04;
public const byte Pose_AngularVelocity = 0x08;
public Spherical position;
public SwingTwist orientation;
public Spherical linearVelocity;
public Spherical angularVelocity;
public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation) {
this.networkId = networkId;
this.thingId = thingId;
this.position = position;
this.orientation = orientation;
this.poseType = 0;
if (this.position != null)
this.poseType |= Pose_Position;
else
this.position = new Spherical(0, 0, 0);
if (this.orientation != null)
this.poseType |= Pose_Orientation;
else
this.orientation = SwingTwist.zero;
}
public PoseMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = PoseMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
buffer[ix++] = this.poseType;
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) {
byte ix = 0;
this.networkId = buffer[ix++];
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.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, SwingTwist orientation) {
PoseMsg msg = new(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);
// Do no process poses with nwid == 0 (== local)
if (msg.networkId == 0)
return true;
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
#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, Participant 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 = 3;
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, Participant 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
} }

4
Messages/NameMsg.cs
View File

@ -8,7 +8,7 @@ namespace Passer.Control.Core {
public byte networkId; public byte networkId;
public byte thingId; public byte thingId;
public byte len; public byte len;
public string? name = null; public string name = "";
public NameMsg(byte networkId, Thing thing) { public NameMsg(byte networkId, Thing thing) {
this.networkId = networkId; this.networkId = networkId;
@ -29,7 +29,7 @@ namespace Passer.Control.Core {
} }
public override byte Serialize(ref byte[] buffer) { public override byte Serialize(ref byte[] buffer) {
if (this.name == null) if (this.name.Length == 0)
return 0; return 0;
byte ix = 0; byte ix = 0;

109
Messages/PoseMsg.cs Normal file
View File

@ -0,0 +1,109 @@
using Passer.LinearAlgebra;
namespace Passer.Control.Core {
public class PoseMsg : IMessage {
public const byte Id = 0x10;
public const byte length = 4 + 4 + 4;
public byte networkId;
public byte thingId;
public byte poseType;
public const byte Pose_Position = 0x01;
public const byte Pose_Orientation = 0x02;
public const byte Pose_LinearVelocity = 0x04;
public const byte Pose_AngularVelocity = 0x08;
public Spherical position = Spherical.zero;
public SwingTwist orientation = SwingTwist.zero;
public Spherical linearVelocity = Spherical.zero;
public Spherical angularVelocity = Spherical.zero;
public PoseMsg(byte networkId, byte thingId, Spherical position, SwingTwist orientation) {
this.networkId = networkId;
this.thingId = thingId;
this.position = position;
this.orientation = orientation;
this.poseType = 0;
if (this.position != null)
this.poseType |= Pose_Position;
if (this.orientation != null)
this.poseType |= Pose_Orientation;
}
public PoseMsg(byte[] buffer) : base(buffer) { }
public override byte Serialize(ref byte[] buffer) {
byte ix = 0;
buffer[ix++] = PoseMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
buffer[ix++] = this.poseType;
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) {
byte ix = 0;
this.networkId = buffer[ix++];
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.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, SwingTwist orientation) {
PoseMsg msg = new(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);
// Do no process poses with nwid == 0 (== local)
if (msg.networkId == 0)
return true;
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);
}
}
}

22
Messages/TextMsg.cs Normal file
View File

@ -0,0 +1,22 @@
namespace Passer.Control.Core {
public class TextMsg(byte[] buffer) : IMessage(buffer) {
public const byte Id = 0xB0;
public string text = "";
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, Participant client, byte packetSize) {
byte[] buffer = await Receive(dataStream, packetSize);
TextMsg msg = new(buffer);
client.messageQueue.Enqueue(msg);
return true;
}
}
}

26
Participant.cs
View File

@ -1,3 +1,4 @@
#nullable enable
using System; using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.Collections.Concurrent; using System.Collections.Concurrent;
@ -13,8 +14,8 @@ namespace Passer.Control.Core {
//public byte networkId = 0; //public byte networkId = 0;
public string name = "Participant"; public string name = "Participant";
public IPEndPoint endPoint = null; public IPEndPoint? endPoint = null;
public UdpClient udpClient = null; public UdpClient? udpClient = null;
public string broadcastIpAddress = "255.255.255.255"; public string broadcastIpAddress = "255.255.255.255";
public readonly ConcurrentQueue<IMessage> messageQueue = new(); public readonly ConcurrentQueue<IMessage> messageQueue = new();
@ -64,7 +65,7 @@ namespace Passer.Control.Core {
public List<RemoteParticipant> senders = new(); public List<RemoteParticipant> senders = new();
public RemoteParticipant GetParticipant(string ipAddress, int port) { public RemoteParticipant? GetParticipant(string ipAddress, int port) {
//Console.WriteLine($"Get Participant {ipAddress}:{port}"); //Console.WriteLine($"Get Participant {ipAddress}:{port}");
foreach (RemoteParticipant sender in senders) { foreach (RemoteParticipant sender in senders) {
if (sender.ipAddress == ipAddress && sender.port == port) if (sender.ipAddress == ipAddress && sender.port == port)
@ -81,7 +82,7 @@ namespace Passer.Control.Core {
return participant; return participant;
} }
protected readonly Dictionary<byte, Func<byte, byte, Thing>> thingMsgProcessors = new(); protected readonly Dictionary<byte, Func<byte, byte, Thing?>> thingMsgProcessors = new();
public delegate Thing ThingConstructor(byte networkId, byte thingId); public delegate Thing ThingConstructor(byte networkId, byte thingId);
public void Register(byte thingType, ThingConstructor constr) { public void Register(byte thingType, ThingConstructor constr) {
@ -93,7 +94,7 @@ namespace Passer.Control.Core {
} }
public void Register<ThingClass>(byte thingType) where ThingClass : Thing { public void Register<ThingClass>(byte thingType) where ThingClass : Thing {
thingMsgProcessors[thingType] = (byte networkId, byte thingId) => thingMsgProcessors[thingType] = static (byte networkId, byte thingId) =>
Activator.CreateInstance(typeof(ThingClass), networkId, thingId) as ThingClass; Activator.CreateInstance(typeof(ThingClass), networkId, thingId) as ThingClass;
Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}"); Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}");
} }
@ -103,17 +104,20 @@ namespace Passer.Control.Core {
#region Update #region Update
protected void ReceiveUDP(IAsyncResult result) { protected void ReceiveUDP(IAsyncResult result) {
if (udpClient == null || this.endPoint == null) if (this.udpClient == null || this.endPoint == null)
return; return;
byte[] data = udpClient.EndReceive(result, ref this.endPoint); byte[] data = this.udpClient.EndReceive(result, ref this.endPoint);
// This does not yet take multi-packet messages into account! // This does not yet take multi-packet messages into account!
if (this.endPoint == null)
return;
// We can receive our own publish (broadcast) packages. How do we recognize them???? // We can receive our own publish (broadcast) packages. How do we recognize them????
// It is hard to determine our source port // It is hard to determine our source port
RemoteParticipant remoteParticipant = this.GetParticipant(endPoint.Address.ToString(), endPoint.Port); string ipAddress = this.endPoint.Address.ToString();
RemoteParticipant? remoteParticipant = GetParticipant(ipAddress, this.endPoint.Port);
if (remoteParticipant == null) if (remoteParticipant == null)
remoteParticipant = this.AddParticipant(endPoint.Address.ToString(), endPoint.Port); remoteParticipant = AddParticipant(ipAddress, this.endPoint.Port);
ReceiveData(data, remoteParticipant); ReceiveData(data, remoteParticipant);
@ -285,7 +289,7 @@ namespace Passer.Control.Core {
protected virtual void Process(RemoteParticipant sender, NameMsg msg) { protected virtual void Process(RemoteParticipant sender, NameMsg msg) {
// Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}"); // 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) if (thing != null)
thing.name = msg.name; thing.name = msg.name;
} }
@ -298,7 +302,7 @@ namespace Passer.Control.Core {
protected virtual void Process(RemoteParticipant sender, CustomMsg msg) { protected virtual void Process(RemoteParticipant sender, CustomMsg msg) {
// Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]"); // 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); thing?.ProcessBinary(msg.bytes);
} }

13
RemoteParticipant.cs
View File

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

4
Sensors/DistanceSensor.cs
View File

@ -5,9 +5,9 @@ namespace Passer.Control.Core {
public class DistanceSensor : Thing { public class DistanceSensor : Thing {
public float distance = 0; public float distance = 0;
public DistanceSensor() : base(true) { } public DistanceSensor(RemoteParticipant participant) : base(participant, true) { }
public DistanceSensor(byte networkId, byte thingId) : base(null, networkId, thingId, (byte)Type.TemperatureSensor) { public DistanceSensor(RemoteParticipant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) {
} }
#if UNITY_5_3_OR_NEWER #if UNITY_5_3_OR_NEWER

5
Sensors/TemperatureSensor.cs
View File

@ -2,11 +2,8 @@ using System;
namespace Passer.Control.Core { namespace Passer.Control.Core {
public class TemperatureSensor : Thing { public class TemperatureSensor(Participant participant, byte networkId, byte thingId) : Thing(participant, networkId, thingId, (byte)Type.TemperatureSensor) {
public float temp = 0; public float temp = 0;
public TemperatureSensor(byte networkId, byte thingId) : base(null, networkId, thingId, (byte)Type.TemperatureSensor) {
}
public override void ProcessBinary(byte[] bytes) { public override void ProcessBinary(byte[] bytes) {
byte ix = 0; byte ix = 0;

5
Sensors/TouchSensor.cs
View File

@ -1,10 +1,9 @@
namespace Passer.Control.Core { namespace Passer.Control.Core {
public class TouchSensor : Thing { public class TouchSensor : Thing {
//public Thing touchedThing = null;
public bool touchedSomething = false; public bool touchedSomething = false;
public TouchSensor(bool invokeEvent = true) : base(invokeEvent) { public TouchSensor(RemoteParticipant participant, bool invokeEvent = true) : base(participant, invokeEvent) {
touchedSomething = false;
} }
#if UNITY_5_3_OR_NEWER #if UNITY_5_3_OR_NEWER

18
SiteServer.cs
View File

@ -23,10 +23,10 @@ namespace Passer.Control.Core {
} }
public void Close() { public void Close() {
this.udpClient.Close(); this.udpClient?.Close();
} }
public override void Publish() { public override void Publish() {
} }
protected override void Process(RemoteParticipant sender, ClientMsg msg) { protected override void Process(RemoteParticipant sender, ClientMsg msg) {
@ -40,13 +40,15 @@ namespace Passer.Control.Core {
protected override void Process(RemoteParticipant sender, ThingMsg msg) { protected override void Process(RemoteParticipant sender, ThingMsg msg) {
//Console.WriteLine($"SiteServer: Process thing [{msg.networkId}/{msg.thingId}]"); //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) { if (thing == null) {
Thing newThing; Thing? newThing = null;
if (thingMsgProcessors.ContainsKey(msg.thingType)) if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<byte, byte, Thing?>? value)) {
newThing = thingMsgProcessors[msg.thingType](msg.networkId, msg.thingId); if (value != null)
else newThing = value(msg.networkId, msg.thingId);
newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType); }
newThing ??= new Thing(sender, msg.networkId, msg.thingId, msg.thingType);
sender.Add(newThing); sender.Add(newThing);
} }
} }

43
Thing.cs
View File

@ -41,16 +41,16 @@ namespace Passer.Control.Core {
public byte networkId; public byte networkId;
public byte id; public byte id;
public event ChangeHandler OnParentChanged; public event ChangeHandler OnParentChanged = delegate {};
private Thing _parent; private Thing? _parent;
public Thing parent { public Thing? parent {
get => _parent; get => _parent;
set { set {
if (_parent == value) if (_parent == value)
return; return;
if (value == null) { if (value == null) {
_parent.RemoveChild(this); _parent?.RemoveChild(this);
_parent = null; _parent = null;
} }
else { else {
@ -74,8 +74,8 @@ namespace Passer.Control.Core {
public List<Thing> children = new List<Thing>(); public List<Thing> children = new List<Thing>();
public byte type; public byte type;
public event ChangeHandler OnNameChanged; public event ChangeHandler OnNameChanged = delegate {};
private string _name; private string _name = "";
public virtual string name { public virtual string name {
get => _name; get => _name;
set { set {
@ -86,11 +86,11 @@ namespace Passer.Control.Core {
} }
} }
public string modelUrl; public string modelUrl = "";
public byte poseUpdated = 0x00; public byte poseUpdated = 0x00;
public event ChangeHandler OnPositionChanged; public event ChangeHandler OnPositionChanged = delegate {};
private Spherical _position; private Spherical _position = Spherical.zero;
public Spherical position { public Spherical position {
get { return _position; } get { return _position; }
set { set {
@ -101,8 +101,8 @@ namespace Passer.Control.Core {
} }
} }
public event ChangeHandler OnOrientationChanged; public event ChangeHandler OnOrientationChanged = delegate {};
private SwingTwist _orientation; private SwingTwist _orientation = SwingTwist.zero;
public SwingTwist orientation { public SwingTwist orientation {
get { return _orientation; } get { return _orientation; }
set { set {
@ -113,8 +113,8 @@ namespace Passer.Control.Core {
} }
} }
public event SphericalHandler OnLinearVelocityChanged; public event SphericalHandler OnLinearVelocityChanged = delegate {};
private Spherical _linearVelocity; private Spherical _linearVelocity = Spherical.zero;
public Spherical linearVelocity { public Spherical linearVelocity {
get => _linearVelocity; get => _linearVelocity;
set { set {
@ -124,7 +124,7 @@ namespace Passer.Control.Core {
} }
} }
} }
public Spherical angularVelocity; public Spherical angularVelocity = Spherical.zero;
#if UNITY_5_3_OR_NEWER #if UNITY_5_3_OR_NEWER
[NonSerialized] [NonSerialized]
@ -135,12 +135,8 @@ namespace Passer.Control.Core {
#region Init #region Init
// public virtual void Init(bool invokeEvent = false) { public Thing(RemoteParticipant participant, bool invokeEvent = false) {
// if (invokeEvent) this.participant = participant;
// InvokeNewThing(this);
// }
public Thing(bool invokeEvent = false) {
if (invokeEvent) if (invokeEvent)
InvokeNewThing(this); InvokeNewThing(this);
} }
@ -149,9 +145,6 @@ namespace Passer.Control.Core {
this.id = thingId; this.id = thingId;
this.type = thingType; this.type = thingType;
this.networkId = networkId; this.networkId = networkId;
//this.Init();
//OnNewThing?.Invoke(this);
//Thing.Add(this);
} }
public virtual void CreateComponent() {} public virtual void CreateComponent() {}
@ -195,10 +188,10 @@ namespace Passer.Control.Core {
//---------- All Things //---------- All Things
private static readonly List<Thing> allThings = new(); private static readonly List<Thing> allThings = [];
public delegate void ThingHandler(Thing t); public delegate void ThingHandler(Thing t);
public static event ThingHandler OnNewThing; public static event ThingHandler OnNewThing = delegate {};
public static void InvokeNewThing(Thing thing) { public static void InvokeNewThing(Thing thing) {
OnNewThing?.Invoke(thing); OnNewThing?.Invoke(thing);
} }

2
test/UnitTest1.cs
View File

@ -65,7 +65,7 @@ namespace ControlCore.test {
public void Test_ThingMsg() { public void Test_ThingMsg() {
SiteServer siteServer = new(); SiteServer siteServer = new();
Participant participant = new("127.0.0.1"); Participant participant = new("127.0.0.1");
Thing thing = new() { Thing thing = new(participant) {
name = "First Thing", name = "First Thing",
modelUrl = "https://passer.life/extras/ant.jpg" modelUrl = "https://passer.life/extras/ant.jpg"
}; };