From d82388fc453f05d70638684174078c2533c238f2 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 7 Apr 2025 09:36:22 +0200 Subject: [PATCH] Migrating from ControlCore to RoboidControl --- Examples/BB2B/Program.cs | 31 ------ LinearAlgebra/src/Matrix.cs | 185 +++++++++++++++++++++++++------- LinearAlgebra/src/Quaternion.cs | 14 +-- Unity/TouchSensor.cs | 1 - src/Things/TemperatureSensor.cs | 2 +- 5 files changed, 155 insertions(+), 78 deletions(-) delete mode 100644 Examples/BB2B/Program.cs diff --git a/Examples/BB2B/Program.cs b/Examples/BB2B/Program.cs deleted file mode 100644 index d4a57f2..0000000 --- a/Examples/BB2B/Program.cs +++ /dev/null @@ -1,31 +0,0 @@ -using System.Threading; -using RoboidControl; - -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); - } - } -} \ No newline at end of file diff --git a/LinearAlgebra/src/Matrix.cs b/LinearAlgebra/src/Matrix.cs index 1f0542e..8ee1d0d 100644 --- a/LinearAlgebra/src/Matrix.cs +++ b/LinearAlgebra/src/Matrix.cs @@ -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]; } } diff --git a/LinearAlgebra/src/Quaternion.cs b/LinearAlgebra/src/Quaternion.cs index f481ef4..cd7e9f4 100644 --- a/LinearAlgebra/src/Quaternion.cs +++ b/LinearAlgebra/src/Quaternion.cs @@ -3,24 +3,26 @@ using System; using Quaternion = UnityEngine.Quaternion; #endif -namespace LinearAlgebra -{ +namespace LinearAlgebra { - public class QuaternionOf - { + public class QuaternionOf { 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; diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs index 2859fc4..bcce86e 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -1,5 +1,4 @@ #if UNITY_5_3_OR_NEWER -using Unity.VisualScripting; using UnityEngine; namespace RoboidControl.Unity { diff --git a/src/Things/TemperatureSensor.cs b/src/Things/TemperatureSensor.cs index 6f7e009..34a7233 100644 --- a/src/Things/TemperatureSensor.cs +++ b/src/Things/TemperatureSensor.cs @@ -17,7 +17,7 @@ namespace RoboidControl { /// The participant for with the sensor is needed /// The network ID of the sensor /// The ID of the thing - 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) { } /// /// Function to extract the temperature received in the binary message