From 8a709e0fd275a058241f476393d48946edd94edd Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 14 Feb 2025 13:18:23 +0100 Subject: [PATCH 1/7] Fix all matrix errors --- LinearAlgebra/Matrix.cs | 256 +++++++++++------------------------- LinearAlgebra/Quaternion.cs | 5 +- 2 files changed, 80 insertions(+), 181 deletions(-) diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs index c2e6334..b5f7e9d 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -4,8 +4,8 @@ using Vector3 = UnityEngine.Vector3; using Vector2 = UnityEngine.Vector2; public readonly struct Slice { - public int start {get;} - public int stop {get;} + public int start { get; } + public int stop { get; } public Slice(int start, int stop) { this.start = start; this.stop = stop; @@ -25,6 +25,15 @@ public class Matrix2 { this.data = data; } + public Matrix2 Clone() { + float[,] data = new float[this.nRows, nCols]; + for (int rowIx = 0; rowIx < this.nRows; rowIx++) { + for (int colIx = 0; colIx < this.nCols; colIx++) + data[rowIx, colIx] = this.data[rowIx, colIx]; + } + return new Matrix2(data); + } + public static Matrix2 Zero(uint nRows, uint nCols) { return new Matrix2(nRows, nCols); } @@ -59,6 +68,30 @@ public class Matrix2 { return new Matrix2(result); } + public Vector3 GetRow3(int rowIx) { + uint cols = this.nCols; + Vector3 row = new() { + x = this.data[rowIx, 0], + y = this.data[rowIx, 1], + z = this.data[rowIx, 2] + }; + return row; + } + public void SetRow3(int rowIx, Vector3 v) { + this.data[rowIx, 0] = v.x; + this.data[rowIx, 1] = v.y; + this.data[rowIx, 2] = v.z; + } + + public Matrix1 GetColumn(int colIx) { + float[] column = new float[this.nRows]; + for (int i = 0; i < this.nRows; i++) { + column[i] = this.data[i, colIx]; + } + return new Matrix1(column); + } + + public Matrix2 Transpose() { float[,] resultData = new float[this.nCols, this.nRows]; for (uint rowIx = 0; rowIx < this.nRows; rowIx++) { @@ -145,7 +178,6 @@ public class Matrix2 { }; } - public static Matrix2 operator *(Matrix2 A, float s) { float[,] result = new float[A.nRows, A.nCols]; @@ -244,6 +276,48 @@ public class Matrix2 { return new Matrix2(inverse); } + + public float Determinant() { + uint n = this.nRows; + if (n != this.nCols) + throw new System.ArgumentException("Matrix must be square."); + + if (n == 1) + return this.data[0, 0]; // Base case for 1x1 matrix + + if (n == 2) // Base case for 2x2 matrix + return this.data[0, 0] * this.data[1, 1] - this.data[0, 1] * this.data[1, 0]; + + float det = 0; + for (int col = 0; col < n; col++) + det += (col % 2 == 0 ? 1 : -1) * this.data[0, col] * this.Minor(0, col).Determinant(); + + return det; + } + + // Helper function to compute the minor of a matrix + private Matrix2 Minor(int rowToRemove, int colToRemove) { + uint n = this.nRows; + float[,] minor = new float[n - 1, n - 1]; + + int r = 0, c = 0; + for (int i = 0; i < n; i++) { + if (i == rowToRemove) continue; + + c = 0; + for (int j = 0; j < n; j++) { + if (j == colToRemove) continue; + + minor[r, c] = this.data[i, j]; + c++; + } + r++; + } + + return new Matrix2(minor); + } + + } public class Matrix1 { @@ -343,180 +417,4 @@ public class Matrix1 { for (int ix = slice.start; ix < slice.stop; ix++, vIx++) this.data[ix] = v.data[vIx]; } -} - -public class Matrix { - private readonly uint rows = 0; - private readonly uint cols = 0; - private float[] data; - - public Matrix(uint rows, uint cols) { - this.rows = rows; - this.cols = cols; - } - - public static float[,] Diagonal(float[] v) { - float[,] r = new float[v.Length, v.Length]; - for (int i = 0; i < v.Length; i++) { - r[i, i] = v[i]; - } - return r; - } - - public static float[,] Transpose(float[,] m) { - int rows = m.GetLength(0); - int cols = m.GetLength(1); - float[,] r = new float[cols, rows]; - for (uint rowIx = 0; rowIx < rows; rowIx++) { - for (uint colIx = 0; colIx < cols; colIx++) - r[colIx, rowIx] = m[rowIx, colIx]; - } - return r; - // double checked code - } - - public static void NegateColumn(float[,] m, uint colIx) { - for (uint rowIx = 0; rowIx < m.GetLength(0); rowIx++) { - m[rowIx, colIx] = -m[rowIx, colIx]; - } - } - - public static float Determinant(float[,] matrix) { - int n = matrix.GetLength(0); - if (n != matrix.GetLength(1)) - throw new System.ArgumentException("Matrix must be square."); - - if (n == 1) - return matrix[0, 0]; // Base case for 1x1 matrix - - if (n == 2) // Base case for 2x2 matrix - return matrix[0, 0] * matrix[1, 1] - matrix[0, 1] * matrix[1, 0]; - - float det = 0; - for (int col = 0; col < n; col++) - det += (col % 2 == 0 ? 1 : -1) * matrix[0, col] * Determinant(Minor(matrix, 0, col)); - - return det; - } - - // Helper function to compute the minor of a matrix - private static float[,] Minor(float[,] matrix, int rowToRemove, int colToRemove) { - int n = matrix.GetLength(0); - float[,] minor = new float[n - 1, n - 1]; - - int r = 0, c = 0; - for (int i = 0; i < n; i++) { - if (i == rowToRemove) continue; - - c = 0; - for (int j = 0; j < n; j++) { - if (j == colToRemove) continue; - - minor[r, c] = matrix[i, j]; - c++; - } - r++; - } - - return minor; - } - - public static float[,] MultiplyMatrices(float[,] A, float[,] B) { - int rowsA = A.GetLength(0); - int colsA = A.GetLength(1); - int rowsB = B.GetLength(0); - int colsB = B.GetLength(1); - - if (colsA != rowsB) - throw new System.ArgumentException("Number of columns in A must match number of rows in B."); - - float[,] result = new float[rowsA, colsB]; - - for (int i = 0; i < rowsA; i++) { - for (int j = 0; j < colsB; j++) { - float sum = 0.0f; - for (int k = 0; k < colsA; k++) - sum += A[i, k] * B[k, j]; - - result[i, j] = sum; - } - } - - return result; - // double checked code - } - - public static float[] MultiplyMatrixVector(float[,] A, float[] v) { - int rows = A.GetLength(0); - int cols = A.GetLength(1); - float[] result = new float[rows]; - - for (int i = 0; i < rows; i++) { - for (int j = 0; j < cols; j++) { - result[i] += A[i, j] * v[j]; - } - } - - return result; - } - - // Vector-matrix multiplication - public static Vector3 MultiplyMatrixVector3(float[,] A, Vector3 v) { - return new Vector3() { - x = 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, - z = A[2, 0] * v.x + A[2, 1] * v.y + A[2, 2] * v.z - }; - } - - public static float[,] MultiplyMatrixScalar(float[,] A, float s) { - int rows = A.GetLength(0); - int cols = A.GetLength(1); - float[,] result = new float[rows, cols]; - - for (int i = 0; i < rows; i++) { - for (int j = 0; j < cols; j++) { - result[i, j] += A[i, j] * s; - } - } - - return result; - } - - public static float[] GetColumn(float[,] M, int col) { - int rows = M.GetLength(0); - float[] column = new float[rows]; - for (int i = 0; i < rows; i++) { - column[i] = M[i, col]; - } - return column; - } - - public static Vector3 GetRow3(float[,] M, int rowIx) { - int cols = M.GetLength(1); - Vector3 row = new(); - row.x = M[rowIx, 0]; - row.y = M[rowIx, 1]; - row.z = M[rowIx, 2]; - return row; - } - - public static void SetRow3(float[,] M, int rowIx, Vector3 v) { - M[rowIx, 0] = v.x; - M[rowIx, 1] = v.y; - M[rowIx, 2] = v.z; - } - - public static float Dot(float[] a, float[] b) { - float sum = 0; - for (int i = 0; i < a.Length; i++) sum += a[i] * b[i]; - return sum; - } - - public static float[,] IdentityMatrix(int size) { - float[,] I = new float[size, size]; - for (int i = 0; i < size; i++) I[i, i] = 1.0f; - return I; - } - } \ No newline at end of file diff --git a/LinearAlgebra/Quaternion.cs b/LinearAlgebra/Quaternion.cs index 55ad014..9a9fed7 100644 --- a/LinearAlgebra/Quaternion.cs +++ b/LinearAlgebra/Quaternion.cs @@ -13,15 +13,16 @@ namespace Passer.LinearAlgebra { this.w = w; } - public static float[,] ToRotationMatrix(Quaternion q) { + public static Matrix2 ToRotationMatrix(Quaternion q) { float w = q.x, x = q.y, y = q.z, z = q.w; - return new float[,] + float[,] result = new float[,] { { 1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y) }, { 2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x) }, { 2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y) } }; + return new Matrix2(result); } } From 87744740d4b304ac49cac8de2872cd49ad9ce1b2 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 17 Feb 2025 09:05:10 +0100 Subject: [PATCH 2/7] Fix merge error --- Unity/DistanceSensor.cs | 1 - 1 file changed, 1 deletion(-) diff --git a/Unity/DistanceSensor.cs b/Unity/DistanceSensor.cs index b7a8d0d..ac74b78 100644 --- a/Unity/DistanceSensor.cs +++ b/Unity/DistanceSensor.cs @@ -7,7 +7,6 @@ namespace Passer.Control.Unity { public class DistanceSensor : Thing { public new Core.DistanceSensor core { - get => (Core.DistanceSensor)base.core; get => (Core.DistanceSensor)base.core; set => base.core = value; } From b10929619d91811454abccfef356c8aef87da753 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 17 Feb 2025 11:53:32 +0100 Subject: [PATCH 3/7] Added initial imu processing --- LinearAlgebra/Matrix.cs | 44 +++++++++++++++++++++++++++---------- LinearAlgebra/Quaternion.cs | 7 +++--- 2 files changed, 37 insertions(+), 14 deletions(-) diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs index b5f7e9d..158c1d6 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -4,9 +4,9 @@ using Vector3 = UnityEngine.Vector3; using Vector2 = UnityEngine.Vector2; public readonly struct Slice { - public int start { get; } - public int stop { get; } - public Slice(int start, int stop) { + public uint start { get; } + public uint stop { get; } + public Slice(uint start, uint stop) { this.start = start; this.stop = stop; } @@ -101,6 +101,9 @@ public class Matrix2 { return new Matrix2(resultData); // double checked code } + public Matrix2 transposed { + get => Transpose(); + } public static Matrix2 operator -(Matrix2 m) { float[,] result = new float[m.nRows, m.nCols]; @@ -195,13 +198,13 @@ public class Matrix2 { public Matrix2 Slice(Slice slice) { return Slice(slice.start, slice.stop); } - public Matrix2 Slice(int from, int to) { + public Matrix2 Slice(uint from, uint 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 (uint rowIx = from; rowIx < to; rowIx++) { for (int colIx = 0; colIx < this.nCols; colIx++) { result[resultRowIx, colIx] = this.data[rowIx, colIx]; } @@ -210,16 +213,35 @@ public class Matrix2 { return new Matrix2(result); } + public Matrix2 Slice(Slice rowRange, Slice colRange) { + return Slice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop)); + } + + 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; + uint resultColIx = 0; + for (uint i = rowRange.start; i < rowRange.stop; i++) { + for (uint 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) { int mRowIx = 0; - for (int rowIx = slice.start; rowIx < slice.stop; rowIx++) { + for (uint rowIx = slice.start; rowIx < slice.stop; rowIx++) { 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) { - for (int i = rowRange.start; i < rowRange.stop; i++) { - for (int j = colRange.start; j < colRange.stop; j++) + 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) { + for (uint i = rowRange.start; i < rowRange.stop; i++) { + for (uint j = colRange.start; j < colRange.stop; j++) this.data[i, j] = m.data[i - rowRange.start, j - colRange.stop]; } } @@ -401,20 +423,20 @@ public class Matrix1 { public Matrix1 Slice(Slice range) { return Slice(range.start, range.stop); } - public Matrix1 Slice(int from, int to) { + public Matrix1 Slice(uint from, uint to) { if (from < 0 || to >= this.magnitude) throw new System.ArgumentException("Slice index out of range."); float[] result = new float[to - from]; int resultIx = 0; - for (int ix = from; ix < to; ix++) + for (uint ix = from; ix < to; ix++) result[resultIx++] = this.data[ix]; return new Matrix1(result); } public void UpdateSlice(Slice slice, Matrix1 v) { int vIx = 0; - for (int ix = slice.start; ix < slice.stop; ix++, vIx++) + for (uint ix = slice.start; ix < slice.stop; ix++, vIx++) this.data[ix] = v.data[vIx]; } } \ No newline at end of file diff --git a/LinearAlgebra/Quaternion.cs b/LinearAlgebra/Quaternion.cs index 9a9fed7..742cd97 100644 --- a/LinearAlgebra/Quaternion.cs +++ b/LinearAlgebra/Quaternion.cs @@ -1,3 +1,4 @@ +using Quaternion = UnityEngine.Quaternion; namespace Passer.LinearAlgebra { public class QuaternionOf { @@ -26,7 +27,7 @@ namespace Passer.LinearAlgebra { } } - public class Quaternion : QuaternionOf { - public Quaternion(float x, float y, float z, float w) : base(x, y, z, w) { } - } + // public class Quaternion : QuaternionOf { + // public Quaternion(float x, float y, float z, float w) : base(x, y, z, w) { } + // } } \ No newline at end of file From c1b9a8c5e6381591295d9be2684e84913aba57e7 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Fri, 21 Feb 2025 12:21:12 +0100 Subject: [PATCH 4/7] Port MSCKF complete (from now) --- LinearAlgebra/Matrix.cs | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs index 158c1d6..21e2670 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -2,6 +2,7 @@ using System; using System.Diagnostics; using Vector3 = UnityEngine.Vector3; using Vector2 = UnityEngine.Vector2; +using Quaternion = UnityEngine.Quaternion; public readonly struct Slice { public uint start { get; } @@ -216,7 +217,7 @@ public class Matrix2 { public Matrix2 Slice(Slice rowRange, Slice colRange) { return Slice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop)); } - + 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]; @@ -374,6 +375,15 @@ public class Matrix1 { return new Matrix1(result); } + public static Matrix1 FromQuaternion(Quaternion q) { + float[] result = new float[4]; + result[0] = q.x; + result[1] = q.y; + result[2] = q.z; + result[3] = q.w; + return new Matrix1(result); + } + public Vector2 vector2 { get { if (this.magnitude != 2) @@ -388,6 +398,13 @@ public class Matrix1 { return new Vector3(this.data[0], this.data[1], this.data[2]); } } + public Quaternion quaternion { + get { + if (this.magnitude != 4) + throw new System.ArgumentException("Matrix1 must be of size 4"); + return new Quaternion(this.data[0], this.data[1], this.data[2], this.data[3]); + } + } public Matrix2 Transpose() { float[,] r = new float[1, this.magnitude]; From f33b105f50ce212fb7fecdc2f73e4806e63c7bb9 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 25 Feb 2025 11:08:29 +0100 Subject: [PATCH 5/7] First MSCKF test passed --- LinearAlgebra/Matrix.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs index 21e2670..696c547 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -243,7 +243,7 @@ public class Matrix2 { public void UpdateSlice((uint start, uint stop) rowRange, (uint start, uint stop) colRange, Matrix2 m) { for (uint i = rowRange.start; i < rowRange.stop; i++) { for (uint j = colRange.start; j < colRange.stop; j++) - this.data[i, j] = m.data[i - rowRange.start, j - colRange.stop]; + this.data[i, j] = m.data[i - rowRange.start, j - colRange.start]; } } From 05cdf0328d45b7af6aa61ff83f302ef23917c6d1 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 25 Feb 2025 11:32:34 +0100 Subject: [PATCH 6/7] namespace update --- Messages/BinaryMsg.cs | 2 +- Messages/DestroyMsg.cs | 2 +- Messages/InvestigateMsg.cs | 2 +- Messages/LowLevelMessages.cs | 2 +- Messages/Messages.cs | 2 +- Messages/ModelUrlMsg.cs | 2 +- Messages/NameMsg.cs | 2 +- Messages/NetworkIdMsg.cs | 2 +- Messages/ParticipantMsg.cs | 2 +- Messages/PoseMsg.cs | 2 +- Messages/TextMsg.cs | 2 +- Messages/ThingMsg.cs | 2 +- Participant.cs | 2 +- RemoteParticipant.cs | 2 +- Sensors/DistanceSensor.cs | 2 +- Sensors/TemperatureSensor.cs | 2 +- Sensors/TouchSensor.cs | 2 +- SiteServer.cs | 2 +- Thing.cs | 2 +- Unity/DebugConsole.cs | 2 +- Unity/DistanceSensor.cs | 2 +- Unity/SiteServer.cs | 2 +- Unity/Thing.cs | 2 +- Unity/TouchSensor.cs | 2 +- 24 files changed, 24 insertions(+), 24 deletions(-) diff --git a/Messages/BinaryMsg.cs b/Messages/BinaryMsg.cs index e72c6fd..21d62c6 100644 --- a/Messages/BinaryMsg.cs +++ b/Messages/BinaryMsg.cs @@ -1,4 +1,4 @@ -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// A message containing binary data for custom communication diff --git a/Messages/DestroyMsg.cs b/Messages/DestroyMsg.cs index 660c142..1963bc3 100644 --- a/Messages/DestroyMsg.cs +++ b/Messages/DestroyMsg.cs @@ -1,4 +1,4 @@ -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// Message notifiying that a Thing no longer exists diff --git a/Messages/InvestigateMsg.cs b/Messages/InvestigateMsg.cs index d822fec..08d54d9 100644 --- a/Messages/InvestigateMsg.cs +++ b/Messages/InvestigateMsg.cs @@ -1,4 +1,4 @@ -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// Message to request details for a Thing diff --git a/Messages/LowLevelMessages.cs b/Messages/LowLevelMessages.cs index 2a067ea..01a579d 100644 --- a/Messages/LowLevelMessages.cs +++ b/Messages/LowLevelMessages.cs @@ -1,6 +1,6 @@ using Passer.LinearAlgebra; -namespace Passer.RoboidControl +namespace RoboidControl { public class LowLevelMessages { diff --git a/Messages/Messages.cs b/Messages/Messages.cs index d75e62b..2fa0045 100644 --- a/Messages/Messages.cs +++ b/Messages/Messages.cs @@ -1,4 +1,4 @@ -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// Root structure for all communcation messages diff --git a/Messages/ModelUrlMsg.cs b/Messages/ModelUrlMsg.cs index 01b6527..be15087 100644 --- a/Messages/ModelUrlMsg.cs +++ b/Messages/ModelUrlMsg.cs @@ -1,4 +1,4 @@ -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// Message for communicating the URL for a model of the thing diff --git a/Messages/NameMsg.cs b/Messages/NameMsg.cs index 57de3aa..632540f 100644 --- a/Messages/NameMsg.cs +++ b/Messages/NameMsg.cs @@ -1,4 +1,4 @@ -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// Message for communicating the name of a thing diff --git a/Messages/NetworkIdMsg.cs b/Messages/NetworkIdMsg.cs index fc095d0..8d81e27 100644 --- a/Messages/NetworkIdMsg.cs +++ b/Messages/NetworkIdMsg.cs @@ -1,4 +1,4 @@ -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// A message communicating the network ID for that participant diff --git a/Messages/ParticipantMsg.cs b/Messages/ParticipantMsg.cs index 6ba3228..6af8fdb 100644 --- a/Messages/ParticipantMsg.cs +++ b/Messages/ParticipantMsg.cs @@ -1,4 +1,4 @@ -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// A participant messages notifies other participants of its presence diff --git a/Messages/PoseMsg.cs b/Messages/PoseMsg.cs index 4f1320d..5edd583 100644 --- a/Messages/PoseMsg.cs +++ b/Messages/PoseMsg.cs @@ -1,6 +1,6 @@ using Passer.LinearAlgebra; -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// Message to communicate the pose of the thing diff --git a/Messages/TextMsg.cs b/Messages/TextMsg.cs index c2dcf16..a1de990 100644 --- a/Messages/TextMsg.cs +++ b/Messages/TextMsg.cs @@ -1,4 +1,4 @@ -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// Message for sending generic text diff --git a/Messages/ThingMsg.cs b/Messages/ThingMsg.cs index e83827d..9725e23 100644 --- a/Messages/ThingMsg.cs +++ b/Messages/ThingMsg.cs @@ -1,4 +1,4 @@ -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// Message providing generic information about a Thing diff --git a/Participant.cs b/Participant.cs index 349f50d..434eabd 100644 --- a/Participant.cs +++ b/Participant.cs @@ -4,7 +4,7 @@ using System.Collections.Concurrent; using System.Net; using System.Net.Sockets; -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// A participant is used for communcation between things diff --git a/RemoteParticipant.cs b/RemoteParticipant.cs index 64b9fb4..ee5a0b9 100644 --- a/RemoteParticipant.cs +++ b/RemoteParticipant.cs @@ -1,7 +1,7 @@ using System; using System.Collections.Generic; -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// A reference to a participant, possibly on a remote location diff --git a/Sensors/DistanceSensor.cs b/Sensors/DistanceSensor.cs index 3967ae5..ade3998 100644 --- a/Sensors/DistanceSensor.cs +++ b/Sensors/DistanceSensor.cs @@ -1,4 +1,4 @@ -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// A sensor measuring the distance in the forward direction diff --git a/Sensors/TemperatureSensor.cs b/Sensors/TemperatureSensor.cs index edb9750..b0d6860 100644 --- a/Sensors/TemperatureSensor.cs +++ b/Sensors/TemperatureSensor.cs @@ -1,6 +1,6 @@ //using System; -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// A temperature sensor diff --git a/Sensors/TouchSensor.cs b/Sensors/TouchSensor.cs index f6bc873..e59678a 100644 --- a/Sensors/TouchSensor.cs +++ b/Sensors/TouchSensor.cs @@ -1,4 +1,4 @@ -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// A sensor which can detect touches diff --git a/SiteServer.cs b/SiteServer.cs index 9bd1ae9..eef52a4 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -2,7 +2,7 @@ using System; using System.Net; using System.Net.Sockets; -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// A site server is a participant which provides a shared simulated environment diff --git a/Thing.cs b/Thing.cs index 92d929d..29fa7c4 100644 --- a/Thing.cs +++ b/Thing.cs @@ -2,7 +2,7 @@ using System; using System.Collections.Generic; using Passer.LinearAlgebra; -namespace Passer.RoboidControl { +namespace RoboidControl { /// /// A thing is the primitive building block diff --git a/Unity/DebugConsole.cs b/Unity/DebugConsole.cs index 5b4e4f6..70b587a 100644 --- a/Unity/DebugConsole.cs +++ b/Unity/DebugConsole.cs @@ -3,7 +3,7 @@ using System.IO; using System.Text; using UnityEngine; -namespace Passer.RoboidControl.Unity { +namespace RoboidControl.Unity { public class UnityLogWriter : TextWriter { public override void Write(char value) { diff --git a/Unity/DistanceSensor.cs b/Unity/DistanceSensor.cs index f8cc98a..e27e2f7 100644 --- a/Unity/DistanceSensor.cs +++ b/Unity/DistanceSensor.cs @@ -2,7 +2,7 @@ using System.Collections; using UnityEngine; -namespace Passer.RoboidControl.Unity { +namespace RoboidControl.Unity { /// /// The Unity representation of a distance sensor diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs index ce665de..ee17799 100644 --- a/Unity/SiteServer.cs +++ b/Unity/SiteServer.cs @@ -3,7 +3,7 @@ using System; using System.Collections.Generic; using UnityEngine; -namespace Passer.RoboidControl.Unity { +namespace RoboidControl.Unity { public class SiteServer : MonoBehaviour { public RoboidControl.SiteServer site; diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 8515edb..175d2ce 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -3,7 +3,7 @@ using System.Collections; using UnityEngine; using UnityEngine.Networking; -namespace Passer.RoboidControl.Unity { +namespace RoboidControl.Unity { /// /// The representation of a Thing in Unity diff --git a/Unity/TouchSensor.cs b/Unity/TouchSensor.cs index 62f6b51..6e6ce44 100644 --- a/Unity/TouchSensor.cs +++ b/Unity/TouchSensor.cs @@ -1,7 +1,7 @@ #if UNITY_5_3_OR_NEWER using UnityEngine; -namespace Passer.RoboidControl.Unity { +namespace RoboidControl.Unity { /// /// The Unity representation of the TouchSensor From ac376338de4df35266136050d49e37c4c3f03180 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 25 Feb 2025 15:46:58 +0100 Subject: [PATCH 7/7] Implemented all tests, some are failing --- LinearAlgebra/Matrix.cs | 136 ++++++++++++++++++++++++++++-------- LinearAlgebra/Quaternion.cs | 39 +++++++++++ Participant.cs | 2 +- SiteServer.cs | 4 +- Unity/Thing.cs | 2 +- 5 files changed, 151 insertions(+), 32 deletions(-) diff --git a/LinearAlgebra/Matrix.cs b/LinearAlgebra/Matrix.cs index 6327145..e1b453c 100644 --- a/LinearAlgebra/Matrix.cs +++ b/LinearAlgebra/Matrix.cs @@ -42,17 +42,26 @@ public class Matrix2 { return new Matrix2(nRows, nCols); } + public static Matrix2 FromVector3(Vector3Float v) { + float[,] result = new float[3, 1]; + result[0, 0] = v.x; + result[1, 0] = v.y; + result[2, 0] = v.z; + return new Matrix2(result); + } + public static Matrix2 Identity(uint size) { return Diagonal(1, size); - // float[,] resultData = new float[size, size]; - // for (int i = 0; i < size; i++) - // resultData[i, i] = 1.0f; - // return new Matrix2(resultData); + } + public static Matrix2 Identity(uint nRows, uint nCols) { + Matrix2 m = Zero(nRows, nCols); + m.FillDiagonal(1); + return m; } public static Matrix2 Diagonal(Matrix1 v) { - float[,] resultData = new float[v.magnitude, v.magnitude]; - for (int ix = 0; ix < v.magnitude; ix++) + float[,] resultData = new float[v.size, v.size]; + for (int ix = 0; ix < v.size; ix++) resultData[ix, ix] = v.data[ix]; return new Matrix2(resultData); } @@ -62,6 +71,16 @@ public class Matrix2 { resultData[ix, ix] = f; return new Matrix2(resultData); } + public void FillDiagonal(Matrix1 v) { + uint n = 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); + for (int ix = 0; ix < n; ix++) + this.data[ix, ix] = f; + } public static Matrix2 SkewMatrix(Vector3Float v) { float[,] result = new float[3, 3] { @@ -81,6 +100,10 @@ public class Matrix2 { }; return row; } + public void SetRow(int rowIx, Matrix1 v) { + for (uint ix = 0; ix < v.size; ix++) + this.data[rowIx, ix] = v.data[ix]; + } public void SetRow3(int rowIx, Vector3Float v) { this.data[rowIx, 0] = v.x; this.data[rowIx, 1] = v.y; @@ -95,6 +118,16 @@ public class Matrix2 { return new Matrix1(column); } + public static bool AllClose(Matrix2 A, Matrix2 B, float atol = 1e-08f) { + for (int i = 0; i < A.nRows; i++) { + for (int j = 0; j < A.nCols; j++) { + float d = MathF.Abs(A.data[i, j] - B.data[i, j]); + if (d > atol) + return false; + } + } + return true; + } public Matrix2 Transpose() { float[,] resultData = new float[this.nCols, this.nRows]; @@ -190,13 +223,34 @@ public class Matrix2 { for (int i = 0; i < A.nRows; i++) { for (int j = 0; j < A.nCols; j++) - result[i, j] += A.data[i, j] * s; + result[i, j] = A.data[i, j] * s; } return new Matrix2(result); } - public static Matrix2 operator *(float scalar, Matrix2 A) { - return A * scalar; + public static Matrix2 operator *(float s, Matrix2 A) { + return A * s; + } + + public static Matrix2 operator /(Matrix2 A, float s) { + float[,] result = new float[A.nRows, A.nCols]; + + for (int i = 0; i < A.nRows; i++) { + for (int j = 0; j < A.nCols; j++) + result[i, j] = A.data[i, j] / s; + } + + return new Matrix2(result); + } + public static Matrix2 operator /(float s, Matrix2 A) { + float[,] result = new float[A.nRows, A.nCols]; + + for (int i = 0; i < A.nRows; i++) { + for (int j = 0; j < A.nCols; j++) + result[i, j] = s / A.data[i, j]; + } + + return new Matrix2(result); } public Matrix2 Slice(Slice slice) { @@ -235,7 +289,7 @@ public class Matrix2 { public void UpdateSlice(Slice slice, Matrix2 m) { int mRowIx = 0; - for (uint rowIx = slice.start; rowIx < slice.stop; rowIx++) { + for (uint rowIx = slice.start; rowIx < slice.stop; rowIx++, mRowIx++) { for (int colIx = 0; colIx < this.nCols; colIx++) this.data[rowIx, colIx] = m.data[mRowIx, colIx]; } @@ -342,25 +396,23 @@ public class Matrix2 { return new Matrix2(minor); } - - } public class Matrix1 { public float[] data { get; } - public uint magnitude => (uint)data.GetLength(0); + public uint size => (uint)data.GetLength(0); - public Matrix1(uint magnitude) { - this.data = new float[magnitude]; + public Matrix1(uint size) { + this.data = new float[size]; } public Matrix1(float[] data) { this.data = data; } - public static Matrix1 Zero(uint magnitude) { - return new Matrix1(magnitude); + public static Matrix1 Zero(uint size) { + return new Matrix1(size); } public static Matrix1 FromVector2(Vector2Float v) { @@ -379,7 +431,7 @@ public class Matrix1 { } public static Matrix1 FromQuaternion(Quaternion q) { - float[] result = new float[4]; + float[] result = new float[4]; result[0] = q.x; result[1] = q.y; result[2] = q.z; @@ -389,49 +441,77 @@ public class Matrix1 { public Vector2Float vector2 { get { - if (this.magnitude != 2) + if (this.size != 2) throw new System.ArgumentException("Matrix1 must be of size 2"); return new Vector2Float(this.data[0], this.data[1]); } } public Vector3Float vector3 { get { - if (this.magnitude != 3) + if (this.size != 3) throw new System.ArgumentException("Matrix1 must be of size 3"); return new Vector3Float(this.data[0], this.data[1], this.data[2]); } } public Quaternion quaternion { get { - if (this.magnitude != 4) + if (this.size != 4) throw new System.ArgumentException("Matrix1 must be of size 4"); return new Quaternion(this.data[0], this.data[1], this.data[2], this.data[3]); } } + public Matrix1 Clone() { + float[] data = new float[this.size]; + for (int rowIx = 0; rowIx < this.size; rowIx++) + data[rowIx] = this.data[rowIx]; + return new Matrix1(data); + } + + + public float magnitude { + get { + float sum = 0; + foreach (var elm in data) + sum += elm; + return sum / data.Length; + } + } + 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 Matrix2 Transpose() { - float[,] r = new float[1, this.magnitude]; - for (uint colIx = 0; colIx < this.magnitude; colIx++) + float[,] r = new float[1, this.size]; + for (uint colIx = 0; colIx < this.size; colIx++) r[1, colIx] = this.data[colIx]; return new Matrix2(r); } public static float Dot(Matrix1 a, Matrix1 b) { - if (a.magnitude != b.magnitude) + if (a.size != b.size) throw new System.ArgumentException("Vectors must be of the same length."); float result = 0.0f; - for (int i = 0; i < a.magnitude; i++) { + for (int i = 0; i < a.size; i++) { result += a.data[i] * b.data[i]; } return result; } public static Matrix1 operator *(Matrix1 A, float f) { - float[] result = new float[A.magnitude]; + float[] result = new float[A.size]; - for (int i = 0; i < A.magnitude; i++) + for (int i = 0; i < A.size; i++) result[i] += A.data[i] * f; return new Matrix1(result); @@ -444,7 +524,7 @@ public class Matrix1 { return Slice(range.start, range.stop); } public Matrix1 Slice(uint from, uint to) { - if (from < 0 || to >= this.magnitude) + if (from < 0 || to >= this.size) throw new System.ArgumentException("Slice index out of range."); float[] result = new float[to - from]; diff --git a/LinearAlgebra/Quaternion.cs b/LinearAlgebra/Quaternion.cs index 742cd97..b1146ed 100644 --- a/LinearAlgebra/Quaternion.cs +++ b/LinearAlgebra/Quaternion.cs @@ -1,3 +1,4 @@ +using System; using Quaternion = UnityEngine.Quaternion; namespace Passer.LinearAlgebra { @@ -25,6 +26,44 @@ namespace Passer.LinearAlgebra { }; return new Matrix2(result); } + + public static Quaternion FromRotationMatrix(Matrix2 m) { + float trace = m.data[0, 0] + m.data[1, 1] + m.data[2, 2]; + float w, x, y, z; + + if (trace > 0) { + float s = 0.5f / (float)Math.Sqrt(trace + 1.0f); + w = 0.25f / s; + x = (m.data[2, 1] - m.data[1, 2]) * s; + y = (m.data[0, 2] - m.data[2, 0]) * s; + z = (m.data[1, 0] - m.data[0, 1]) * s; + } + else { + if (m.data[0, 0] > m.data[1, 1] && m.data[0, 0] > m.data[2, 2]) { + float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[0, 0] - m.data[1, 1] - m.data[2, 2]); + w = (m.data[2, 1] - m.data[1, 2]) / s; + x = 0.25f * s; + y = (m.data[0, 1] + m.data[1, 0]) / s; + z = (m.data[0, 2] + m.data[2, 0]) / s; + } + else if (m.data[1, 1] > m.data[2, 2]) { + float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[1, 1] - m.data[0, 0] - m.data[2, 2]); + w = (m.data[0, 2] - m.data[2, 0]) / s; + x = (m.data[0, 1] + m.data[1, 0]) / s; + y = 0.25f * s; + z = (m.data[1, 2] + m.data[2, 1]) / s; + } + else { + float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[2, 2] - m.data[0, 0] - m.data[1, 1]); + w = (m.data[1, 0] - m.data[0, 1]) / s; + x = (m.data[0, 2] + m.data[2, 0]) / s; + y = (m.data[1, 2] + m.data[2, 1]) / s; + z = 0.25f * s; + } + } + + return new Quaternion(x, y, z, w); + } } // public class Quaternion : QuaternionOf { diff --git a/Participant.cs b/Participant.cs index 434eabd..d32b756 100644 --- a/Participant.cs +++ b/Participant.cs @@ -299,7 +299,7 @@ namespace RoboidControl { } protected virtual void Process(RemoteParticipant sender, ModelUrlMsg msg) { - Console.WriteLine($"Participant: Process model [{msg.networkId}/{msg.thingId}] {msg.url}"); + //Console.WriteLine($"Participant: Process model [{msg.networkId}/{msg.thingId}] {msg.url}"); Thing thing = sender.Get(msg.networkId, msg.thingId); if (thing != null) thing.modelUrl = msg.url; diff --git a/SiteServer.cs b/SiteServer.cs index eef52a4..178dceb 100644 --- a/SiteServer.cs +++ b/SiteServer.cs @@ -52,7 +52,7 @@ namespace RoboidControl { protected override void Process(RemoteParticipant sender, NetworkIdMsg 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); if (thing == null) { Thing newThing = null; @@ -63,7 +63,7 @@ namespace RoboidControl { } if (newThing == null) { newThing = new Thing(sender, msg.networkId, msg.thingId, msg.thingType); - Console.WriteLine("Created generic new core thing"); + // Console.WriteLine("Created generic new core thing"); } if (msg.parentId != 0) { Thing parentThing = Get(msg.networkId, msg.parentId); diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 175d2ce..d58c40c 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -35,7 +35,7 @@ namespace RoboidControl.Unity { } public static Thing Create(RoboidControl.Thing core) { - Debug.Log("Creating new Unity thing"); + // Debug.Log("Creating new Unity thing"); GameObject gameObj = string.IsNullOrEmpty(core.name) ? new("Thing") : new(core.name);