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

Implemented all tests, some are failing

Browse Source
This commit is contained in:
Pascal Serrarens 2025-02-25 15:46:58 +01:00
parent 05cdf0328d
commit ac376338de
5 changed files with 151 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

136
LinearAlgebra/Matrix.cs
View File

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

39
LinearAlgebra/Quaternion.cs
View File

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

2
Participant.cs
View File

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

4
SiteServer.cs
View File

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

2
Unity/Thing.cs
View File

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