From ac376338de4df35266136050d49e37c4c3f03180 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 25 Feb 2025 15:46:58 +0100 Subject: [PATCH] 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);