Migrating from ControlCore to RoboidControl

Examples/BB2B/Program.cs

@@ -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);
-        }
-    }
-}

LinearAlgebra/src/Matrix.cs

@@ -10,9 +10,9 @@ namespace LinearAlgebra
 
     public readonly struct Slice
     {
-        public uint start { get; }
+        public int start { get; }
-        public uint stop { get; }
+        public int stop { get; }
-        public Slice(uint start, uint stop)
+        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 int nRows => data.GetLength(0);
-        public uint nCols => (uint)data.GetLength(1);
+        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((int start, int stop) rowRange, (int start, int stop) colRange)
-        public Matrix2 Slice((uint start, uint stop) rowRange, (uint start, uint stop) colRange)
         {
             float[,] result = new float[rowRange.stop - rowRange.start, colRange.stop - colRange.start];
 
-            uint resultRowIx = 0;
+            int resultRowIx = 0;
-            uint resultColIx = 0;
+            int resultColIx = 0;
-            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++)
                     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];
         }
     }

LinearAlgebra/src/Quaternion.cs

@@ -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;

Unity/TouchSensor.cs

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

src/Things/TemperatureSensor.cs

@@ -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