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

Added initial imu processing

Browse Source
This commit is contained in:
Pascal Serrarens 2025-02-17 11:53:32 +01:00
parent 87744740d4
commit b10929619d
2 changed files with 37 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
LinearAlgebra/Matrix.cs
View File

@ -4,9 +4,9 @@ using Vector3 = UnityEngine.Vector3;
using Vector2 = UnityEngine.Vector2; using Vector2 = UnityEngine.Vector2;
public readonly struct Slice { public readonly struct Slice {
public int start { get; } public uint start { get; }
public int stop { get; } public uint stop { get; }
public Slice(int start, int stop) { public Slice(uint start, uint stop) {
this.start = start; this.start = start;
this.stop = stop; this.stop = stop;
} }
@ -101,6 +101,9 @@ public class Matrix2 {
return new Matrix2(resultData); return new Matrix2(resultData);
// double checked code // double checked code
} }
public Matrix2 transposed {
get => Transpose();
}
public static Matrix2 operator -(Matrix2 m) { public static Matrix2 operator -(Matrix2 m) {
float[,] result = new float[m.nRows, m.nCols]; float[,] result = new float[m.nRows, m.nCols];
@ -195,13 +198,13 @@ public class Matrix2 {
public Matrix2 Slice(Slice slice) { public Matrix2 Slice(Slice slice) {
return Slice(slice.start, slice.stop); 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) if (from < 0 || to >= this.nRows)
throw new System.ArgumentException("Slice index out of range."); throw new System.ArgumentException("Slice index out of range.");
float[,] result = new float[to - from, this.nCols]; float[,] result = new float[to - from, this.nCols];
int resultRowIx = 0; 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++) { for (int colIx = 0; colIx < this.nCols; colIx++) {
result[resultRowIx, colIx] = this.data[rowIx, colIx]; result[resultRowIx, colIx] = this.data[rowIx, colIx];
} }
@ -210,16 +213,35 @@ public class Matrix2 {
return new Matrix2(result); 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) { public void UpdateSlice(Slice slice, Matrix2 m) {
int mRowIx = 0; 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++) for (int colIx = 0; colIx < this.nCols; colIx++)
this.data[rowIx, colIx] = m.data[mRowIx, colIx]; this.data[rowIx, colIx] = m.data[mRowIx, colIx];
} }
} }
public void UpdateSlice(Slice rowRange, Slice colRange, Matrix2 m) { public void UpdateSlice(Slice rowRange, Slice colRange, Matrix2 m) {
for (int i = rowRange.start; i < rowRange.stop; i++) { UpdateSlice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop), m);
for (int j = colRange.start; j < colRange.stop; j++) }
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.stop];
} }
} }
@ -401,20 +423,20 @@ public class Matrix1 {
public Matrix1 Slice(Slice range) { public Matrix1 Slice(Slice range) {
return Slice(range.start, range.stop); 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) if (from < 0 || to >= this.magnitude)
throw new System.ArgumentException("Slice index out of range."); throw new System.ArgumentException("Slice index out of range.");
float[] result = new float[to - from]; float[] result = new float[to - from];
int resultIx = 0; int resultIx = 0;
for (int ix = from; ix < to; ix++) for (uint ix = from; ix < to; ix++)
result[resultIx++] = this.data[ix]; result[resultIx++] = this.data[ix];
return new Matrix1(result); return new Matrix1(result);
} }
public void UpdateSlice(Slice slice, Matrix1 v) { public void UpdateSlice(Slice slice, Matrix1 v) {
int vIx = 0; 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]; this.data[ix] = v.data[vIx];
} }
} }

7
LinearAlgebra/Quaternion.cs
View File

@ -1,3 +1,4 @@
using Quaternion = UnityEngine.Quaternion;
namespace Passer.LinearAlgebra { namespace Passer.LinearAlgebra {
public class QuaternionOf<T> { public class QuaternionOf<T> {
@ -26,7 +27,7 @@ namespace Passer.LinearAlgebra {
} }
} }
public class Quaternion : QuaternionOf<float> { // public class Quaternion : QuaternionOf<float> {
public Quaternion(float x, float y, float z, float w) : base(x, y, z, w) { } // public Quaternion(float x, float y, float z, float w) : base(x, y, z, w) { }
} // }
} }