diff --git a/LinearAlgebra/.gitignore b/LinearAlgebra/.gitignore
new file mode 100644
index 0000000..f0b2f47
--- /dev/null
+++ b/LinearAlgebra/.gitignore
@@ -0,0 +1,6 @@
+DoxyGen/DoxyWarnLogfile.txt
+.vscode/settings.json
+**bin
+**obj
+**.meta
+*.sln
diff --git a/LinearAlgebra/src/Angle.cs b/LinearAlgebra/src/Angle.cs
index 78ae979..ca03a50 100644
--- a/LinearAlgebra/src/Angle.cs
+++ b/LinearAlgebra/src/Angle.cs
@@ -1,11 +1,13 @@
using System;
-namespace Passer.LinearAlgebra {
+namespace LinearAlgebra
+{
///
/// %Angle utilities
///
- public static class Angle {
+ public static class Angle
+ {
public const float pi = 3.1415927410125732421875F;
// public static float Rad2Deg = 360.0f / ((float)Math.PI * 2);
// public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f;
@@ -21,7 +23,8 @@ namespace Passer.LinearAlgebra {
/// The maximum angle
/// The clamped angle
/// Angles are normalized
- public static float Clamp(float angle, float min, float max) {
+ public static float Clamp(float angle, float min, float max)
+ {
float normalizedAngle = Normalize(angle);
return Float.Clamp(normalizedAngle, min, max);
}
@@ -33,7 +36,8 @@ namespace Passer.LinearAlgebra {
/// The second angle
/// the angle between the two angles
/// Angle values should be degrees
- public static float Difference(float a, float b) {
+ public static float Difference(float a, float b)
+ {
float r = Normalize(b - a);
return r;
}
@@ -44,7 +48,8 @@ namespace Passer.LinearAlgebra {
/// The angle to normalize
/// The normalized angle in interval (-180..180]
/// Angle values should be in degrees
- public static float Normalize(float angle) {
+ public static float Normalize(float angle)
+ {
if (float.IsInfinity(angle))
return angle;
@@ -61,7 +66,8 @@ namespace Passer.LinearAlgebra {
/// Maximum angle to rotate
/// The resulting angle
/// This function is compatible with radian and degrees angles
- public static float MoveTowards(float fromAngle, float toAngle, float maxAngle) {
+ public static float MoveTowards(float fromAngle, float toAngle, float maxAngle)
+ {
float d = toAngle - fromAngle;
d = Normalize(d);
d = Math.Sign(d) * Float.Clamp(Math.Abs(d), 0, maxAngle);
@@ -77,7 +83,8 @@ namespace Passer.LinearAlgebra {
/// Vectors a and b must be normalized
/// \deprecated Please use Vector2.ToFactor instead.
[Obsolete("Please use Vector2.ToFactor instead.")]
- public static float ToFactor(Vector2 v1, Vector2 v2) {
+ public static float ToFactor(Vector2 v1, Vector2 v2)
+ {
return (1 - Vector2.Dot(v1, v2)) / 2;
}
diff --git a/LinearAlgebra/src/Direction.cs b/LinearAlgebra/src/Direction.cs
index bd7477f..258e27e 100644
--- a/LinearAlgebra/src/Direction.cs
+++ b/LinearAlgebra/src/Direction.cs
@@ -1,14 +1,18 @@
-namespace Passer.LinearAlgebra {
+namespace LinearAlgebra
+{
- public class Direction {
+ public class Direction
+ {
public float horizontal;
public float vertical;
- public Direction() {
+ public Direction()
+ {
horizontal = 0;
vertical = 0;
}
- public Direction(float horizontal, float vertical) {
+ public Direction(float horizontal, float vertical)
+ {
this.horizontal = horizontal;
this.vertical = vertical;
//Normalize();
@@ -21,8 +25,10 @@ namespace Passer.LinearAlgebra {
public readonly static Direction left = new Direction(-90, 0);
public readonly static Direction right = new Direction(90, 0);
- public void Normalize() {
- if (this.vertical > 90 || this.vertical < -90) {
+ public void Normalize()
+ {
+ if (this.vertical > 90 || this.vertical < -90)
+ {
this.horizontal += 180;
this.vertical = 180 - this.vertical;
}
diff --git a/LinearAlgebra/src/Float.cs b/LinearAlgebra/src/Float.cs
index 4959b75..70873f0 100644
--- a/LinearAlgebra/src/Float.cs
+++ b/LinearAlgebra/src/Float.cs
@@ -1,9 +1,11 @@
-namespace Passer.LinearAlgebra {
+namespace LinearAlgebra
+{
///
/// Float number utilities
///
- public class Float {
+ public class Float
+ {
///
/// The precision of float numbers
///
@@ -20,7 +22,8 @@ namespace Passer.LinearAlgebra {
/// The minimum value
/// The maximum value
/// The clamped value
- public static float Clamp(float f, float min, float max) {
+ public static float Clamp(float f, float min, float max)
+ {
if (f < min)
return min;
if (f > max)
@@ -33,7 +36,8 @@ namespace Passer.LinearAlgebra {
///
/// The value to clamp
/// The clamped value
- public static float Clamp01(float f) {
+ public static float Clamp01(float f)
+ {
return Clamp(f, 0, 1);
}
}
diff --git a/LinearAlgebra/src/Matrix.cs b/LinearAlgebra/src/Matrix.cs
index 4d37dda..1f0542e 100644
--- a/LinearAlgebra/src/Matrix.cs
+++ b/LinearAlgebra/src/Matrix.cs
@@ -1,95 +1,112 @@
using System;
-using System.Diagnostics;
-using Passer.LinearAlgebra;
#if UNITY_5_3_OR_NEWER
using Vector3Float = UnityEngine.Vector3;
using Vector2Float = UnityEngine.Vector2;
using Quaternion = UnityEngine.Quaternion;
#endif
-public readonly struct Slice {
- public uint start { get; }
- public uint stop { get; }
- public Slice(uint start, uint stop) {
- this.start = start;
- this.stop = stop;
- }
-}
+namespace LinearAlgebra
+{
-public class Matrix2 {
- public float[,] data { get; }
-
- public uint nRows => (uint)data.GetLength(0);
- public uint nCols => (uint)data.GetLength(1);
-
- public Matrix2(uint nRows, uint nCols) {
- this.data = new float[nRows, nCols];
- }
- public Matrix2(float[,] data) {
- 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];
+ public readonly struct Slice
+ {
+ public uint start { get; }
+ public uint stop { get; }
+ public Slice(uint start, uint stop)
+ {
+ this.start = start;
+ this.stop = stop;
}
- return new Matrix2(data);
}
- public static Matrix2 Zero(uint nRows, uint nCols) {
- return new Matrix2(nRows, nCols);
- }
+ public class Matrix2
+ {
+ public float[,] data { get; }
- 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 uint nRows => (uint)data.GetLength(0);
+ public uint nCols => (uint)data.GetLength(1);
- public static Matrix2 Identity(uint size) {
- return Diagonal(1, size);
- }
- public static Matrix2 Identity(uint nRows, uint nCols) {
- Matrix2 m = Zero(nRows, nCols);
- m.FillDiagonal(1);
- return m;
- }
+ public Matrix2(uint nRows, uint nCols)
+ {
+ this.data = new float[nRows, nCols];
+ }
+ public Matrix2(float[,] data)
+ {
+ this.data = data;
+ }
- public static Matrix2 Diagonal(Matrix1 v) {
- 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);
- }
- public static Matrix2 Diagonal(float f, uint size) {
- float[,] resultData = new float[size, size];
- for (int ix = 0; ix < size; ix++)
- 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 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 SkewMatrix(Vector3Float v) {
- float[,] result = new float[3, 3] {
+ public static Matrix2 Zero(uint nRows, uint nCols)
+ {
+ 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);
+ }
+ 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.size, v.size];
+ for (int ix = 0; ix < v.size; ix++)
+ resultData[ix, ix] = v.data[ix];
+ return new Matrix2(resultData);
+ }
+ public static Matrix2 Diagonal(float f, uint size)
+ {
+ float[,] resultData = new float[size, size];
+ for (int ix = 0; ix < size; ix++)
+ 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] {
{0, -v.z, v.y},
{v.z, 0, -v.x},
{-v.y, v.x, 0}
};
- return new Matrix2(result);
- }
+ return new Matrix2(result);
+ }
#if UNITY_5_3_OR_NEWER
public Vector3Float GetRow3(int rowIx) {
@@ -101,336 +118,396 @@ public class Matrix2 {
};
return row;
}
-#endif
- 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;
- 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];
+#endif
+ 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;
+ this.data[rowIx, 2] = v.z;
}
- 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;
+ 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 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];
+ for (uint rowIx = 0; rowIx < this.nRows; rowIx++)
+ {
+ for (uint colIx = 0; colIx < this.nCols; colIx++)
+ resultData[colIx, rowIx] = this.data[rowIx, colIx];
+ }
+ 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];
+
+ for (int i = 0; i < m.nRows; i++)
+ {
+ for (int j = 0; j < m.nCols; j++)
+ result[i, j] = -m.data[i, j];
+ }
+ return new Matrix2(result);
+ }
+
+ public static Matrix2 operator -(Matrix2 A, Matrix2 B)
+ {
+ if (A.nRows != B.nRows || A.nCols != B.nCols)
+ throw new System.ArgumentException("Size of A must match size of B.");
+
+ float[,] result = new float[A.nRows, B.nCols];
+
+ for (int i = 0; i < A.nRows; i++)
+ {
+ for (int j = 0; j < A.nCols; j++)
+ result[i, j] = A.data[i, j] - B.data[i, j];
+ }
+ return new Matrix2(result);
+ }
+
+ public static Matrix2 operator +(Matrix2 A, Matrix2 B)
+ {
+ if (A.nRows != B.nRows || A.nCols != B.nCols)
+ throw new System.ArgumentException("Size of A must match size of B.");
+
+ float[,] result = new float[A.nRows, B.nCols];
+
+ for (int i = 0; i < A.nRows; i++)
+ {
+ for (int j = 0; j < A.nCols; j++)
+ result[i, j] = A.data[i, j] + B.data[i, j];
+ }
+ return new Matrix2(result);
+ }
+
+ public static Matrix2 operator *(Matrix2 A, Matrix2 B)
+ {
+ if (A.nCols != B.nRows)
+ throw new System.ArgumentException("Number of columns in A must match number of rows in B.");
+
+ float[,] result = new float[A.nRows, B.nCols];
+
+ for (int i = 0; i < A.nRows; i++)
+ {
+ for (int j = 0; j < B.nCols; j++)
+ {
+ float sum = 0.0f;
+ for (int k = 0; k < A.nCols; k++)
+ sum += A.data[i, k] * B.data[k, j];
+
+ result[i, j] = sum;
+ }
+ }
+
+ return new Matrix2(result);
+ // double checked code
+ }
+
+ public static Matrix1 operator *(Matrix2 A, Matrix1 v)
+ {
+ float[] result = new float[A.nRows];
+
+ for (int i = 0; i < A.nRows; i++)
+ {
+ for (int j = 0; j < A.nCols; j++)
+ {
+ result[i] += A.data[i, j] * v.data[j];
+ }
+ }
+
+ return new Matrix1(result);
+ }
+
+ public static Vector3Float operator *(Matrix2 A, Vector3Float v)
+ {
+ return new Vector3Float(
+ A.data[0, 0] * v.x + A.data[0, 1] * v.y + A.data[0, 2] * v.z,
+ A.data[1, 0] * v.x + A.data[1, 1] * v.y + A.data[1, 2] * v.z,
+ A.data[2, 0] * v.x + A.data[2, 1] * v.y + A.data[2, 2] * v.z
+ );
+ }
+
+ 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)
+ {
+ 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)
+ {
+ return Slice(slice.start, slice.stop);
+ }
+ 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 (uint 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 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 (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];
}
}
- return true;
- }
-
- public Matrix2 Transpose() {
- float[,] resultData = new float[this.nCols, this.nRows];
- for (uint rowIx = 0; rowIx < this.nRows; rowIx++) {
- for (uint colIx = 0; colIx < this.nCols; colIx++)
- resultData[colIx, rowIx] = this.data[rowIx, colIx];
+ public void UpdateSlice(Slice rowRange, Slice colRange, Matrix2 m)
+ {
+ UpdateSlice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop), m);
}
- 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];
-
- for (int i = 0; i < m.nRows; i++) {
- for (int j = 0; j < m.nCols; j++)
- result[i, j] = -m.data[i, j];
- }
- return new Matrix2(result);
- }
-
- public static Matrix2 operator -(Matrix2 A, Matrix2 B) {
- if (A.nRows != B.nRows || A.nCols != B.nCols)
- throw new System.ArgumentException("Size of A must match size of B.");
-
- float[,] result = new float[A.nRows, B.nCols];
-
- for (int i = 0; i < A.nRows; i++) {
- for (int j = 0; j < A.nCols; j++)
- result[i, j] = A.data[i, j] - B.data[i, j];
- }
- return new Matrix2(result);
- }
-
- public static Matrix2 operator +(Matrix2 A, Matrix2 B) {
- if (A.nRows != B.nRows || A.nCols != B.nCols)
- throw new System.ArgumentException("Size of A must match size of B.");
-
- float[,] result = new float[A.nRows, B.nCols];
-
- for (int i = 0; i < A.nRows; i++) {
- for (int j = 0; j < A.nCols; j++)
- result[i, j] = A.data[i, j] + B.data[i, j];
- }
- return new Matrix2(result);
- }
-
- public static Matrix2 operator *(Matrix2 A, Matrix2 B) {
- if (A.nCols != B.nRows)
- throw new System.ArgumentException("Number of columns in A must match number of rows in B.");
-
- float[,] result = new float[A.nRows, B.nCols];
-
- for (int i = 0; i < A.nRows; i++) {
- for (int j = 0; j < B.nCols; j++) {
- float sum = 0.0f;
- for (int k = 0; k < A.nCols; k++)
- sum += A.data[i, k] * B.data[k, j];
-
- result[i, j] = sum;
+ 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.start];
}
}
- return new Matrix2(result);
- // double checked code
- }
+ public Matrix2 Inverse()
+ {
+ Matrix2 A = this;
+ // unchecked
+ uint n = A.nRows;
- public static Matrix1 operator *(Matrix2 A, Matrix1 v) {
- float[] result = new float[A.nRows];
-
- for (int i = 0; i < A.nRows; i++) {
- for (int j = 0; j < A.nCols; j++) {
- result[i] += A.data[i, j] * v.data[j];
+ // Create an identity matrix of the same size as the original matrix
+ float[,] augmentedMatrix = new float[n, 2 * n];
+ for (int i = 0; i < n; i++)
+ {
+ for (int j = 0; j < n; j++)
+ {
+ augmentedMatrix[i, j] = A.data[i, j];
+ augmentedMatrix[i, j + n] = (i == j) ? 1 : 0; // Identity matrix
+ }
}
- }
- return new Matrix1(result);
- }
+ // Perform Gaussian elimination
+ for (int i = 0; i < n; i++)
+ {
+ // Find the pivot row
+ float pivot = augmentedMatrix[i, i];
+ if (Math.Abs(pivot) < 1e-10) // Check for singular matrix
+ throw new InvalidOperationException("Matrix is singular and cannot be inverted.");
- public static Vector3Float operator *(Matrix2 A, Vector3Float v) {
- return new Vector3Float(
- A.data[0, 0] * v.x + A.data[0, 1] * v.y + A.data[0, 2] * v.z,
- A.data[1, 0] * v.x + A.data[1, 1] * v.y + A.data[1, 2] * v.z,
- A.data[2, 0] * v.x + A.data[2, 1] * v.y + A.data[2, 2] * v.z
- );
- }
+ // Normalize the pivot row
+ for (int j = 0; j < 2 * n; j++)
+ augmentedMatrix[i, j] /= pivot;
- 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) {
- 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) {
- return Slice(slice.start, slice.stop);
- }
- 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 (uint rowIx = from; rowIx < to; rowIx++) {
- for (int colIx = 0; colIx < this.nCols; colIx++) {
- result[resultRowIx, colIx] = this.data[rowIx, colIx];
+ // Eliminate the column below the pivot
+ for (int j = i + 1; j < n; j++)
+ {
+ float factor = augmentedMatrix[j, i];
+ for (int k = 0; k < 2 * n; k++)
+ augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k];
+ }
}
- resultRowIx++;
- }
- 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 (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];
- }
- }
- 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) {
- 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.start];
- }
- }
-
- public Matrix2 Inverse() {
- Matrix2 A = this;
- // unchecked
- uint n = A.nRows;
-
- // Create an identity matrix of the same size as the original matrix
- float[,] augmentedMatrix = new float[n, 2 * n];
- for (int i = 0; i < n; i++) {
- for (int j = 0; j < n; j++) {
- augmentedMatrix[i, j] = A.data[i, j];
- augmentedMatrix[i, j + n] = (i == j) ? 1 : 0; // Identity matrix
+ // Back substitution
+ for (uint i = n - 1; i >= 0; i--)
+ {
+ // Eliminate the column above the pivot
+ for (uint j = i - 1; j >= 0; j--)
+ {
+ float factor = augmentedMatrix[j, i];
+ for (int k = 0; k < 2 * n; k++)
+ augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k];
+ }
}
- }
- // Perform Gaussian elimination
- for (int i = 0; i < n; i++) {
- // Find the pivot row
- float pivot = augmentedMatrix[i, i];
- if (Math.Abs(pivot) < 1e-10) // Check for singular matrix
- throw new InvalidOperationException("Matrix is singular and cannot be inverted.");
-
- // Normalize the pivot row
- for (int j = 0; j < 2 * n; j++)
- augmentedMatrix[i, j] /= pivot;
-
- // Eliminate the column below the pivot
- for (int j = i + 1; j < n; j++) {
- float factor = augmentedMatrix[j, i];
- for (int k = 0; k < 2 * n; k++)
- augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k];
+ // Extract the inverse matrix from the augmented matrix
+ float[,] inverse = new float[n, n];
+ for (int i = 0; i < n; i++)
+ {
+ for (int j = 0; j < n; j++)
+ inverse[i, j] = augmentedMatrix[i, j + n];
}
+
+ return new Matrix2(inverse);
}
- // Back substitution
- for (uint i = n - 1; i >= 0; i--) {
- // Eliminate the column above the pivot
- for (uint j = i - 1; j >= 0; j--) {
- float factor = augmentedMatrix[j, i];
- for (int k = 0; k < 2 * n; k++)
- augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k];
+ 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
+ {
+ public float[] data { get; }
+
+ public uint size => (uint)data.GetLength(0);
+
+ public Matrix1(uint size)
+ {
+ this.data = new float[size];
}
- // Extract the inverse matrix from the augmented matrix
- float[,] inverse = new float[n, n];
- for (int i = 0; i < n; i++) {
- for (int j = 0; j < n; j++)
- inverse[i, j] = augmentedMatrix[i, j + n];
+ public Matrix1(float[] data)
+ {
+ this.data = data;
}
- 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++;
+ public static Matrix1 Zero(uint size)
+ {
+ return new Matrix1(size);
}
- return new Matrix2(minor);
- }
-}
+ public static Matrix1 FromVector2(Vector2Float v)
+ {
+ float[] result = new float[2];
+ result[0] = v.x;
+ result[1] = v.y;
+ return new Matrix1(result);
+ }
-public class Matrix1 {
- public float[] data { get; }
-
- public uint size => (uint)data.GetLength(0);
-
- public Matrix1(uint size) {
- this.data = new float[size];
- }
-
- public Matrix1(float[] data) {
- this.data = data;
- }
-
- public static Matrix1 Zero(uint size) {
- return new Matrix1(size);
- }
-
- public static Matrix1 FromVector2(Vector2Float v) {
- float[] result = new float[2];
- result[0] = v.x;
- result[1] = v.y;
- return new Matrix1(result);
- }
-
- public static Matrix1 FromVector3(Vector3Float v) {
- float[] result = new float[3];
- result[0] = v.x;
- result[1] = v.y;
- result[2] = v.z;
- return new Matrix1(result);
- }
+ public static Matrix1 FromVector3(Vector3Float v)
+ {
+ float[] result = new float[3];
+ result[0] = v.x;
+ result[1] = v.y;
+ result[2] = v.z;
+ return new Matrix1(result);
+ }
#if UNITY_5_3_OR_NEWER
public static Matrix1 FromQuaternion(Quaternion q) {
@@ -443,20 +520,24 @@ public class Matrix1 {
}
#endif
- public Vector2Float vector2 {
- get {
- if (this.size != 2)
- throw new System.ArgumentException("Matrix1 must be of size 2");
- return new Vector2Float(this.data[0], this.data[1]);
+ public Vector2Float vector2
+ {
+ get
+ {
+ 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.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 Vector3Float vector3
+ {
+ get
+ {
+ 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]);
+ }
}
- }
#if UNITY_5_3_OR_NEWER
public Quaternion quaternion {
@@ -468,82 +549,97 @@ public class Matrix1 {
}
#endif
- 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 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 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.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.size != b.size)
+ throw new System.ArgumentException("Vectors must be of the same length.");
+
+ float result = 0.0f;
+ 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.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)
+ {
+ return A * f;
+ }
+
+ public Matrix1 Slice(Slice range)
+ {
+ return Slice(range.start, range.stop);
+ }
+ public Matrix1 Slice(uint from, uint 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++)
+ result[resultIx++] = this.data[ix];
+
+ return new Matrix1(result);
+ }
+ public void UpdateSlice(Slice slice, Matrix1 v)
+ {
+ int vIx = 0;
+ for (uint ix = slice.start; ix < slice.stop; ix++, vIx++)
+ this.data[ix] = v.data[vIx];
}
}
- 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.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.size != b.size)
- throw new System.ArgumentException("Vectors must be of the same length.");
-
- float result = 0.0f;
- 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.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) {
- return A * f;
- }
-
- public Matrix1 Slice(Slice range) {
- return Slice(range.start, range.stop);
- }
- public Matrix1 Slice(uint from, uint 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++)
- result[resultIx++] = this.data[ix];
-
- return new Matrix1(result);
- }
- public void UpdateSlice(Slice slice, Matrix1 v) {
- int vIx = 0;
- 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/src/Quat32.cs b/LinearAlgebra/src/Quat32.cs
index 6bebd04..3026f8e 100644
--- a/LinearAlgebra/src/Quat32.cs
+++ b/LinearAlgebra/src/Quat32.cs
@@ -1,6 +1,6 @@
using System;
-namespace Passer.LinearAlgebra
+namespace LinearAlgebra
{
public class Quat32
{
diff --git a/LinearAlgebra/src/Quaternion.cs b/LinearAlgebra/src/Quaternion.cs
index 57f8f5d..f481ef4 100644
--- a/LinearAlgebra/src/Quaternion.cs
+++ b/LinearAlgebra/src/Quaternion.cs
@@ -2,15 +2,19 @@ using System;
#if UNITY_5_3_OR_NEWER
using Quaternion = UnityEngine.Quaternion;
#endif
-namespace Passer.LinearAlgebra {
- public class QuaternionOf {
+namespace LinearAlgebra
+{
+
+ public class QuaternionOf
+ {
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;
diff --git a/LinearAlgebra/src/Spherical.cs b/LinearAlgebra/src/Spherical.cs
index be0c38a..497a508 100644
--- a/LinearAlgebra/src/Spherical.cs
+++ b/LinearAlgebra/src/Spherical.cs
@@ -3,35 +3,42 @@ using System;
using Vector3Float = UnityEngine.Vector3;
#endif
-namespace Passer.LinearAlgebra {
- public class Spherical {
+namespace LinearAlgebra
+{
+ public class Spherical
+ {
public float distance;
public Direction direction;
public static Spherical zero = new Spherical(0, 0, 0);
public static Spherical forward = new Spherical(1, 0, 0);
- public Spherical(float distance, float horizontal, float vertical) {
+ public Spherical(float distance, float horizontal, float vertical)
+ {
this.distance = distance;
this.direction = new Direction(horizontal, vertical);
}
- public Spherical(float distance, Direction direction) {
+ public Spherical(float distance, Direction direction)
+ {
this.distance = distance;
this.direction = direction;
}
- public static Spherical FromVector3(Vector3Float v) {
+ public static Spherical FromVector3(Vector3Float v)
+ {
float distance = v.magnitude;
if (distance == 0.0f)
return new Spherical(distance, 0, 0);
- else {
+ else
+ {
float verticalAngle = (float)((Angle.pi / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg);
- float horizontalAngle = (float) Math.Atan2(v.x, v.z) * Angle.Rad2Deg;
+ float horizontalAngle = (float)Math.Atan2(v.x, v.z) * Angle.Rad2Deg;
return new Spherical(distance, horizontalAngle, verticalAngle);
}
}
- public Vector3Float ToVector3() {
+ public Vector3Float ToVector3()
+ {
float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad;
float horizontalRad = this.direction.horizontal * Angle.Deg2Rad;
float cosVertical = (float)Math.Cos(verticalRad);
diff --git a/LinearAlgebra/src/SwingTwist.cs b/LinearAlgebra/src/SwingTwist.cs
index 7f4bbfd..a7a73da 100644
--- a/LinearAlgebra/src/SwingTwist.cs
+++ b/LinearAlgebra/src/SwingTwist.cs
@@ -1,4 +1,4 @@
-namespace Passer.LinearAlgebra
+namespace LinearAlgebra
{
public class SwingTwist
diff --git a/LinearAlgebra/src/Vector2.cs b/LinearAlgebra/src/Vector2.cs
index c7d00a4..603b0de 100644
--- a/LinearAlgebra/src/Vector2.cs
+++ b/LinearAlgebra/src/Vector2.cs
@@ -1,50 +1,63 @@
using System;
using System.Numerics;
-namespace Passer.LinearAlgebra {
+namespace LinearAlgebra
+{
- public class Vector2Of where T : IComparable {
+ public class Vector2Of where T : IComparable
+ {
public T x;
public T y;
- public Vector2Of(T x, T y) {
+ public Vector2Of(T x, T y)
+ {
this.x = x;
this.y = y;
}
}
- public class Vector2Int : Vector2Of {
+ public class Vector2Int : Vector2Of
+ {
public Vector2Int(int x, int y) : base(x, y) { }
- public static Vector2Int operator -(Vector2Int v1, Vector2Int v2) {
+ public static Vector2Int operator -(Vector2Int v1, Vector2Int v2)
+ {
return new Vector2Int(v1.x - v2.x, v1.y - v2.y);
}
- public float magnitude {
- get {
+ public float magnitude
+ {
+ get
+ {
return (float)Math.Sqrt(this.x * this.x + this.y * this.y);
}
}
- public static float Distance(Vector2Int v1, Vector2Int v2) {
+ public static float Distance(Vector2Int v1, Vector2Int v2)
+ {
return (v1 - v2).magnitude;
}
}
- public class Vector2Float : Vector2Of {
+ public class Vector2Float : Vector2Of
+ {
public Vector2Float(float x, float y) : base(x, y) { }
- public static Vector2Float operator -(Vector2Float v1, Vector2Float v2) {
+ public static Vector2Float operator -(Vector2Float v1, Vector2Float v2)
+ {
return new Vector2Float(v1.x - v2.x, v1.y - v2.y);
}
- public float magnitude {
- get {
+ public float magnitude
+ {
+ get
+ {
return (float)Math.Sqrt(this.x * this.x + this.y * this.y);
}
}
- public static float Distance(Vector2Float v1, Vector2Float v2) {
+ public static float Distance(Vector2Float v1, Vector2Float v2)
+ {
return (v1 - v2).magnitude;
}
}
@@ -52,7 +65,8 @@ namespace Passer.LinearAlgebra {
///
/// 2-dimensional vectors
///
- public struct Vector2 : IEquatable {
+ public struct Vector2 : IEquatable
+ {
///
/// The right axis of the vector
@@ -69,7 +83,8 @@ namespace Passer.LinearAlgebra {
///
/// x axis value
/// y axis value
- public Vector2(float x, float y) {
+ public Vector2(float x, float y)
+ {
this.x = x;
this.y = y;
}
@@ -114,8 +129,10 @@ namespace Passer.LinearAlgebra {
/// The squared length is computationally simpler than the real length.
/// Think of Pythagoras A^2 + B^2 = C^2.
/// This leaves out the calculation of the squared root of C.
- public float sqrMagnitude {
- get {
+ public float sqrMagnitude
+ {
+ get
+ {
float d = x * x + y * y;
return d;
}
@@ -125,8 +142,10 @@ namespace Passer.LinearAlgebra {
/// The length of this vector
///
/// The length of this vector
- public float magnitude {
- get {
+ public float magnitude
+ {
+ get
+ {
float d = (float)Math.Sqrt(x * x + y * y);
return d;
}
@@ -136,8 +155,10 @@ namespace Passer.LinearAlgebra {
/// Convert the vector to a length of a 1
///
/// The vector with length 1
- public Vector2 normalized {
- get {
+ public Vector2 normalized
+ {
+ get
+ {
float l = magnitude;
Vector2 v = zero;
if (l > Float.epsilon)
@@ -152,7 +173,8 @@ namespace Passer.LinearAlgebra {
/// The first vector
/// The second vector
/// The result of adding the two vectors
- public static Vector2 operator +(Vector2 v1, Vector2 v2) {
+ public static Vector2 operator +(Vector2 v1, Vector2 v2)
+ {
Vector2 v = new Vector2(v1.x + v2.x, v1.y + v2.y);
return v;
}
@@ -163,7 +185,8 @@ namespace Passer.LinearAlgebra {
/// The first vector
/// The second vector
/// The result of adding the two vectors
- public static Vector2 operator -(Vector2 v1, Vector2 v2) {
+ public static Vector2 operator -(Vector2 v1, Vector2 v2)
+ {
Vector2 v = new Vector2(v1.x - v2.x, v1.y - v2.y);
return v;
}
@@ -174,7 +197,8 @@ namespace Passer.LinearAlgebra {
/// The vector to negate
/// The negated vector
/// This will result in a vector pointing in the opposite direction
- public static Vector2 operator -(Vector2 v1) {
+ public static Vector2 operator -(Vector2 v1)
+ {
Vector2 v = new Vector2(-v1.x, -v1.y);
return v;
}
@@ -186,7 +210,8 @@ namespace Passer.LinearAlgebra {
/// The scaling factor
/// The scaled vector
/// Each component of the vector will be multipled with the same factor.
- public static Vector2 operator *(Vector2 v1, float f) {
+ public static Vector2 operator *(Vector2 v1, float f)
+ {
Vector2 v = new Vector2(v1.x * f, v1.y * f);
return v;
}
@@ -198,7 +223,8 @@ namespace Passer.LinearAlgebra {
/// The vector to scale
/// The scaled vector
/// Each component of the vector will be multipled with the same factor.
- public static Vector2 operator *(float f, Vector2 v1) {
+ public static Vector2 operator *(float f, Vector2 v1)
+ {
Vector2 v = new Vector2(f * v1.x, f * v1.y);
return v;
}
@@ -210,7 +236,8 @@ namespace Passer.LinearAlgebra {
/// The scaling factor
/// The scaled vector
/// Each component of the vector will be devided by the same factor.
- public static Vector2 operator /(Vector2 v1, float f) {
+ public static Vector2 operator /(Vector2 v1, float f)
+ {
Vector2 v = new Vector2(v1.x / f, v1.y / f);
return v;
}
@@ -227,7 +254,8 @@ namespace Passer.LinearAlgebra {
///
/// The object to compare to
/// false when the object is not a Vector2 or does not have equal values
- public override bool Equals(object obj) {
+ public override bool Equals(object obj)
+ {
if (!(obj is Vector2 v))
return false;
@@ -243,7 +271,8 @@ namespace Passer.LinearAlgebra {
/// Note that this uses a Float equality check which cannot be not exact in all cases.
/// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon
/// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
- public static bool operator ==(Vector2 v1, Vector2 v2) {
+ public static bool operator ==(Vector2 v1, Vector2 v2)
+ {
return (v1.x == v2.x && v1.y == v2.y);
}
@@ -256,7 +285,8 @@ namespace Passer.LinearAlgebra {
/// Note that this uses a Float equality check which cannot be not exact in all case.
/// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon.
/// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon
- public static bool operator !=(Vector2 v1, Vector2 v2) {
+ public static bool operator !=(Vector2 v1, Vector2 v2)
+ {
return (v1.x != v2.x || v1.y != v2.y);
}
@@ -264,7 +294,8 @@ namespace Passer.LinearAlgebra {
/// Get an hash code for the vector
///
/// The hash code
- public override int GetHashCode() {
+ public override int GetHashCode()
+ {
return (x, y).GetHashCode();
}
@@ -274,7 +305,8 @@ namespace Passer.LinearAlgebra {
/// The first vector
/// The second vector
/// The distance between the two vectors
- public static float Distance(Vector2 v1, Vector2 v2) {
+ public static float Distance(Vector2 v1, Vector2 v2)
+ {
float x = v1.x - v2.x;
float y = v1.y - v2.y;
float d = (float)Math.Sqrt(x * x + y * y);
@@ -287,7 +319,8 @@ namespace Passer.LinearAlgebra {
/// The first vector
/// The second vector
/// The dot product of the two vectors
- public static float Dot(Vector2 v1, Vector2 v2) {
+ public static float Dot(Vector2 v1, Vector2 v2)
+ {
return v1.x * v2.x + v1.y * v2.y;
}
@@ -301,7 +334,8 @@ namespace Passer.LinearAlgebra {
/// The factor f is unclamped. Value 0 matches the *v1* vector, Value 1
/// matches the *v2* vector Value -1 is *v1* vector minus the difference
/// between *v1* and *v2* etc.
- public static Vector2 Lerp(Vector2 v1, Vector2 v2, float f) {
+ public static Vector2 Lerp(Vector2 v1, Vector2 v2, float f)
+ {
Vector2 v = v1 + (v2 - v1) * f;
return v;
}
@@ -313,7 +347,8 @@ namespace Passer.LinearAlgebra {
/// The ending vector
/// The axis to rotate around
/// The signed angle in degrees
- public static float SignedAngle(Vector2 from, Vector2 to) {
+ public static float SignedAngle(Vector2 from, Vector2 to)
+ {
//float sign = Math.Sign(v1.y * v2.x - v1.x * v2.y);
//return Vector2.Angle(v1, v2) * sign;
@@ -336,13 +371,15 @@ namespace Passer.LinearAlgebra {
/// The vector to rotate
/// The angle in degrees
///
- public static Vector2 Rotate(Vector2 v1, float angle) {
+ public static Vector2 Rotate(Vector2 v1, float angle)
+ {
float sin = (float)Math.Sin(angle * Angle.Deg2Rad);
float cos = (float)Math.Cos(angle * Angle.Deg2Rad);
float tx = v1.x;
float ty = v1.y;
- Vector2 v = new Vector2() {
+ Vector2 v = new Vector2()
+ {
x = (cos * tx) - (sin * ty),
y = (sin * tx) + (cos * ty)
};
@@ -356,7 +393,8 @@ namespace Passer.LinearAlgebra {
/// The second vector
/// The resulting factor in interval [0..1]
/// Vectors a and b must be normalized
- public static float ToFactor(Vector2 v1, Vector2 v2) {
+ public static float ToFactor(Vector2 v1, Vector2 v2)
+ {
return (1 - Vector2.Dot(v1, v2)) / 2;
}
}
diff --git a/LinearAlgebra/src/Vector3.cs b/LinearAlgebra/src/Vector3.cs
index 3b17b06..616b82e 100644
--- a/LinearAlgebra/src/Vector3.cs
+++ b/LinearAlgebra/src/Vector3.cs
@@ -1,13 +1,16 @@
#if !UNITY_5_3_OR_NEWER
using System;
-namespace Passer.LinearAlgebra {
- public class Vector3Of {
+namespace LinearAlgebra
+{
+ public class Vector3Of
+ {
public T x;
public T y;
public T z;
- public Vector3Of(T x, T y, T z) {
+ public Vector3Of(T x, T y, T z)
+ {
this.x = x;
this.y = y;
this.z = z;
@@ -18,13 +21,16 @@ namespace Passer.LinearAlgebra {
// }
}
- public class Vector3Int : Vector3Of {
+ public class Vector3Int : Vector3Of
+ {
public Vector3Int(int x, int y, int z) : base(x, y, z) { }
}
- public class Vector3Float : Vector3Of {
+ public class Vector3Float : Vector3Of
+ {
public Vector3Float(float x, float y, float z) : base(x, y, z) { }
- public float magnitude {
+ public float magnitude
+ {
get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
}
}
@@ -33,7 +39,8 @@ namespace Passer.LinearAlgebra {
/// 3-dimensional vectors
///
/// This uses the right-handed coordinate system.
- public struct Vector3 : IEquatable {
+ public struct Vector3 : IEquatable
+ {
///
/// The right axis of the vector
@@ -54,7 +61,8 @@ namespace Passer.LinearAlgebra {
/// x axis value
/// y axis value
/// z axis value
- public Vector3(float x, float y, float z) {
+ public Vector3(float x, float y, float z)
+ {
this.x = x;
this.y = y;
this.z = z;
@@ -93,15 +101,19 @@ namespace Passer.LinearAlgebra {
///
public static readonly Vector3 forward = new Vector3(0, 1, 0);
- public float magnitude {
- get {
+ public float magnitude
+ {
+ get
+ {
float d = (float)Math.Sqrt(x * x + y * y);
return d;
}
}
- public Vector3 normalized {
- get {
+ public Vector3 normalized
+ {
+ get
+ {
float l = magnitude;
Vector3 v = zero;
if (l > Float.epsilon)
@@ -110,66 +122,79 @@ namespace Passer.LinearAlgebra {
}
}
- public static Vector3 operator +(Vector3 v1, Vector3 v2) {
+ public static Vector3 operator +(Vector3 v1, Vector3 v2)
+ {
Vector3 v = new Vector3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
return v;
}
- public static Vector3 operator -(Vector3 v1, Vector3 v2) {
+ public static Vector3 operator -(Vector3 v1, Vector3 v2)
+ {
Vector3 v = new Vector3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
return v;
}
- public static Vector3 operator -(Vector3 v1) {
+ public static Vector3 operator -(Vector3 v1)
+ {
Vector3 v = new Vector3(-v1.x, -v1.y, -v1.z);
return v;
}
- public static Vector3 operator *(Vector3 v1, float d) {
+ public static Vector3 operator *(Vector3 v1, float d)
+ {
Vector3 v = new Vector3(v1.x * d, v1.y * d, v1.z * d);
return v;
}
- public static Vector3 operator *(float d, Vector3 v1) {
+ public static Vector3 operator *(float d, Vector3 v1)
+ {
Vector3 v = new Vector3(d * v1.x, d * v1.y, d * v1.z);
return v;
}
- public static Vector3 operator /(Vector3 v1, float d) {
+ public static Vector3 operator /(Vector3 v1, float d)
+ {
Vector3 v = new Vector3(v1.x / d, v1.y / d, v1.z / d);
return v;
}
public bool Equals(Vector3 v) => (x == v.x && y == v.y && z == v.z);
- public override bool Equals(object obj) {
+ public override bool Equals(object obj)
+ {
if (!(obj is Vector3 v))
return false;
return (x == v.x && y == v.y && z == v.z);
}
- public static bool operator ==(Vector3 v1, Vector3 v2) {
+ public static bool operator ==(Vector3 v1, Vector3 v2)
+ {
return (v1.x == v2.x && v1.y == v2.y && v1.z == v2.z);
}
- public static bool operator !=(Vector3 v1, Vector3 v2) {
+ public static bool operator !=(Vector3 v1, Vector3 v2)
+ {
return (v1.x != v2.x || v1.y != v2.y || v1.z != v2.z);
}
- public override int GetHashCode() {
+ public override int GetHashCode()
+ {
return (x, y, z).GetHashCode();
}
- public static float Distance(Vector3 v1, Vector3 v2) {
+ public static float Distance(Vector3 v1, Vector3 v2)
+ {
return (v2 - v1).magnitude;
}
- public static float Dot(Vector3 v1, Vector3 v2) {
+ public static float Dot(Vector3 v1, Vector3 v2)
+ {
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}
- public static Vector3 Lerp(Vector3 v1, Vector3 v2, float f) {
+ public static Vector3 Lerp(Vector3 v1, Vector3 v2, float f)
+ {
Vector3 v = v1 + (v2 - v1) * f;
return v;
}
diff --git a/LinearAlgebra/src/float16.cs b/LinearAlgebra/src/float16.cs
index 4195ee6..5d94014 100644
--- a/LinearAlgebra/src/float16.cs
+++ b/LinearAlgebra/src/float16.cs
@@ -1,8 +1,10 @@
using System;
-namespace Passer.LinearAlgebra {
+namespace LinearAlgebra
+{
- public class float16 {
+ public class float16
+ {
//
// FILE: float16.cpp
// AUTHOR: Rob Tillaart
@@ -14,12 +16,14 @@ namespace Passer.LinearAlgebra {
public float16() { _value = 0; }
- public float16(float f) {
+ public float16(float f)
+ {
//_value = f32tof16(f);
_value = F32ToF16__(f);
}
- public float toFloat() {
+ public float toFloat()
+ {
return f16tof32(_value);
}
@@ -153,7 +157,8 @@ namespace Passer.LinearAlgebra {
//
// CORE CONVERSION
//
- float f16tof32(ushort _value) {
+ float f16tof32(ushort _value)
+ {
//ushort sgn;
ushort man;
int exp;
@@ -168,11 +173,13 @@ namespace Passer.LinearAlgebra {
//Debug.Log($"{sgn} {exp} {man}");
// ZERO
- if ((_value & 0x7FFF) == 0) {
+ if ((_value & 0x7FFF) == 0)
+ {
return sgn ? -0 : 0;
}
// NAN & INF
- if (exp == 0x001F) {
+ if (exp == 0x001F)
+ {
if (man == 0)
return sgn ? float.NegativeInfinity : float.PositiveInfinity; //-INFINITY : INFINITY;
else
@@ -186,43 +193,50 @@ namespace Passer.LinearAlgebra {
f = 1;
// PROCESS MANTISSE
- for (int i = 9; i >= 0; i--) {
+ for (int i = 9; i >= 0; i--)
+ {
f *= 2;
if ((man & (1 << i)) != 0)
f = f + 1;
}
//Debug.Log($"{f}");
f = f * (float)Math.Pow(2.0f, exp - 25);
- if (exp == 0) {
+ if (exp == 0)
+ {
f = f * (float)Math.Pow(2.0f, -13); // 5.96046447754e-8;
}
//Debug.Log($"{f}");
return sgn ? -f : f;
}
- public static uint SingleToInt32Bits(float value) {
+ public static uint SingleToInt32Bits(float value)
+ {
byte[] bytes = BitConverter.GetBytes(value);
if (BitConverter.IsLittleEndian)
Array.Reverse(bytes); // If the system is little-endian, reverse the byte order
return BitConverter.ToUInt32(bytes, 0);
}
- public ushort F32ToF16__(float f) {
+ public ushort F32ToF16__(float f)
+ {
uint t = BitConverter.ToUInt32(BitConverter.GetBytes(f), 0);
ushort man = (ushort)((t & 0x007FFFFF) >> 12);
int exp = (int)((t & 0x7F800000) >> 23);
bool sgn = (t & 0x80000000) != 0;
// handle 0
- if ((t & 0x7FFFFFFF) == 0) {
+ if ((t & 0x7FFFFFFF) == 0)
+ {
return sgn ? (ushort)0x8000 : (ushort)0x0000;
}
// denormalized float32 does not fit in float16
- if (exp == 0x00) {
+ if (exp == 0x00)
+ {
return sgn ? (ushort)0x8000 : (ushort)0x0000;
}
// handle infinity & NAN
- if (exp == 0x00FF) {
+ if (exp == 0x00FF)
+ {
if (man != 0)
return 0xFE00; // NAN
return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF
@@ -231,11 +245,13 @@ namespace Passer.LinearAlgebra {
// normal numbers
exp = exp - 127 + 15;
// overflow does not fit => INF
- if (exp > 30) {
+ if (exp > 30)
+ {
return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF
}
// subnormal numbers
- if (exp < -38) {
+ if (exp < -38)
+ {
return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ?
}
if (exp <= 0) // subnormal
@@ -260,7 +276,8 @@ namespace Passer.LinearAlgebra {
}
//This function is faulty!!!!
- ushort f32tof16(float f) {
+ ushort f32tof16(float f)
+ {
//uint t = *(uint*)&f;
//uint t = (uint)BitConverter.SingleToInt32Bits(f);
uint t = SingleToInt32Bits(f);
@@ -270,15 +287,18 @@ namespace Passer.LinearAlgebra {
bool sgn = (t & 0x80000000) != 0;
// handle 0
- if ((t & 0x7FFFFFFF) == 0) {
+ if ((t & 0x7FFFFFFF) == 0)
+ {
return sgn ? (ushort)0x8000 : (ushort)0x0000;
}
// denormalized float32 does not fit in float16
- if (exp == 0x00) {
+ if (exp == 0x00)
+ {
return sgn ? (ushort)0x8000 : (ushort)0x0000;
}
// handle infinity & NAN
- if (exp == 0x00FF) {
+ if (exp == 0x00FF)
+ {
if (man != 0)
return 0xFE00; // NAN
return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF
@@ -287,11 +307,13 @@ namespace Passer.LinearAlgebra {
// normal numbers
exp = (short)(exp - 127 + 15);
// overflow does not fit => INF
- if (exp > 30) {
+ if (exp > 30)
+ {
return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF
}
// subnormal numbers
- if (exp < -38) {
+ if (exp < -38)
+ {
return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ?
}
if (exp <= 0) // subnormal
diff --git a/LinearAlgebra/test/AngleTest.cs b/LinearAlgebra/test/AngleTest.cs
index 85f0cc0..75522f3 100644
--- a/LinearAlgebra/test/AngleTest.cs
+++ b/LinearAlgebra/test/AngleTest.cs
@@ -1,12 +1,17 @@
-#if NUNIT
using NUnit.Framework;
-using Passer.LinearAlgebra;
-namespace LinearAlgebraTest {
- public class AngleTest {
+namespace LinearAlgebra.Test
+{
+ public class Tests
+ {
+ [SetUp]
+ public void Setup()
+ {
+ }
[Test]
- public void Normalize() {
+ public void Test_Normalize()
+ {
float r = 0;
r = Angle.Normalize(90);
@@ -38,7 +43,8 @@ namespace LinearAlgebraTest {
}
[Test]
- public void Clamp() {
+ public void Clamp()
+ {
float r = 0;
r = Angle.Clamp(1, 0, 2);
@@ -63,11 +69,12 @@ namespace LinearAlgebraTest {
Assert.AreEqual(r, 1, "Clamp 1 0 INFINITY");
r = Angle.Clamp(1, float.NegativeInfinity, 1);
- Assert.AreEqual(r, 1, "Clamp 1 -INFINITY 1");
+ Assert.AreEqual(r, 1, "Clamp 1 -INFINITY 1");
}
[Test]
- public void Difference() {
+ public void Difference()
+ {
float r = 0;
r = Angle.Difference(0, 90);
@@ -105,7 +112,8 @@ namespace LinearAlgebraTest {
}
[Test]
- public void MoveTowards() {
+ public void MoveTowards()
+ {
float r = 0;
r = Angle.MoveTowards(0, 90, 30);
@@ -158,5 +166,4 @@ namespace LinearAlgebraTest {
}
}
-}
-#endif
\ No newline at end of file
+}
\ No newline at end of file
diff --git a/src/Messages/LowLevelMessages.cs b/src/Messages/LowLevelMessages.cs
index 27835e6..39da789 100644
--- a/src/Messages/LowLevelMessages.cs
+++ b/src/Messages/LowLevelMessages.cs
@@ -1,19 +1,15 @@
-using Passer.LinearAlgebra;
+using LinearAlgebra;
-namespace RoboidControl
-{
- public class LowLevelMessages
- {
+namespace RoboidControl {
+ public class LowLevelMessages {
- public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v)
- {
+ public static void SendSpherical(byte[] buffer, ref byte ix, Spherical v) {
SendFloat16(buffer, ref ix, new float16(v.distance));
SendAngle8(buffer, ref ix, v.direction.horizontal);
SendAngle8(buffer, ref ix, v.direction.vertical);
}
- public static Spherical ReceiveSpherical(byte[] data, ref byte ix)
- {
+ public static Spherical ReceiveSpherical(byte[] data, ref byte ix) {
float distance = ReceiveFloat16(data, ref ix);
float horizontal = ReceiveAngle8(data, ref ix);
float vertical = ReceiveAngle8(data, ref ix);
@@ -21,20 +17,17 @@ namespace RoboidControl
return v;
}
- public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s)
- {
+ public static void SendQuat32(byte[] buffer, ref byte ix, SwingTwist s) {
Quat32 q32 = Quat32.FromSwingTwist(s);
SendQuat32(buffer, ref ix, q32);
}
- public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q)
- {
+ public static void SendQuat32(byte[] buffer, ref byte ix, Quat32 q) {
int qx = (int)(q.x * 127 + 128);
int qy = (int)(q.y * 127 + 128);
int qz = (int)(q.z * 127 + 128);
int qw = (int)(q.w * 255);
- if (q.w < 0)
- {
+ if (q.w < 0) {
qx = -qx;
qy = -qy;
qz = -qz;
@@ -46,8 +39,7 @@ namespace RoboidControl
buffer[ix++] = (byte)qz;
buffer[ix++] = (byte)qw;
}
- public static Quat32 ReceiveQuat32(byte[] data, ref byte ix)
- {
+ public static Quat32 ReceiveQuat32(byte[] data, ref byte ix) {
Quat32 q = new Quat32(
(data[ix++] - 128.0F) / 127.0F,
(data[ix++] - 128.0F) / 127.0F,
@@ -56,8 +48,7 @@ namespace RoboidControl
return q;
}
- public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix)
- {
+ public static SwingTwist ReceiveSwingTwist(byte[] data, ref byte ix) {
Quat32 q32 = ReceiveQuat32(data, ref ix);
// UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w);
// SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z);
@@ -66,8 +57,7 @@ namespace RoboidControl
return r;
}
- public static void SendAngle8(byte[] buffer, ref byte ix, float angle)
- {
+ public static void SendAngle8(byte[] buffer, ref byte ix, float angle) {
// Normalize angle
while (angle >= 180)
angle -= 360;
@@ -77,28 +67,24 @@ namespace RoboidControl
buffer[ix++] = (byte)value;
}
- public static float ReceiveAngle8(byte[] data, ref byte ix)
- {
+ public static float ReceiveAngle8(byte[] data, ref byte ix) {
float value = (data[ix++] * 180) / 128.0F;
return value;
}
- public static void SendFloat16(byte[] data, ref byte ix, float f)
- {
+ public static void SendFloat16(byte[] data, ref byte ix, float f) {
float16 f16 = new float16(f);
ushort binary = f16.GetBinary();
data[ix++] = (byte)(binary >> 8);
data[ix++] = (byte)(binary & 255);
}
- public static void SendFloat16(byte[] data, ref byte ix, float16 f)
- {
+ public static void SendFloat16(byte[] data, ref byte ix, float16 f) {
ushort binary = f.GetBinary();
data[ix++] = (byte)((binary >> 8) & 0xFF);
data[ix++] = (byte)(binary & 0xFF);
}
- public static float ReceiveFloat16(byte[] data, ref byte ix)
- {
+ public static float ReceiveFloat16(byte[] data, ref byte ix) {
byte msb = data[ix++];
byte lsb = data[ix++];
ushort value = (ushort)(msb << 8 | lsb);
diff --git a/src/Messages/PoseMsg.cs b/src/Messages/PoseMsg.cs
index 5edd583..f29588f 100644
--- a/src/Messages/PoseMsg.cs
+++ b/src/Messages/PoseMsg.cs
@@ -1,4 +1,4 @@
-using Passer.LinearAlgebra;
+using LinearAlgebra;
namespace RoboidControl {
diff --git a/src/Thing.cs b/src/Thing.cs
index 3f12b62..bf5b109 100644
--- a/src/Thing.cs
+++ b/src/Thing.cs
@@ -1,7 +1,6 @@
using System;
using System.Collections.Generic;
-using System.Diagnostics;
-using Passer.LinearAlgebra;
+using LinearAlgebra;
namespace RoboidControl {