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

Merge commit '8357c5d6231947a3ecd32a81b723506640fff441' into V2

Browse Source
This commit is contained in:
Pascal Serrarens 2025-04-17 08:48:30 +02:00
commit 19b4c33e05
21 changed files with 669 additions and 210 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
Examples/BB2B/Program.cs
View File

@ -1,33 +0,0 @@
using System.Threading;
using RoboidControl;
#if !UNITY_5_3_OR_NEWER
class BB2B {
static void Main() {
// The robot's propulsion is a differential drive
DifferentialDrive bb2b = new();
// It has a touch sensor at the front left of the roboid
TouchSensor touchLeft = new(bb2b);
// and other one on the right
TouchSensor touchRight = new(bb2b);
// Do forever:
while (true) {
// The left wheel turns forward when nothing is touched on the right side
// and turn backward when the roboid hits something on the right
float leftWheelSpeed = touchRight.touchedSomething ? -600.0f : 600.0f;
// The right wheel does the same, but instead is controlled by
// touches on the left side
float rightWheelSpeed = touchLeft.touchedSomething ? -600.0f : 600.0f;
// When both sides are touching something, both wheels will turn backward
// and the roboid will move backwards
bb2b.SetWheelVelocity(leftWheelSpeed, rightWheelSpeed);
// Update the roboid state
bb2b.Update(true);
// and sleep for 100ms
Thread.Sleep(100);
}
}
}
#endif

287
LinearAlgebra/src/Decomposition.cs Normal file
View File

@ -0,0 +1,287 @@
using System;
namespace LinearAlgebra {
class QR {
// QR Decomposition of a matrix A
public static (Matrix2 Q, Matrix2 R) Decomposition(Matrix2 A) {
int nRows = A.nRows;
int nCols = A.nCols;
float[,] Q = new float[nRows, nCols];
float[,] R = new float[nCols, nCols];
// Perform Gram-Schmidt orthogonalization
for (uint colIx = 0; colIx < nCols; colIx++) {
// Step 1: v = column(ix) of A
float[] v = new float[nRows];
for (int rowIx = 0; rowIx < nRows; rowIx++)
v[rowIx] = A.data[rowIx, colIx];
// Step 2: Subtract projections of v onto previous q's (orthogonalize)
for (uint colIx2 = 0; colIx2 < colIx; colIx2++) {
float dotProd = 0;
for (int i = 0; i < nRows; i++)
dotProd += Q[i, colIx2] * v[i];
for (int i = 0; i < nRows; i++)
v[i] -= dotProd * Q[i, colIx2];
}
// Step 3: Normalize v to get column(ix) of Q
float norm = 0;
for (int rowIx = 0; rowIx < nRows; rowIx++)
norm += v[rowIx] * v[rowIx];
norm = (float)Math.Sqrt(norm);
for (int rowIx = 0; rowIx < nRows; rowIx++)
Q[rowIx, colIx] = v[rowIx] / norm;
// Store the coefficients of R
for (int colIx2 = 0; colIx2 <= colIx; colIx2++) {
R[colIx2, colIx] = 0;
for (int k = 0; k < nRows; k++)
R[colIx2, colIx] += Q[k, colIx2] * A.data[k, colIx];
}
}
return (new Matrix2(Q), new Matrix2(R));
}
// Reduced QR Decomposition of a matrix A
public static (Matrix2 Q, Matrix2 R) ReducedDecomposition(Matrix2 A) {
int nRows = A.nRows;
int nCols = A.nCols;
float[,] Q = new float[nRows, nCols];
float[,] R = new float[nCols, nCols];
// Perform Gram-Schmidt orthogonalization
for (int colIx = 0; colIx < nCols; colIx++) {
// Step 1: v = column(colIx) of A
float[] columnIx = new float[nRows];
bool isZeroColumn = true;
for (int rowIx = 0; rowIx < nRows; rowIx++) {
columnIx[rowIx] = A.data[rowIx, colIx];
if (columnIx[rowIx] != 0)
isZeroColumn = false;
}
if (isZeroColumn) {
for (int rowIx = 0; rowIx < nRows; rowIx++)
Q[rowIx, colIx] = 0;
// Set corresponding R element to 0
R[colIx, colIx] = 0;
Console.WriteLine($"zero column {colIx}");
continue;
}
// Step 2: Subtract projections of v onto previous q's (orthogonalize)
for (int colIx2 = 0; colIx2 < colIx; colIx2++) {
// Compute the dot product of v and column(colIx2) of Q
float dotProduct = 0;
for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
dotProduct += columnIx[rowIx2] * Q[rowIx2, colIx2];
// Subtract the projection from v
for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
columnIx[rowIx2] -= dotProduct * Q[rowIx2, colIx2];
}
// Step 3: Normalize v to get column(colIx) of Q
float norm = 0;
for (int rowIx = 0; rowIx < nRows; rowIx++)
norm += columnIx[rowIx] * columnIx[rowIx];
if (norm == 0)
throw new Exception("invalid value");
norm = (float)Math.Sqrt(norm);
for (int rowIx = 0; rowIx < nRows; rowIx++)
Q[rowIx, colIx] = columnIx[rowIx] / norm;
// Here is where it deviates from the Full QR Decomposition !
// Step 4: Compute the row(colIx) of R
for (int colIx2 = colIx; colIx2 < nCols; colIx2++) {
float dotProduct = 0;
for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++)
dotProduct += Q[rowIx2, colIx] * A.data[rowIx2, colIx2];
R[colIx, colIx2] = dotProduct;
}
}
if (!float.IsFinite(R[0, 0]))
throw new Exception("invalid value");
return (new Matrix2(Q), new Matrix2(R));
}
}
class SVD {
// According to ChatGPT, Mathnet uses Golub-Reinsch SVD algorithm
// 1. Bidiagonalization: The input matrix AA is reduced to a bidiagonal form using Golub-Kahan bidiagonalization.
// This process involves applying a sequence of Householder reflections to AA to create a bidiagonal matrix.
// This step reduces the complexity by making the matrix simpler while retaining the essential structure needed for SVD.
//
// 2. Diagonalization: Once the matrix is in bidiagonal form,
// the singular values are computed using an iterative process
// (typically involving QR factorization or Jacobi rotations) until convergence.
// This process diagonalizes the bidiagonal matrix and allows extraction of the singular values.
//
// 3. Computing UU and VTVT: After obtaining the singular values,
// the left singular vectors UU and right singular vectors VTVT are computed
// using the accumulated transformations (such as Householder reflections) from the bidiagonalization step.
// Bidiagnolizations through Householder transformations
public static (Matrix2 U1, Matrix2 B, Matrix2 V1) Bidiagonalization(Matrix2 A) {
int m = A.nRows; // Rows of A
int n = A.nCols; // Columns of A
float[,] U1 = new float[m, m]; // Left orthogonal matrix
float[,] V1 = new float[n, n]; // Right orthogonal matrix
float[,] B = A.Clone().data; // Copy A to B for transformation
// Initialize U1 and V1 as identity matrices
for (int i = 0; i < m; i++)
U1[i, i] = 1;
for (int i = 0; i < n; i++)
V1[i, i] = 1;
// Perform Householder reflections to create a bidiagonal matrix B
for (int j = 0; j < n; j++) {
// Step 1: Construct the Householder vector y
float[] y = new float[m - j];
for (int i = j; i < m; i++)
y[i - j] = B[i, j];
// Step 2: Compute the norm and scalar alpha
float norm = 0;
for (int i = 0; i < y.Length; i++)
norm += y[i] * y[i];
norm = (float)Math.Sqrt(norm);
if (B[j, j] > 0)
norm = -norm;
float alpha = (float)Math.Sqrt(0.5 * (norm * (norm - B[j, j])));
float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j])));
// Step 3: Apply the reflection to zero out below diagonal
for (int k = j; k < n; k++) {
float dot = 0;
for (int i = j; i < m; i++)
dot += y[i - j] * B[i, k];
dot /= r;
for (int i = j; i < m; i++)
B[i, k] -= 2 * dot * y[i - j];
}
// Step 4: Update U1 with the Householder reflection (U1 * Householder)
for (int i = j; i < m; i++)
U1[i, j] = y[i - j] / alpha;
// Step 5: Update V1 (storing the Householder vector y)
// Correct indexing: we only need to store part of y in V1 from index j to n
for (int i = j; i < n; i++)
V1[j, i] = B[j, i];
// Repeat steps for further columns if necessary
}
return (new Matrix2(U1), new Matrix2(B), new Matrix2(V1));
}
public static Matrix2 Bidiagonalize(Matrix2 A) {
int m = A.nRows; // Rows of A
int n = A.nCols; // Columns of A
float[,] B = A.Clone().data; // Copy A to B for transformation
// Perform Householder reflections to create a bidiagonal matrix B
for (int j = 0; j < n; j++) {
// Step 1: Construct the Householder vector y
float[] y = new float[m - j];
for (int i = j; i < m; i++)
y[i - j] = B[i, j];
// Step 2: Compute the norm and scalar alpha
float norm = 0;
for (int i = 0; i < y.Length; i++)
norm += y[i] * y[i];
norm = (float)Math.Sqrt(norm);
if (B[j, j] > 0)
norm = -norm;
float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j])));
// Step 3: Apply the reflection to zero out below diagonal
for (int k = j; k < n; k++) {
float dot = 0;
for (int i = j; i < m; i++)
dot += y[i - j] * B[i, k];
dot /= r;
for (int i = j; i < m; i++)
B[i, k] -= 2 * dot * y[i - j];
}
// Repeat steps for further columns if necessary
}
return new Matrix2(B);
}
// QR Iteration for diagonalization of a bidiagonal matrix B
public static (Matrix1 singularValues, Matrix2 U, Matrix2 Vt) QRIteration(Matrix2 B) {
int m = B.nRows;
int n = B.nCols;
Matrix2 U = new(m, m); // Left singular vectors (U)
Matrix2 Vt = new(n, n); // Right singular vectors (V^T)
float[] singularValues = new float[Math.Min(m, n)]; // Singular values
// Initialize U and Vt as identity matrices
for (int i = 0; i < m; i++)
U.data[i, i] = 1;
for (int i = 0; i < n; i++)
Vt.data[i, i] = 1;
// Perform QR iterations
float tolerance = 1e-7f; //1e-12f; for double
bool converged = false;
while (!converged) {
// Perform QR decomposition on the matrix B
(Matrix2 Q, Matrix2 R) = QR.Decomposition(B);
// Update B to be the product Q * R //R * Q
B = R * Q;
// Accumulate the transformations in U and Vt
U *= Q;
Vt *= R;
// Check convergence by looking at the off-diagonal elements of B
converged = true;
for (int i = 0; i < m - 1; i++) {
for (int j = i + 1; j < n; j++) {
if (Math.Abs(B.data[i, j]) > tolerance) {
converged = false;
break;
}
}
}
}
// Extract singular values (diagonal elements of B)
for (int i = 0; i < Math.Min(m, n); i++)
singularValues[i] = B.data[i, i];
return (new Matrix1(singularValues), U, Vt);
}
public static (Matrix2 U, Matrix1 S, Matrix2 Vt) Decomposition(Matrix2 A) {
if (A.nRows != A.nCols)
throw new ArgumentException("SVD: matrix A has to be square.");
Matrix2 B = Bidiagonalize(A);
(Matrix1 S, Matrix2 U, Matrix2 Vt) = QRIteration(B);
return (U, S, Vt);
}
}
}

185
LinearAlgebra/src/Matrix.cs
View File

@ -10,9 +10,9 @@ namespace LinearAlgebra
public readonly struct Slice
{
public uint start { get; }
public uint stop { get; }
public Slice(uint start, uint stop)
public int start { get; }
public int stop { get; }
public Slice(int start, int stop)
{
this.start = start;
this.stop = stop;
@ -23,10 +23,10 @@ namespace LinearAlgebra
{
public float[,] data { get; }
public uint nRows => (uint)data.GetLength(0);
public uint nCols => (uint)data.GetLength(1);
public int nRows => data.GetLength(0);
public int nCols => data.GetLength(1);
public Matrix2(uint nRows, uint nCols)
public Matrix2(int nRows, int nCols)
{
this.data = new float[nRows, nCols];
}
@ -46,7 +46,7 @@ namespace LinearAlgebra
return new Matrix2(data);
}
public static Matrix2 Zero(uint nRows, uint nCols)
public static Matrix2 Zero(int nRows, int nCols)
{
return new Matrix2(nRows, nCols);
}
@ -60,11 +60,11 @@ namespace LinearAlgebra
return new Matrix2(result);
}
public static Matrix2 Identity(uint size)
public static Matrix2 Identity(int size)
{
return Diagonal(1, size);
}
public static Matrix2 Identity(uint nRows, uint nCols)
public static Matrix2 Identity(int nRows, int nCols)
{
Matrix2 m = Zero(nRows, nCols);
m.FillDiagonal(1);
@ -78,7 +78,7 @@ namespace LinearAlgebra
resultData[ix, ix] = v.data[ix];
return new Matrix2(resultData);
}
public static Matrix2 Diagonal(float f, uint size)
public static Matrix2 Diagonal(float f, int size)
{
float[,] resultData = new float[size, size];
for (int ix = 0; ix < size; ix++)
@ -87,13 +87,13 @@ namespace LinearAlgebra
}
public void FillDiagonal(Matrix1 v)
{
uint n = Math.Min(Math.Min(this.nRows, this.nCols), v.size);
int n = (int)Math.Min(Math.Min(this.nRows, this.nCols), v.size);
for (int ix = 0; ix < n; ix++)
this.data[ix, ix] = v.data[ix];
}
public void FillDiagonal(float f)
{
uint n = Math.Min(this.nRows, this.nCols);
int n = Math.Min(this.nRows, this.nCols);
for (int ix = 0; ix < n; ix++)
this.data[ix, ix] = f;
}
@ -108,9 +108,17 @@ namespace LinearAlgebra
return new Matrix2(result);
}
public Matrix1 GetRow(int rowIx) {
float[] row = new float[this.nCols];
for (int colIx = 0; colIx < this.nCols; colIx++) {
row[colIx] = this.data[rowIx, colIx];
}
return new Matrix1(row);
}
#if UNITY_5_3_OR_NEWER
public Vector3Float GetRow3(int rowIx) {
uint cols = this.nCols;
int cols = this.nCols;
Vector3Float row = new() {
x = this.data[rowIx, 0],
y = this.data[rowIx, 1],
@ -131,6 +139,14 @@ namespace LinearAlgebra
this.data[rowIx, 2] = v.z;
}
public void SwapRows(int row1, int row2) {
for (uint ix = 0; ix < this.nCols; ix++) {
float temp = this.data[row1, ix];
this.data[row1, ix] = this.data[row2, ix];
this.data[row2, ix] = temp;
}
}
public Matrix1 GetColumn(int colIx)
{
float[] column = new float[this.nRows];
@ -140,6 +156,15 @@ namespace LinearAlgebra
}
return new Matrix1(column);
}
public void SetColumn(int colIx, Matrix1 v) {
for (uint ix = 0; ix < v.size; ix++)
this.data[ix, colIx] = v.data[ix];
}
public void SetColumn(int colIx, Vector3Float v) {
this.data[0, colIx] = v.x;
this.data[1, colIx] = v.y;
this.data[2, colIx] = v.z;
}
public static bool AllClose(Matrix2 A, Matrix2 B, float atol = 1e-08f)
{
@ -302,18 +327,37 @@ namespace LinearAlgebra
return new Matrix2(result);
}
public Matrix2 GetRows(Slice slice) {
return GetRows(slice.start, slice.stop);
}
public Matrix2 GetRows(int from, int to) {
if (from < 0 || to >= this.nRows)
throw new System.ArgumentException("Slice index out of range.");
float[,] result = new float[to - from, this.nCols];
int resultRowIx = 0;
for (int rowIx = from; rowIx < to; rowIx++) {
for (int colIx = 0; colIx < this.nCols; colIx++)
result[resultRowIx, colIx] = this.data[rowIx, colIx];
resultRowIx++;
}
return new Matrix2(result);
}
public Matrix2 Slice(Slice slice)
{
return Slice(slice.start, slice.stop);
}
public Matrix2 Slice(uint from, uint to)
public Matrix2 Slice(int from, int to)
{
if (from < 0 || to >= this.nRows)
throw new System.ArgumentException("Slice index out of range.");
float[,] result = new float[to - from, this.nCols];
int resultRowIx = 0;
for (uint rowIx = from; rowIx < to; rowIx++)
for (int rowIx = from; rowIx < to; rowIx++)
{
for (int colIx = 0; colIx < this.nCols; colIx++)
{
@ -328,39 +372,42 @@ namespace LinearAlgebra
{
return Slice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop));
}
public Matrix2 Slice((uint start, uint stop) rowRange, (uint start, uint stop) colRange)
public Matrix2 Slice((int start, int stop) rowRange, (int start, int stop) colRange)
{
float[,] result = new float[rowRange.stop - rowRange.start, colRange.stop - colRange.start];
uint resultRowIx = 0;
uint resultColIx = 0;
for (uint i = rowRange.start; i < rowRange.stop; i++)
int resultRowIx = 0;
int resultColIx = 0;
for (int i = rowRange.start; i < rowRange.stop; i++)
{
for (uint j = colRange.start; j < colRange.stop; j++)
for (int j = colRange.start; j < colRange.stop; j++)
result[resultRowIx, resultColIx] = this.data[i, j];
}
return new Matrix2(result);
}
public void UpdateSlice(Slice slice, Matrix2 m)
{
public void UpdateSlice(Slice slice, Matrix2 m) {
UpdateSlice((slice.start, slice.stop), m);
}
public void UpdateSlice((int start, int stop) slice, Matrix2 m) {
// if (slice.start == slice.stop)
// Console.WriteLine("WARNING: no data is updates when start equals stop in a slice!");
int mRowIx = 0;
for (uint rowIx = slice.start; rowIx < slice.stop; rowIx++, mRowIx++)
{
for (int rowIx = slice.start; rowIx < slice.stop; rowIx++, mRowIx++) {
for (int colIx = 0; colIx < this.nCols; colIx++)
this.data[rowIx, colIx] = m.data[mRowIx, colIx];
}
}
public void UpdateSlice(Slice rowRange, Slice colRange, Matrix2 m)
{
UpdateSlice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop), m);
}
public void UpdateSlice((uint start, uint stop) rowRange, (uint start, uint stop) colRange, Matrix2 m)
public void UpdateSlice((int start, int stop) rowRange, (int start, int stop) colRange, Matrix2 m)
{
for (uint i = rowRange.start; i < rowRange.stop; i++)
for (int i = rowRange.start; i < rowRange.stop; i++)
{
for (uint j = colRange.start; j < colRange.stop; j++)
for (int j = colRange.start; j < colRange.stop; j++)
this.data[i, j] = m.data[i - rowRange.start, j - colRange.start];
}
}
@ -369,7 +416,7 @@ namespace LinearAlgebra
{
Matrix2 A = this;
// unchecked
uint n = A.nRows;
int n = A.nRows;
// Create an identity matrix of the same size as the original matrix
float[,] augmentedMatrix = new float[n, 2 * n];
@ -404,10 +451,10 @@ namespace LinearAlgebra
}
// Back substitution
for (uint i = n - 1; i >= 0; i--)
for (int i = n - 1; i >= 0; i--)
{
// Eliminate the column above the pivot
for (uint j = i - 1; j >= 0; j--)
for (int j = i - 1; j >= 0; j--)
{
float factor = augmentedMatrix[j, i];
for (int k = 0; k < 2 * n; k++)
@ -428,7 +475,7 @@ namespace LinearAlgebra
public float Determinant()
{
uint n = this.nRows;
int n = this.nRows;
if (n != this.nCols)
throw new System.ArgumentException("Matrix must be square.");
@ -448,7 +495,7 @@ namespace LinearAlgebra
// Helper function to compute the minor of a matrix
private Matrix2 Minor(int rowToRemove, int colToRemove)
{
uint n = this.nRows;
int n = this.nRows;
float[,] minor = new float[n - 1, n - 1];
int r = 0, c = 0;
@ -469,15 +516,46 @@ namespace LinearAlgebra
return new Matrix2(minor);
}
public static Matrix2 DeleteRows(Matrix2 A, Slice rowRange) {
float[,] result = new float[A.nRows - (rowRange.stop - rowRange.start), A.nCols];
int resultRowIx = 0;
for (int i = 0; i < A.nRows; i++) {
if (i >= rowRange.start && i < rowRange.stop)
continue;
for (int j = 0; j < A.nCols; j++)
result[resultRowIx, j] = A.data[i, j];
resultRowIx++;
}
return new Matrix2(result);
}
internal static Matrix2 DeleteColumns(Matrix2 A, Slice colRange) {
float[,] result = new float[A.nRows, A.nCols - (colRange.stop - colRange.start)];
for (int i = 0; i < A.nRows; i++) {
int resultColIx = 0;
for (int j = 0; j < A.nCols; j++) {
if (j >= colRange.start && j < colRange.stop)
continue;
result[i, resultColIx++] = A.data[i, j];
}
}
return new Matrix2(result);
}
}
public class Matrix1
{
public float[] data { get; }
public uint size => (uint)data.GetLength(0);
public int size => data.GetLength(0);
public Matrix1(uint size)
public Matrix1(int size)
{
this.data = new float[size];
}
@ -487,7 +565,7 @@ namespace LinearAlgebra
this.data = data;
}
public static Matrix1 Zero(uint size)
public static Matrix1 Zero(int size)
{
return new Matrix1(size);
}
@ -604,6 +682,18 @@ namespace LinearAlgebra
return result;
}
public static Matrix1 operator -(Matrix1 A, Matrix1 B) {
if (A.size != B.size)
throw new System.ArgumentException("Size of A must match size of B.");
float[] result = new float[A.size];
for (int i = 0; i < A.size; i++) {
result[i] = A.data[i] - B.data[i];
}
return new Matrix1(result);
}
public static Matrix1 operator *(Matrix1 A, float f)
{
float[] result = new float[A.size];
@ -618,18 +708,35 @@ namespace LinearAlgebra
return A * f;
}
public static Matrix1 operator /(Matrix1 A, float f) {
float[] result = new float[A.size];
for (int i = 0; i < A.size; i++)
result[i] = A.data[i] / f;
return new Matrix1(result);
}
public static Matrix1 operator /(float f, Matrix1 A) {
float[] result = new float[A.size];
for (int i = 0; i < A.size; i++)
result[i] = f / A.data[i];
return new Matrix1(result);
}
public Matrix1 Slice(Slice range)
{
return Slice(range.start, range.stop);
}
public Matrix1 Slice(uint from, uint to)
public Matrix1 Slice(int from, int to)
{
if (from < 0 || to >= this.size)
throw new System.ArgumentException("Slice index out of range.");
float[] result = new float[to - from];
int resultIx = 0;
for (uint ix = from; ix < to; ix++)
for (int ix = from; ix < to; ix++)
result[resultIx++] = this.data[ix];
return new Matrix1(result);
@ -637,7 +744,7 @@ namespace LinearAlgebra
public void UpdateSlice(Slice slice, Matrix1 v)
{
int vIx = 0;
for (uint ix = slice.start; ix < slice.stop; ix++, vIx++)
for (int ix = slice.start; ix < slice.stop; ix++, vIx++)
this.data[ix] = v.data[vIx];
}
}

14
LinearAlgebra/src/Quaternion.cs
View File

@ -3,24 +3,26 @@ using System;
using Quaternion = UnityEngine.Quaternion;
#endif
namespace LinearAlgebra
{
namespace LinearAlgebra {
public class QuaternionOf<T>
{
public class QuaternionOf<T> {
public T x;
public T y;
public T z;
public T w;
public QuaternionOf(T x, T y, T z, T w)
{
public QuaternionOf(T x, T y, T z, T w) {
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
public static Quaternion Reflect(Quaternion q) {
return new(-q.x, -q.y, -q.z, q.w);
}
#if UNITY_5_3_OR_NEWER
public static Matrix2 ToRotationMatrix(Quaternion q) {
float w = q.x, x = q.y, y = q.z, z = q.w;

1
Unity/SiteServer.cs
View File

@ -18,6 +18,7 @@ namespace RoboidControl.Unity {
}
void OnApplicationQuit() {
if (site != null)
site.Close();
}

4
Unity/Thing.cs
View File

@ -31,7 +31,7 @@ namespace RoboidControl.Unity {
core.component = this;
SiteServer siteServer = FindAnyObjectByType<SiteServer>();
if (siteServer == null) {
if (siteServer == null || siteServer.site == null) {
Debug.LogWarning("No site server found");
return;
}
@ -93,7 +93,7 @@ namespace RoboidControl.Unity {
}
private void PoseChanged() {
Debug.Log($"{this} pose changed");
// Debug.Log($"{this} pose changed");
if (core.positionUpdated)
this.transform.localPosition = core.position.ToVector3();
if (core.orientationUpdated)

1
Unity/TouchSensor.cs
View File

@ -1,5 +1,4 @@
#if UNITY_5_3_OR_NEWER
using Unity.VisualScripting;
using UnityEngine;
namespace RoboidControl.Unity {

41
src/Messages/BinaryMsg.cs
View File

@ -12,7 +12,7 @@ namespace RoboidControl {
/// The length of the message, excluding the binary data
/// </summary>
/// For the total size of the message this.bytes.Length should be added to this value.
public const byte length = 2;
public const byte length = 4;
/// <summary>
/// The network ID identifying the thing
/// </summary>
@ -23,23 +23,15 @@ namespace RoboidControl {
public byte thingId;
public Thing thing;
/// <summary>
/// The length of the data
/// </summary>
public byte dataLength;
/// <summary>
/// The binary data
/// </summary>
public byte[] bytes;
public byte[] data;
/// <summary>
/// Create a new message for sending
/// </summary>
/// <param name="networkId">The netowork ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="bytes">The binary data for the thing</param>
public BinaryMsg(byte networkId, byte thingId, byte[] bytes) : base() {
this.networkId = networkId;
this.thingId = thingId;
this.bytes = bytes;
}
/// <summary>
/// Create an empty message for sending
/// </summary>
@ -49,31 +41,34 @@ namespace RoboidControl {
this.networkId = networkId;
this.thingId = thing.id;
this.thing = thing;
this.bytes = this.thing.GenerateBinary();
// if (this.bytes.Length > 0)
// System.Console.Write($"Binary message for [{networkId}/{thing.id}]");
this.data = this.thing.GenerateBinary();
this.dataLength = (byte)this.data.Length;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public BinaryMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
byte length = (byte)(buffer.Length - ix);
this.bytes = new byte[length];
for (uint bytesIx = 0; bytesIx < length; bytesIx++)
this.bytes[bytesIx] = buffer[ix++];
this.dataLength = buffer[ix++];
this.data = new byte[this.dataLength];
for (uint dataIx = 0; dataIx < this.dataLength; dataIx++)
this.data[dataIx] = buffer[ix++];
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < BinaryMsg.length + this.bytes.Length || this.bytes.Length == 0)
if (buffer.Length < BinaryMsg.length + this.data.Length || this.data.Length == 0)
return 0;
#if DEBUG
System.Console.WriteLine($"Send BinaryMsg [{this.networkId}/{this.thingId}] {this.dataLength}");
#endif
byte ix = 0;
buffer[ix++] = BinaryMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
foreach (byte b in bytes)
buffer[ix++] = this.dataLength;
foreach (byte b in data)
buffer[ix++] = b;
return ix;

25
src/Messages/ModelUrlMsg.cs
View File

@ -21,6 +21,10 @@ namespace RoboidControl {
/// </summary>
public byte thingId;
/// <summary>
/// The length of the url string, excluding the null terminator
/// </summary>
public byte urlLength;
/// <summary>
/// The URL of the model
/// </summary>
public string url = null;
@ -33,6 +37,7 @@ namespace RoboidControl {
public ModelUrlMsg(byte networkId, Thing thing) {
this.networkId = networkId;
this.thingId = thing.id;
this.urlLength = (byte)thing.modelUrl.Length;
this.url = thing.modelUrl;
}
/// <summary>
@ -41,26 +46,30 @@ namespace RoboidControl {
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="url">The URL to send</param>
public ModelUrlMsg(byte networkId, byte thingId, string url) {
this.networkId = networkId;
this.thingId = thingId;
this.url = url;
}
// public ModelUrlMsg(byte networkId, byte thingId, string url) {
// this.networkId = networkId;
// this.thingId = thingId;
// this.urlLength = (byte)url.Length;
// this.url = url;
// }
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public ModelUrlMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
int strlen = buffer[ix++];
url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
this.urlLength = buffer[ix++];
this.url = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, this.urlLength);
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (this.url == null)
if (string.IsNullOrEmpty(this.url))
return 0;
#if DEBUG
System.Console.WriteLine($"Send ModelUrlMsg [{this.networkId}/{this.thingId}] {this.urlLength} {this.url}");
#endif
byte ix = 0;
buffer[ix++] = ModelUrlMsg.Id;
buffer[ix++] = this.networkId;

30
src/Messages/NameMsg.cs
View File

@ -20,10 +20,11 @@ namespace RoboidControl {
/// The ID of the thing
/// </summary>
public byte thingId;
/// <summary>
/// The length of the name, excluding null terminator
/// </summary>
public byte len;
public byte nameLength;
/// <summary>
/// The name of the thing, not terminated with a null character
/// </summary>
@ -38,6 +39,7 @@ namespace RoboidControl {
this.networkId = networkId;
this.thingId = thing.id;
this.name = thing.name;
this.nameLength = (byte)this.name.Length;
}
/// <summary>
/// Create a new message for sending
@ -45,34 +47,34 @@ namespace RoboidControl {
/// <param name="networkId">The network ID of the thing</param>
/// <param name="thingId">The ID of the thing</param>
/// <param name="name">The name of the thing</param>
public NameMsg(byte networkId, byte thingId, string name) {
this.networkId = networkId;
this.thingId = thingId;
this.name = name;
}
// public NameMsg(byte networkId, byte thingId, string name) {
// this.networkId = networkId;
// this.thingId = thingId;
// this.name = name;
// }
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public NameMsg(byte[] buffer) {
byte ix = 1;
this.networkId = buffer[ix++];
this.thingId = buffer[ix++];
int strlen = buffer[ix++];
this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, strlen);
this.nameLength = buffer[ix++];
this.name = System.Text.Encoding.UTF8.GetString(buffer, (int)ix, this.nameLength);
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < NameMsg.length + this.name.Length || this.name.Length == 0)
if (buffer.Length < NameMsg.length + this.name.Length || string.IsNullOrEmpty(this.name))
return 0;
#if DEBUG
System.Console.WriteLine($"Send NameMsg [{this.networkId}/{this.thingId}] {this.nameLength} {this.name}");
#endif
byte ix = 0;
buffer[ix++] = NameMsg.Id;
buffer[ix++] = this.networkId;
buffer[ix++] = this.thingId;
int nameLength = this.name.Length;
buffer[ix++] = (byte)nameLength;
for (int nameIx = 0; nameIx < nameLength; nameIx++)
buffer[ix++] = this.nameLength;
for (int nameIx = 0; nameIx < this.nameLength; nameIx++)
buffer[ix++] = (byte)this.name[nameIx];
return ix;

3
src/Messages/NetworkIdMsg.cs
View File

@ -34,6 +34,9 @@ namespace RoboidControl {
if (buffer.Length < NetworkIdMsg.length)
return 0;
#if DEBUG
System.Console.WriteLine($"Send NetworkIdMsg {this.networkId}");
#endif
buffer[0] = NetworkIdMsg.Id;
buffer[1] = this.networkId;
return NetworkIdMsg.length;

28
src/Messages/PoseMsg.cs
View File

@ -88,6 +88,34 @@ namespace RoboidControl {
this.linearVelocity = linearVelocity;
this.angularVelocity = angularVelocity;
}
public PoseMsg(byte networkId, Thing thing, bool force = false) {
this.networkId = networkId;
this.thingId = thing.id;
this.poseType = 0;
if (thing.positionUpdated || force) {
this.position = thing.position;
this.poseType |= Pose_Position;
//thing.positionUpdated = false; // this is also reset in Thing.update, leave it out here?
}
if (thing.orientationUpdated || force) {
this.orientation = thing.orientation;
this.poseType |= Pose_Orientation;
//thing.orientationUpdated = false; // this is also reset in Thing.update, leave it out here?
}
if (thing.linearVelocityUpdated) {
this.linearVelocity = thing.linearVelocity;
this.poseType |= Pose_LinearVelocity;
thing.linearVelocityUpdated = false;
}
if (thing.angularVelocityUpdated) {
this.angularVelocity = thing.angularVelocity;
this.poseType |= Pose_AngularVelocity;
thing.angularVelocityUpdated = false;
}
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public PoseMsg(byte[] buffer) : base(buffer) {
byte ix = 1;

15
src/Messages/TextMsg.cs
View File

@ -13,6 +13,10 @@ namespace RoboidControl {
/// </summary>
public const byte length = 2;
/// <summary>
/// The length of the text without the null terminator
/// </summary>
public byte textLength;
/// <summary>
/// The text
/// </summary>
public string text = "";
@ -22,20 +26,27 @@ namespace RoboidControl {
/// </summary>
/// <param name="text">The text to send</param>
public TextMsg(string text) {
this.textLength = (byte)text.Length;
this.text = text;
}
/// @copydoc Passer::RoboidControl::IMessage::IMessage(byte[] buffer)
public TextMsg(byte[] buffer) : base(buffer) { }
public TextMsg(byte[] buffer) : base(buffer) {
this.textLength = buffer[0];
this.text = System.Text.Encoding.UTF8.GetString(buffer, 1, this.textLength);
}
/// @copydoc Passer::RoboidControl::IMessage::Serialize
public override byte Serialize(ref byte[] buffer) {
if (buffer.Length < TextMsg.length + this.text.Length || this.text.Length == 0)
return 0;
#if DEBUG
System.Console.WriteLine($"Send TextMsg {this.textLength} {this.text}");
#endif
byte ix = 0;
buffer[ix++] = TextMsg.Id;
buffer[ix++] = (byte)this.text.Length;
buffer[ix++] = this.textLength;
for (int textIx = 0; textIx < this.text.Length; textIx++)
buffer[ix++] = (byte)this.text[textIx];
return ix;

3
src/Messages/ThingMsg.cs
View File

@ -77,6 +77,9 @@ namespace RoboidControl {
if (buffer.Length < ThingMsg.length)
return 0;
#if DEBUG
System.Console.WriteLine($"Send ThingMsg [{this.networkId}/{this.thingId}] {this.thingType} {this.parentId}");
#endif
byte ix = 0;
buffer[ix++] = ThingMsg.id;
buffer[ix++] = this.networkId;

10
src/Participant.cs
View File

@ -44,6 +44,16 @@ namespace RoboidControl {
/// </summary>
protected readonly List<Thing> things = new List<Thing>();
public virtual void Update(ulong currentTimeMS = 0) {
int n = this.things.Count;
for (int ix = 0; ix < n; ix++) {
Thing thing = this.things[ix];
if (thing != null)
thing.Update(currentTimeMS, true);
}
}
/// <summary>
/// Get a thing with the given ids
/// </summary>

137
src/LocalParticipant.cs → src/ParticipantUDP.cs
View File

@ -9,7 +9,7 @@ namespace RoboidControl {
/// <summary>
/// A participant is used for communcation between things
/// </summary>
public class LocalParticipant : Participant {
public class ParticipantUDP : Participant {
public byte[] buffer = new byte[1024];
public ulong publishInterval = 3000; // = 3 seconds
@ -26,7 +26,7 @@ namespace RoboidControl {
/// <summary>
/// Create a porticiapnt
/// </summary>
public LocalParticipant() {
public ParticipantUDP() {
//senders.Add(this);
}
@ -34,7 +34,7 @@ namespace RoboidControl {
/// Create a participant with the give UDP port
/// </summary>
/// <param name="port">The port number on which to communicate</param>
public LocalParticipant(int port) : this() {
public ParticipantUDP(int port) : this() {
this.port = port;
}
@ -43,7 +43,7 @@ namespace RoboidControl {
/// </summary>
/// <param name="ipAddress">The ip address of the site server</param>
/// <param name="port">The port number of the site server</param>
public LocalParticipant(string ipAddress = "0.0.0.0", int port = 7681) : this() {
public ParticipantUDP(string ipAddress = "0.0.0.0", int port = 7681) : this() {
this.ipAddress = ipAddress;
this.port = port;
@ -61,15 +61,15 @@ namespace RoboidControl {
/// </summary>
/// <param name="udpClient">UDP client to use for communication</param>
/// <param name="port">The port number on which to communicate</param>
public LocalParticipant(UdpClient udpClient, int port) : this() {
public ParticipantUDP(UdpClient udpClient, int port) : this() {
this.udpClient = udpClient;
this.port = port;
}
private static LocalParticipant isolatedParticipant = null;
public static LocalParticipant Isolated() {
private static ParticipantUDP isolatedParticipant = null;
public static ParticipantUDP Isolated() {
if (isolatedParticipant == null)
isolatedParticipant = new LocalParticipant(0);
isolatedParticipant = new ParticipantUDP(0);
return isolatedParticipant;
}
@ -94,20 +94,20 @@ namespace RoboidControl {
protected readonly Dictionary<byte, Func<Participant, byte, byte, Thing>> thingMsgProcessors = new();
public delegate Thing ThingConstructor(Participant sender, byte networkId, byte thingId);
public void Register(byte thingType, ThingConstructor constr) {
thingMsgProcessors[thingType] = new Func<Participant, byte, byte, Thing>(constr);
}
// public delegate Thing ThingConstructor(Participant sender, byte networkId, byte thingId);
// public void Register(byte thingType, ThingConstructor constr) {
// thingMsgProcessors[thingType] = new Func<Participant, byte, byte, Thing>(constr);
// }
public void Register<ThingClass>(Thing.Type thingType) where ThingClass : Thing {
Register<ThingClass>((byte)thingType);
}
// public void Register<ThingClass>(Thing.Type thingType) where ThingClass : Thing {
// Register<ThingClass>((byte)thingType);
// }
public void Register<ThingClass>(byte thingType) where ThingClass : Thing {
thingMsgProcessors[thingType] = (participant, networkId, thingId) =>
Activator.CreateInstance(typeof(ThingClass), participant, networkId, thingId) as ThingClass;
Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}");
}
// public void Register<ThingClass>(byte thingType) where ThingClass : Thing {
// thingMsgProcessors[thingType] = (participant, networkId, thingId) =>
// Activator.CreateInstance(typeof(ThingClass), participant, networkId, thingId) as ThingClass;
// Console.WriteLine($"Registering {typeof(ThingClass)} for thing type {thingType}");
// }
#endregion Init
@ -134,7 +134,7 @@ namespace RoboidControl {
}
protected ulong nextPublishMe = 0;
public virtual void Update(ulong currentTimeMS = 0) {
public override void Update(ulong currentTimeMS = 0) {
if (currentTimeMS == 0) {
#if UNITY_5_3_OR_NEWER
currentTimeMS = (ulong)(UnityEngine.Time.time * 1000);
@ -152,13 +152,20 @@ namespace RoboidControl {
Thing thing = this.things[ix];
if (thing != null && thing.parent == null) {
thing.Update(currentTimeMS, true);
// if (thing.owner != this) {
// BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
// this.Send(thing.owner, binaryMsg);
// }
// if (this.isIsolated == false) {
if (thing.owner != this) { // should not happen....
PoseMsg poseMsg = new(thing.owner.networkId, thing);
this.Send(thing.owner, poseMsg);
BinaryMsg binaryMsg = new(thing.owner.networkId, thing);
this.Send(thing.owner, binaryMsg);
}
}
}
for (int ownerIx = 0; ownerIx < this.owners.Count; ownerIx++) {
Participant owner = this.owners[ownerIx];
owner.Update(currentTimeMS);
}
}
public virtual void Publish() {
this.Publish(new ParticipantMsg(this.networkId));
@ -169,7 +176,7 @@ namespace RoboidControl {
#region Send
public void SendThingInfo(Participant owner, Thing thing) {
Console.WriteLine("Send thing info");
// Console.WriteLine("Send thing info");
this.Send(owner, new ThingMsg(this.networkId, thing));
this.Send(owner, new NameMsg(this.networkId, thing));
this.Send(owner, new ModelUrlMsg(this.networkId, thing));
@ -200,7 +207,7 @@ namespace RoboidControl {
if (bufferSize <= 0)
return true;
// Console.WriteLine($"publish to {broadcastIpAddress.ToString()} {this.port}");
Console.WriteLine($"publish to {broadcastIpAddress.ToString()} {this.port}");
this.udpClient?.Send(this.buffer, bufferSize, this.broadcastIpAddress, this.port);
return true;
}
@ -227,7 +234,7 @@ namespace RoboidControl {
#region Receive
public void ReceiveData(byte[] data, Participant remoteParticipant) {
public void ReceiveData(byte[] data, Participant sender) {
byte msgId = data[0];
if (msgId == 0xFF) {
// Timeout
@ -236,29 +243,29 @@ namespace RoboidControl {
switch (msgId) {
case ParticipantMsg.Id: // 0xA0 / 160
this.Process(remoteParticipant, new ParticipantMsg(data));
this.Process(sender, new ParticipantMsg(data));
break;
case NetworkIdMsg.Id: // 0xA1 / 161
this.Process(remoteParticipant, new NetworkIdMsg(data));
this.Process(sender, new NetworkIdMsg(data));
break;
case InvestigateMsg.Id: // 0x81
// result = await InvestigateMsg.Receive(dataStream, client, packetSize);
break;
case ThingMsg.id: // 0x80 / 128
this.Process(remoteParticipant, new ThingMsg(data));
this.Process(sender, new ThingMsg(data));
break;
case NameMsg.Id: // 0x91 / 145
this.Process(remoteParticipant, new NameMsg(data));
this.Process(sender, new NameMsg(data));
break;
case ModelUrlMsg.Id: // 0x90 / 144
this.Process(remoteParticipant, new ModelUrlMsg(data));
this.Process(sender, new ModelUrlMsg(data));
break;
case PoseMsg.Id: // 0x10 / 16
this.Process(remoteParticipant, new PoseMsg(data));
this.Process(sender, new PoseMsg(data));
// result = await PoseMsg.Receive(dataStream, client, packetSize);
break;
case BinaryMsg.Id: // 0xB1 / 177
this.Process(remoteParticipant, new BinaryMsg(data));
this.Process(sender, new BinaryMsg(data));
break;
case TextMsg.Id: // 0xB0 / 176
// result = await TextMsg.Receive(dataStream, client, packetSize);
@ -275,10 +282,17 @@ namespace RoboidControl {
#region Process
protected virtual void Process(Participant sender, ParticipantMsg msg) { }
protected virtual void Process(Participant sender, ParticipantMsg msg) {
#if DEBUG
Console.WriteLine($"{this.name} Process participantMsg {msg.networkId}");
#endif
}
protected virtual void Process(Participant sender, NetworkIdMsg msg) {
Console.WriteLine($"{this.name} receive network id {this.networkId} {msg.networkId}");
#if DEBUG
Console.WriteLine($"{this.name} Process SiteMsg {this.networkId} -> {msg.networkId}");
#endif
if (this.networkId != msg.networkId) {
this.networkId = msg.networkId;
foreach (Thing thing in this.things) //Thing.GetAllThings())
@ -286,53 +300,76 @@ namespace RoboidControl {
}
}
protected virtual void Process(Participant sender, InvestigateMsg msg) { }
protected virtual void Process(Participant sender, InvestigateMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: InvestigateMsg [{msg.networkId}/{msg.thingId}]");
#endif
}
protected virtual void Process(Participant sender, ThingMsg msg) {
//Console.WriteLine($"Participant: Process thing [{msg.networkId}/{msg.thingId}]");
#if DEBUG
Console.WriteLine($"Participant: Process ThingMsg [{msg.networkId}/{msg.thingId}] {msg.thingType} {msg.parentId}");
#endif
}
protected virtual void Process(Participant sender, NameMsg msg) {
Console.WriteLine($"Participant: Process name [{msg.networkId}/{msg.thingId}] {msg.name}");
#if DEBUG
Console.WriteLine($"Participant: Process NameMsg [{msg.networkId}/{msg.thingId}] {msg.nameLength} {msg.name}");
#endif
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null)
thing.name = msg.name;
}
protected virtual void Process(Participant sender, ModelUrlMsg msg) {
Console.WriteLine($"Participant: Process model [{msg.networkId}/{msg.thingId}] {msg.url}");
#if DEBUG
Console.WriteLine($"Participant: Process ModelUrlMsg [{msg.networkId}/{msg.thingId}] {msg.urlLength} {msg.url}");
#endif
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null)
thing.modelUrl = msg.url;
}
protected virtual void Process(Participant sender, PoseMsg msg) {
//Console.WriteLine($"Participant: Process pose [{msg.networkId}/{msg.thingId}] {msg.poseType}");
#if DEBUG
Console.WriteLine($"Participant: Process PoseMsg [{msg.networkId}/{msg.thingId}] {msg.poseType}");
#endif
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing != null) {
if ((msg.poseType & PoseMsg.Pose_Position) != 0)
thing.position = msg.position;
if ((msg.poseType & PoseMsg.Pose_Orientation) != 0) {
if ((msg.poseType & PoseMsg.Pose_Orientation) != 0)
thing.orientation = msg.orientation;
Console.Write($"{thing.id} orientation changed");
}
if ((msg.poseType & PoseMsg.Pose_LinearVelocity) != 0)
thing.linearVelocity = msg.linearVelocity;
if ((msg.poseType & PoseMsg.Pose_AngularVelocity) != 0)
thing.angularVelocity = msg.angularVelocity;
}
}
protected virtual void Process(Participant sender, BinaryMsg msg) {
// Console.WriteLine($"Participant: Process binary [{msg.networkId}/{msg.thingId}]");
#if DEBUG
Console.WriteLine($"Participant: Process BinaryMsg [{msg.networkId}/{msg.thingId}] {msg.dataLength}");
#endif
Thing thing = sender.Get(msg.networkId, msg.thingId);
thing?.ProcessBinary(msg.bytes);
thing?.ProcessBinary(msg.data);
}
protected virtual void Process(Participant sender, TextMsg temsgxt) { }
protected virtual void Process(Participant sender, TextMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: Process TextMsg {msg.textLength} {msg.text}");
#endif
protected virtual void Process(Participant sender, DestroyMsg msg) { }
}
protected virtual void Process(Participant sender, DestroyMsg msg) {
#if DEBUG
Console.WriteLine($"Participant: Process ThingMsg [{msg.networkId}/{msg.thingId}]");
#endif
}
private void ForwardMessage(IMessage msg) {
// foreach (Participant client in senders) {

29
src/SiteServer.cs
View File

@ -7,7 +7,7 @@ namespace RoboidControl {
/// <summary>
/// A site server is a participant which provides a shared simulated environment
/// </summary>
public class SiteServer : LocalParticipant {
public class SiteServer : ParticipantUDP {
public SiteServer(int port = 7681) : this("0.0.0.0", port) { }
@ -29,7 +29,7 @@ namespace RoboidControl {
new AsyncCallback(result => ReceiveUDP(result)),
new Tuple<UdpClient, IPEndPoint>(this.udpClient, new(IPAddress.Any, port)));
Register<TouchSensor>(Thing.Type.TouchSensor);
// Register<TouchSensor>(Thing.Type.TouchSensor);
}
/// <summary>
@ -42,11 +42,12 @@ namespace RoboidControl {
public override void Publish() {
}
protected override void Process(Participant remoteParticipant, ParticipantMsg msg) {
if (msg.networkId == 0) {
Console.WriteLine($"{this.name} received New Participant -> {remoteParticipant.networkId}");
this.Send(remoteParticipant, new NetworkIdMsg(remoteParticipant.networkId));
}
protected override void Process(Participant sender, ParticipantMsg msg) {
base.Process(sender, msg);
//if (msg.networkId == 0) {
//Console.WriteLine($"{this.name} received New Participant -> {sender.networkId}");
this.Send(sender, new NetworkIdMsg(sender.networkId));
//}
}
protected override void Process(Participant sender, NetworkIdMsg msg) { }
@ -56,15 +57,15 @@ namespace RoboidControl {
Thing thing = sender.Get(msg.networkId, msg.thingId);
if (thing == null) {
Thing newThing = null;
if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<Participant, byte, byte, Thing> msgProcessor)) {
//Console.WriteLine("Found thing message processor");
if (msgProcessor != null)
newThing = msgProcessor(sender, msg.networkId, msg.thingId);
}
if (newThing == null) {
// if (thingMsgProcessors.TryGetValue(msg.thingType, out Func<Participant, byte, byte, Thing> msgProcessor)) {
// //Console.WriteLine("Found thing message processor");
// if (msgProcessor != null)
// newThing = msgProcessor(sender, msg.networkId, msg.thingId);
// }
// if (newThing == null) {
newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType);
// Console.WriteLine("Created generic new core thing");
}
// }
if (msg.parentId != 0) {
Thing parentThing = Get(msg.networkId, msg.parentId);
if (parentThing == null)

15
src/Thing.cs
View File

@ -49,6 +49,7 @@ namespace RoboidControl {
public Thing(Participant owner, byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) {
this.owner = owner;
this.type = thingType;
if (this.owner != null)
this.owner.Add(this);
if (invokeEvent)
InvokeNewThing(this);
@ -65,7 +66,7 @@ namespace RoboidControl {
/// Create a new thing without communication abilities
/// </summary>
/// <param name="thingType">The type of thing</param>
public Thing(byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) : this(LocalParticipant.Isolated(), thingType, invokeEvent) {
public Thing(byte thingType = (byte)Type.Undetermined, bool invokeEvent = true) : this(ParticipantUDP.Isolated(), thingType, invokeEvent) {
}
/// <summary>
@ -286,10 +287,6 @@ namespace RoboidControl {
}
}
/// <summary>
/// Event triggered when the orientation has changed
/// </summary>
//public event ChangeHandler OnOrientationChanged = delegate { };
/// <summary>
/// Boolean indicating the thing has an updated orientation
/// </summary>
public bool orientationUpdated = false;
@ -303,7 +300,7 @@ namespace RoboidControl {
set {
if (_linearVelocity != value) {
_linearVelocity = value;
hasLinearVelocity = _linearVelocity.distance != 0;
linearVelocityUpdated = true;
OnLinearVelocityChanged?.Invoke(_linearVelocity);
}
}
@ -315,7 +312,7 @@ namespace RoboidControl {
/// <summary>
/// Boolean indicating the thing has an updated linear velocity
/// </summary>
public bool hasLinearVelocity = false;
public bool linearVelocityUpdated = false;
private Spherical _angularVelocity = Spherical.zero;
/// <summary>
@ -326,7 +323,7 @@ namespace RoboidControl {
set {
if (_angularVelocity != value) {
_angularVelocity = value;
hasAngularVelocity = _angularVelocity.distance != 0;
angularVelocityUpdated = true;
OnAngularVelocityChanged?.Invoke(_angularVelocity);
}
}
@ -338,7 +335,7 @@ namespace RoboidControl {
/// <summary>
/// Boolean indicating the thing has an updated angular velocity
/// </summary>
public bool hasAngularVelocity = false;
public bool angularVelocityUpdated = false;
#if UNITY_5_3_OR_NEWER
/// <summary>

2
src/Things/DifferentialDrive.cs
View File

@ -8,7 +8,7 @@ namespace RoboidControl {
public DifferentialDrive() { }
/// @brief Create a differential drive with networking support
/// @param participant The local participant
public DifferentialDrive(LocalParticipant participant) : base(participant, Type.Undetermined) { }
public DifferentialDrive(ParticipantUDP participant) : base(participant, Type.Undetermined) { }
/// @brief Configures the dimensions of the drive
/// @param wheelDiameter The diameter of the wheels in meters

2
src/Things/TemperatureSensor.cs
View File

@ -17,7 +17,7 @@ namespace RoboidControl {
/// <param name="participant">The participant for with the sensor is needed</param>
/// <param name="networkId">The network ID of the sensor</param>
/// <param name="thingId">The ID of the thing</param>
public TemperatureSensor(LocalParticipant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) { }
public TemperatureSensor(Participant participant, byte networkId, byte thingId) : base(participant, networkId, thingId, (byte)Type.TemperatureSensor) { }
/// <summary>
/// Function to extract the temperature received in the binary message

2
src/Things/TouchSensor.cs
View File

@ -26,7 +26,7 @@ namespace RoboidControl {
public TouchSensor(Thing parent) : base(parent) { }
public LocalParticipant thisParticipant;
public ParticipantUDP thisParticipant;
/// <summary>
/// Value which is true when the sensor is touching something, false otherwise