From 7ecc78ee557798b58b68d1ed8bc3e3033e252ae3 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 12 Mar 2025 14:52:14 +0100 Subject: [PATCH 01/20] Migrated to RoboidControl --- .gitignore | 6 + src/Angle.cs | 117 ++++++ src/Direction.cs | 60 +++ src/Float.cs | 45 +++ src/LinearAlgebra.csproj | 14 + src/Matrix.cs | 645 +++++++++++++++++++++++++++++++++ src/Quat32.cs | 98 +++++ src/Quaternion.cs | 80 ++++ src/Spherical.cs | 57 +++ src/SwingTwist.cs | 46 +++ src/Vector2.cs | 401 ++++++++++++++++++++ src/Vector3.cs | 204 +++++++++++ src/float16.cs | 344 ++++++++++++++++++ test/AngleTest.cs | 171 +++++++++ test/LinearAlgebra_Test.csproj | 19 + 15 files changed, 2307 insertions(+) create mode 100644 .gitignore create mode 100644 src/Angle.cs create mode 100644 src/Direction.cs create mode 100644 src/Float.cs create mode 100644 src/LinearAlgebra.csproj create mode 100644 src/Matrix.cs create mode 100644 src/Quat32.cs create mode 100644 src/Quaternion.cs create mode 100644 src/Spherical.cs create mode 100644 src/SwingTwist.cs create mode 100644 src/Vector2.cs create mode 100644 src/Vector3.cs create mode 100644 src/float16.cs create mode 100644 test/AngleTest.cs create mode 100644 test/LinearAlgebra_Test.csproj diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..f0b2f47 --- /dev/null +++ b/.gitignore @@ -0,0 +1,6 @@ +DoxyGen/DoxyWarnLogfile.txt +.vscode/settings.json +**bin +**obj +**.meta +*.sln diff --git a/src/Angle.cs b/src/Angle.cs new file mode 100644 index 0000000..c3dd711 --- /dev/null +++ b/src/Angle.cs @@ -0,0 +1,117 @@ +using System; + +namespace LinearAlgebra +{ + + /// + /// %Angle utilities + /// + 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; + + public const float Rad2Deg = 360.0f / ((float)Math.PI * 2); //0.0174532924F; + public const float Deg2Rad = ((float)Math.PI * 2) / 360.0f; //57.29578F; + + /// + /// Clamp the angle between the given min and max values + /// + /// The angle to clamp + /// The minimum angle + /// The maximum angle + /// The clamped angle + /// Angles are normalized + public static float Clamp(float angle, float min, float max) + { + float normalizedAngle = Normalize(angle); + return Float.Clamp(normalizedAngle, min, max); + } + + /// + /// Determine the angle difference, result is a normalized angle + /// + /// First first angle + /// The second angle + /// the angle between the two angles + /// Angle values should be degrees + public static float Difference(float a, float b) + { + float r = Normalize(b - a); + return r; + } + + /// + /// Normalize an angle to the range -180 < angle <= 180 + /// + /// The angle to normalize + /// The normalized angle in interval (-180..180] + /// Angle values should be in degrees + public static float Normalize(float angle) + { + if (float.IsInfinity(angle)) + return angle; + + while (angle <= -180) angle += 360; + while (angle > 180) angle -= 360; + return angle; + } + + /// + /// Rotate from one angle to the other with a maximum degrees + /// + /// Starting angle + /// Target angle + /// 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) + { + float d = toAngle - fromAngle; + d = Normalize(d); + d = Math.Sign(d) * Float.Clamp(Math.Abs(d), 0, maxAngle); + return fromAngle + d; + } + + /// + /// Map interval of angles between vectors [0..Pi] to interval [0..1] + /// + /// The first vector + /// The second vector + /// The resulting factor in interval [0..1] + /// 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) + { + return (1 - Vector2.Dot(v1, v2)) / 2; + } + + // Normalize all vector angles to the range -180 < angle < 180 + //public static Vector3 Normalize(Vector3 angles) { + // float x = Normalize(angles.x); + // float y = Normalize(angles.y); + // float z = Normalize(angles.z); + // return new Vector3(x, y, z); + //} + + // Returns the signed angle in degrees between from and to. + //public static float SignedAngle(Vector3 from, Vector3 to) { + // float angle = Vector3.Angle(from, to); + // Vector3 cross = Vector3.Cross(from, to); + // if (cross.y < 0) angle = -angle; + // return angle; + //} + + // Returns the signed angle in degrees between from and to. + //public static float SignedAngle(Vector2 from, Vector2 to) { + // float sign = Math.Sign(from.y * to.x - from.x * to.y); + // return Vector2.Angle(from, to) * sign; + //} + + //public static Quaternion ToQuaternion(Rotation orientation) { + // return new Quaternion(orientation.x, orientation.y, orientation.z, orientation.w); + //} + } +} \ No newline at end of file diff --git a/src/Direction.cs b/src/Direction.cs new file mode 100644 index 0000000..d16ac72 --- /dev/null +++ b/src/Direction.cs @@ -0,0 +1,60 @@ +using System; +#if UNITY_5_3_OR_NEWER +using Vector3Float = UnityEngine.Vector3; +#endif + +namespace LinearAlgebra +{ + + public class Direction + { + public float horizontal; + public float vertical; + + public Direction() + { + horizontal = 0; + vertical = 0; + } + public Direction(float horizontal, float vertical) + { + this.horizontal = horizontal; + this.vertical = vertical; + //Normalize(); + } + + public readonly static Direction forward = new Direction(0, 0); + public readonly static Direction backward = new Direction(-180, 0); + public readonly static Direction up = new Direction(0, 90); + public readonly static Direction down = new Direction(0, -90); + 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) + { + this.horizontal += 180; + this.vertical = 180 - this.vertical; + } + } + + public Vector3Float ToVector3() + { + float verticalRad = (Angle.pi / 2) - this.vertical * Angle.Deg2Rad; + float horizontalRad = this.horizontal * Angle.Deg2Rad; + float cosVertical = (float)Math.Cos(verticalRad); + float sinVertical = (float)Math.Sin(verticalRad); + float cosHorizontal = (float)Math.Cos(horizontalRad); + float sinHorizontal = (float)Math.Sin(horizontalRad); + + float x = sinVertical * sinHorizontal; + float y = cosVertical; + float z = sinVertical * cosHorizontal; + + Vector3Float v = new(x, y, z); + return v; + } + } + +} \ No newline at end of file diff --git a/src/Float.cs b/src/Float.cs new file mode 100644 index 0000000..70873f0 --- /dev/null +++ b/src/Float.cs @@ -0,0 +1,45 @@ +namespace LinearAlgebra +{ + + /// + /// Float number utilities + /// + public class Float + { + /// + /// The precision of float numbers + /// + public const float epsilon = 1E-05f; + /// + /// The square of the float number precision + /// + public const float sqrEpsilon = 1e-10f; + + /// + /// Clamp the value between the given minimum and maximum values + /// + /// The value to clamp + /// The minimum value + /// The maximum value + /// The clamped value + public static float Clamp(float f, float min, float max) + { + if (f < min) + return min; + if (f > max) + return max; + return f; + } + + /// + /// Clamp the value between to the interval [0..1] + /// + /// The value to clamp + /// The clamped value + public static float Clamp01(float f) + { + return Clamp(f, 0, 1); + } + } + +} \ No newline at end of file diff --git a/src/LinearAlgebra.csproj b/src/LinearAlgebra.csproj new file mode 100644 index 0000000..14d3947 --- /dev/null +++ b/src/LinearAlgebra.csproj @@ -0,0 +1,14 @@ + + + + false + false + net5.0 + + + + + + + + diff --git a/src/Matrix.cs b/src/Matrix.cs new file mode 100644 index 0000000..1f0542e --- /dev/null +++ b/src/Matrix.cs @@ -0,0 +1,645 @@ +using System; +#if UNITY_5_3_OR_NEWER +using Vector3Float = UnityEngine.Vector3; +using Vector2Float = UnityEngine.Vector2; +using Quaternion = UnityEngine.Quaternion; +#endif + +namespace LinearAlgebra +{ + + public readonly struct Slice + { + public uint start { get; } + public uint stop { get; } + public Slice(uint start, uint stop) + { + this.start = start; + this.stop = stop; + } + } + + 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]; + } + return new Matrix2(data); + } + + 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); + } + +#if UNITY_5_3_OR_NEWER + public Vector3Float GetRow3(int rowIx) { + uint cols = this.nCols; + Vector3Float row = new() { + x = this.data[rowIx, 0], + y = this.data[rowIx, 1], + z = this.data[rowIx, 2] + }; + 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]; + } + 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]; + } + } + 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 + } + } + + // 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]; + } + } + + // 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]; + } + } + + // 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); + } + + 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]; + } + + 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); + } + +#if UNITY_5_3_OR_NEWER + 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); + } +#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 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 { + get { + 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]); + } + } +#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 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]; + } + } + +} \ No newline at end of file diff --git a/src/Quat32.cs b/src/Quat32.cs new file mode 100644 index 0000000..3026f8e --- /dev/null +++ b/src/Quat32.cs @@ -0,0 +1,98 @@ +using System; + +namespace LinearAlgebra +{ + public class Quat32 + { + public float x; + public float y; + public float z; + public float w; + + public Quat32() + { + this.x = 0; + this.y = 0; + this.z = 0; + this.w = 1; + } + + public Quat32(float x, float y, float z, float w) + { + this.x = x; + this.y = y; + this.z = z; + this.w = w; + } + + public static Quat32 FromSwingTwist(SwingTwist s) + { + Quat32 q32 = Quat32.Euler(-s.swing.vertical, s.swing.horizontal, s.twist); + return q32; + } + + public static Quat32 Euler(float yaw, float pitch, float roll) + { + float rollOver2 = roll * Angle.Deg2Rad * 0.5f; + float sinRollOver2 = (float)Math.Sin((float)rollOver2); + float cosRollOver2 = (float)Math.Cos((float)rollOver2); + float pitchOver2 = pitch * 0.5f; + float sinPitchOver2 = (float)Math.Sin((float)pitchOver2); + float cosPitchOver2 = (float)Math.Cos((float)pitchOver2); + float yawOver2 = yaw * 0.5f; + float sinYawOver2 = (float)Math.Sin((float)yawOver2); + float cosYawOver2 = (float)Math.Cos((float)yawOver2); + Quat32 result = new Quat32() + { + w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + + sinYawOver2 * sinPitchOver2 * sinRollOver2, + x = sinYawOver2 * cosPitchOver2 * cosRollOver2 + + cosYawOver2 * sinPitchOver2 * sinRollOver2, + y = cosYawOver2 * sinPitchOver2 * cosRollOver2 - + sinYawOver2 * cosPitchOver2 * sinRollOver2, + z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - + sinYawOver2 * sinPitchOver2 * cosRollOver2 + }; + return result; + } + + public void ToAngles(out float right, out float up, out float forward) + { + float test = this.x * this.y + this.z * this.w; + if (test > 0.499f) + { // singularity at north pole + right = 0; + up = 2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg; + forward = 90; + return; + //return Vector3(0, 2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, 90); + } + else if (test < -0.499f) + { // singularity at south pole + right = 0; + up = -2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg; + forward = -90; + return; + //return Vector3(0, -2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, -90); + } + else + { + float sqx = this.x * this.x; + float sqy = this.y * this.y; + float sqz = this.z * this.z; + + right = (float)Math.Atan2(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * Angle.Rad2Deg; + up = (float)Math.Atan2(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * Angle.Rad2Deg; + forward = (float)Math.Asin(2 * test) * Angle.Rad2Deg; + return; + // return Vector3( + // atan2f(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * + // Rad2Deg, + // atan2f(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * + // Rad2Deg, + // asinf(2 * test) * Angle.Rad2Deg); + } + } + + } +} \ No newline at end of file diff --git a/src/Quaternion.cs b/src/Quaternion.cs new file mode 100644 index 0000000..f481ef4 --- /dev/null +++ b/src/Quaternion.cs @@ -0,0 +1,80 @@ +using System; +#if UNITY_5_3_OR_NEWER +using Quaternion = UnityEngine.Quaternion; +#endif + +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) + { + this.x = x; + this.y = y; + this.z = z; + this.w = w; + } + +#if UNITY_5_3_OR_NEWER + public static Matrix2 ToRotationMatrix(Quaternion q) { + float w = q.x, x = q.y, y = q.z, z = q.w; + + 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); + } + + 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); + } +#endif + } + + // 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 diff --git a/src/Spherical.cs b/src/Spherical.cs new file mode 100644 index 0000000..497a508 --- /dev/null +++ b/src/Spherical.cs @@ -0,0 +1,57 @@ +using System; +#if UNITY_5_3_OR_NEWER +using Vector3Float = UnityEngine.Vector3; +#endif + +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) + { + this.distance = distance; + this.direction = new Direction(horizontal, vertical); + } + public Spherical(float distance, Direction direction) + { + this.distance = distance; + this.direction = direction; + } + + public static Spherical FromVector3(Vector3Float v) + { + float distance = v.magnitude; + if (distance == 0.0f) + return new Spherical(distance, 0, 0); + 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; + return new Spherical(distance, horizontalAngle, verticalAngle); + } + } + + 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); + float sinVertical = (float)Math.Sin(verticalRad); + float cosHorizontal = (float)Math.Cos(horizontalRad); + float sinHorizontal = (float)Math.Sin(horizontalRad); + + float x = this.distance * sinVertical * sinHorizontal; + float y = this.distance * cosVertical; + float z = this.distance * sinVertical * cosHorizontal; + + Vector3Float v = new Vector3Float(x, y, z); + return v; + } + } +} \ No newline at end of file diff --git a/src/SwingTwist.cs b/src/SwingTwist.cs new file mode 100644 index 0000000..acf8978 --- /dev/null +++ b/src/SwingTwist.cs @@ -0,0 +1,46 @@ +using System.Numerics; +#if UNITY_5_3_OR_NEWER +using Quaternion = UnityEngine.Quaternion; +#endif + +namespace LinearAlgebra +{ + + public class SwingTwist + { + public Direction swing; + public float twist; + + public static readonly SwingTwist zero = new SwingTwist(0, 0, 0); + + public SwingTwist(Direction swing, float twist) + { + this.swing = swing; + this.twist = twist; + } + public SwingTwist(float horizontalSwing, float verticalSwing, float twist) + { + this.swing = new Direction(horizontalSwing, verticalSwing); + this.swing.Normalize(); + this.twist = twist; + } + public static SwingTwist FromQuat32(Quat32 q32) + { + // UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w); + // SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z); + q32.ToAngles(out float right, out float up, out float forward); + SwingTwist r = new SwingTwist(up, right, forward); + return r; + } + +#if UNITY_5_3_OR_NEWER + public Quaternion ToQuaternion() { + Quaternion q = Quaternion.Euler(-this.swing.vertical, + this.swing.horizontal, + this.twist); + return q; + } +#endif + } + +} \ No newline at end of file diff --git a/src/Vector2.cs b/src/Vector2.cs new file mode 100644 index 0000000..603b0de --- /dev/null +++ b/src/Vector2.cs @@ -0,0 +1,401 @@ +using System; +using System.Numerics; + +namespace LinearAlgebra +{ + + public class Vector2Of where T : IComparable + { + public T x; + public T y; + + public Vector2Of(T x, T y) + { + this.x = x; + this.y = y; + } + } + + public class Vector2Int : Vector2Of + { + public Vector2Int(int x, int y) : base(x, y) { } + + public static Vector2Int operator -(Vector2Int v1, Vector2Int v2) + { + return new Vector2Int(v1.x - v2.x, v1.y - v2.y); + } + + public float magnitude + { + get + { + return (float)Math.Sqrt(this.x * this.x + this.y * this.y); + } + } + + public static float Distance(Vector2Int v1, Vector2Int v2) + { + return (v1 - v2).magnitude; + } + } + + public class Vector2Float : Vector2Of + { + public Vector2Float(float x, float y) : base(x, y) { } + + public static Vector2Float operator -(Vector2Float v1, Vector2Float v2) + { + return new Vector2Float(v1.x - v2.x, v1.y - v2.y); + } + + public float magnitude + { + get + { + return (float)Math.Sqrt(this.x * this.x + this.y * this.y); + } + } + + public static float Distance(Vector2Float v1, Vector2Float v2) + { + return (v1 - v2).magnitude; + } + } + + /// + /// 2-dimensional vectors + /// + public struct Vector2 : IEquatable + { + + /// + /// The right axis of the vector + /// + public float x; // left/right + /// + /// The upward/forward axis of the vector + /// + public float y; // forward/backward + // directions are to be inline with Vector3 as much as possible... + + /// + /// Create a new 2-dimensional vector + /// + /// x axis value + /// y axis value + public Vector2(float x, float y) + { + this.x = x; + this.y = y; + } + + /// + /// A vector with zero for all axis + /// + public static readonly Vector2 zero = new Vector2(0, 0); + /// + /// A vector with values (1, 1) + /// + public static readonly Vector2 one = new Vector2(1, 1); + /// + /// A vector with values (0, 1) + /// + public static readonly Vector2 up = new Vector2(0, 1); + /// + /// A vector with values (0, -1) + /// + public static readonly Vector2 down = new Vector2(0, -1); + /// + /// A vector with values (0, 1) + /// + public static readonly Vector2 forward = new Vector2(0, 1); + /// + /// A vector with values (0, -1) + /// + public static readonly Vector2 back = new Vector2(0, -1); + /// + /// A vector3 with values (-1, 0) + /// + public static readonly Vector2 left = new Vector2(-1, 0); + /// + /// A vector with values (1, 0) + /// + public static readonly Vector2 right = new Vector2(1, 0); + + /// + /// The squared length of this vector + /// + /// The squared length + /// 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 + { + float d = x * x + y * y; + return d; + } + } + + /// + /// The length of this vector + /// + /// The length of this vector + public float magnitude + { + get + { + float d = (float)Math.Sqrt(x * x + y * y); + return d; + } + } + + /// + /// Convert the vector to a length of a 1 + /// + /// The vector with length 1 + public Vector2 normalized + { + get + { + float l = magnitude; + Vector2 v = zero; + if (l > Float.epsilon) + v = this / l; + return v; + } + } + + /// + /// Add two vectors + /// + /// The first vector + /// The second vector + /// The result of adding the two vectors + public static Vector2 operator +(Vector2 v1, Vector2 v2) + { + Vector2 v = new Vector2(v1.x + v2.x, v1.y + v2.y); + return v; + } + + /// + /// Subtract two vectors + /// + /// The first vector + /// The second vector + /// The result of adding the two vectors + public static Vector2 operator -(Vector2 v1, Vector2 v2) + { + Vector2 v = new Vector2(v1.x - v2.x, v1.y - v2.y); + return v; + } + + /// + /// Negate the vector + /// + /// The vector to negate + /// The negated vector + /// This will result in a vector pointing in the opposite direction + public static Vector2 operator -(Vector2 v1) + { + Vector2 v = new Vector2(-v1.x, -v1.y); + return v; + } + + /// + /// Scale a vector uniformly up + /// + /// The vector to scale + /// 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) + { + Vector2 v = new Vector2(v1.x * f, v1.y * f); + return v; + } + + /// + /// Scale a vector uniformly up + /// + /// The scaling factor + /// 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) + { + Vector2 v = new Vector2(f * v1.x, f * v1.y); + return v; + } + + /// + /// Scale a vector uniformly down + /// + /// The vector to scale + /// 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) + { + Vector2 v = new Vector2(v1.x / f, v1.y / f); + return v; + } + + /// + /// Tests if the vector has equal values as the given vector + /// + /// The vector to compare to + /// true if the vector values are equal + public bool Equals(Vector2 v1) => x == v1.x && y == v1.y; + + /// + /// Tests if the vector is equal to the given object + /// + /// 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) + { + if (!(obj is Vector2 v)) + return false; + + return (x == v.x && y == v.y); + } + + /// + /// Tests if the two vectors have equal values + /// + /// The first vector + /// The second vector + /// truewhen the vectors have equal values + /// 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) + { + return (v1.x == v2.x && v1.y == v2.y); + } + + /// + /// Tests if two vectors have different values + /// + /// The first vector + /// The second vector + /// truewhen the vectors have different values + /// 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) + { + return (v1.x != v2.x || v1.y != v2.y); + } + + /// + /// Get an hash code for the vector + /// + /// The hash code + public override int GetHashCode() + { + return (x, y).GetHashCode(); + } + + /// + /// Get the distance between two vectors + /// + /// The first vector + /// The second vector + /// The distance between the two vectors + 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); + return d; + } + + /// + /// The dot product of two vectors + /// + /// The first vector + /// The second vector + /// The dot product of the two vectors + public static float Dot(Vector2 v1, Vector2 v2) + { + return v1.x * v2.x + v1.y * v2.y; + } + + /// + /// Lerp between two vectors + /// + /// The from vector + /// The to vector + /// The interpolation distance [0..1] + /// The lerped vector + /// 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) + { + Vector2 v = v1 + (v2 - v1) * f; + return v; + } + + /// + /// Calculate the signed angle between two vectors. + /// + /// The starting vector + /// The ending vector + /// The axis to rotate around + /// The signed angle in degrees + 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; + + float sqrMagFrom = from.sqrMagnitude; + float sqrMagTo = to.sqrMagnitude; + + if (sqrMagFrom == 0 || sqrMagTo == 0) + return 0; + //if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo)) + // return nanf(""); + + float angleFrom = (float)Math.Atan2(from.y, from.x); + float angleTo = (float)Math.Atan2(to.y, to.x); + return (angleTo - angleFrom) * Angle.Rad2Deg; + } + + /// + /// Rotates the vector with the given angle + /// + /// The vector to rotate + /// The angle in degrees + /// + 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() + { + x = (cos * tx) - (sin * ty), + y = (sin * tx) + (cos * ty) + }; + return v; + } + + /// + /// Map interval of angles between vectors [0..Pi] to interval [0..1] + /// + /// The first vector + /// 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) + { + return (1 - Vector2.Dot(v1, v2)) / 2; + } + } +} \ No newline at end of file diff --git a/src/Vector3.cs b/src/Vector3.cs new file mode 100644 index 0000000..616b82e --- /dev/null +++ b/src/Vector3.cs @@ -0,0 +1,204 @@ +#if !UNITY_5_3_OR_NEWER +using System; + +namespace LinearAlgebra +{ + public class Vector3Of + { + public T x; + public T y; + public T z; + + public Vector3Of(T x, T y, T z) + { + this.x = x; + this.y = y; + this.z = z; + } + + // public uint magnitude { + // get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); + // } + } + + public class Vector3Int : Vector3Of + { + public Vector3Int(int x, int y, int z) : base(x, y, z) { } + } + public class Vector3Float : Vector3Of + { + public Vector3Float(float x, float y, float z) : base(x, y, z) { } + + public float magnitude + { + get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); + } + } + + /// + /// 3-dimensional vectors + /// + /// This uses the right-handed coordinate system. + public struct Vector3 : IEquatable + { + + /// + /// The right axis of the vector + /// + public float x; //> left/right + /// + /// The upward axis of the vector + /// + public float y; //> up/down + /// + /// The forward axis of the vector + /// + public float z; //> forward/backward + + /// + /// Create a new 3-dimensional vector + /// + /// x axis value + /// y axis value + /// z axis value + public Vector3(float x, float y, float z) + { + this.x = x; + this.y = y; + this.z = z; + } + + /// + /// A vector with zero for all axis + /// + public static readonly Vector3 zero = new Vector3(0, 0, 0); + /// + /// A vector with one for all axis + /// + public static readonly Vector3 one = new Vector3(1, 1, 1); + /// + /// A vector3 with values (-1, 0, 0) + /// + public static readonly Vector3 left = new Vector3(-1, 0, 0); + /// + /// A vector with values (1, 0, 0) + /// + public static readonly Vector3 right = new Vector3(1, 0, 0); + /// + /// A vector with values (0, -1, 0) + /// + public static readonly Vector3 down = new Vector3(0, -1, 0); + /// + /// A vector with values (0, 1, 0) + /// + public static readonly Vector3 up = new Vector3(0, 1, 0); + /// + /// A vector with values (0, 0, -1) + /// + public static readonly Vector3 back = new Vector3(0, -1, 0); + /// + /// A vector with values (0, 0, 1) + /// + public static readonly Vector3 forward = new Vector3(0, 1, 0); + + public float magnitude + { + get + { + float d = (float)Math.Sqrt(x * x + y * y); + return d; + } + } + + public Vector3 normalized + { + get + { + float l = magnitude; + Vector3 v = zero; + if (l > Float.epsilon) + v = this / l; + return v; + } + } + + 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) + { + 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 v = new Vector3(-v1.x, -v1.y, -v1.z); + return v; + } + + 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) + { + Vector3 v = new Vector3(d * v1.x, d * v1.y, d * v1.z); + return v; + } + + 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) + { + 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) + { + return (v1.x == v2.x && v1.y == v2.y && v1.z == v2.z); + } + + 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() + { + return (x, y, z).GetHashCode(); + } + + public static float Distance(Vector3 v1, Vector3 v2) + { + return (v2 - v1).magnitude; + } + + 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) + { + Vector3 v = v1 + (v2 - v1) * f; + return v; + } + + } +} +#endif \ No newline at end of file diff --git a/src/float16.cs b/src/float16.cs new file mode 100644 index 0000000..5d94014 --- /dev/null +++ b/src/float16.cs @@ -0,0 +1,344 @@ +using System; + +namespace LinearAlgebra +{ + + public class float16 + { + // + // FILE: float16.cpp + // AUTHOR: Rob Tillaart + // VERSION: 0.1.8 + // PURPOSE: library for Float16s for Arduino + // URL: http://en.wikipedia.org/wiki/Half-precision_floating-point_format + + ushort _value; + + public float16() { _value = 0; } + + public float16(float f) + { + //_value = f32tof16(f); + _value = F32ToF16__(f); + } + + public float toFloat() + { + return f16tof32(_value); + } + + public ushort GetBinary() { return _value; } + public void SetBinary(ushort value) { _value = value; } + + ////////////////////////////////////////////////////////// + // + // EQUALITIES + // + /* + bool float16::operator ==(const float16 &f) { return (_value == f._value); } + + bool float16::operator !=(const float16 &f) { return (_value != f._value); } + + bool float16::operator >(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value < f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value > f._value; + } + + bool float16::operator >=(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value <= f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value >= f._value; + } + + bool float16::operator <(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value > f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value < f._value; + } + + bool float16::operator <=(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value >= f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value <= f._value; + } + + ////////////////////////////////////////////////////////// + // + // NEGATION + // + float16 float16::operator -() { + float16 f16; + f16.setBinary(_value ^ 0x8000); + return f16; + } + + ////////////////////////////////////////////////////////// + // + // MATH + // + float16 float16::operator +(const float16 &f) { + return float16(this->toDouble() + f.toDouble()); + } + + float16 float16::operator -(const float16 &f) { + return float16(this->toDouble() - f.toDouble()); + } + + float16 float16::operator *(const float16 &f) { + return float16(this->toDouble() * f.toDouble()); + } + + float16 float16::operator /(const float16 &f) { + return float16(this->toDouble() / f.toDouble()); + } + + float16 & float16::operator+=(const float16 &f) { + *this = this->toDouble() + f.toDouble(); + return *this; + } + + float16 & float16::operator-=(const float16 &f) { + *this = this->toDouble() - f.toDouble(); + return *this; + } + + float16 & float16::operator*=(const float16 &f) { + *this = this->toDouble() * f.toDouble(); + return *this; + } + + float16 & float16::operator/=(const float16 &f) { + *this = this->toDouble() / f.toDouble(); + return *this; + } + + ////////////////////////////////////////////////////////// + // + // MATH HELPER FUNCTIONS + // + int float16::sign() { + if (_value & 0x8000) + return -1; + if (_value & 0xFFFF) + return 1; + return 0; + } + + bool float16::isZero() { return ((_value & 0x7FFF) == 0x0000); } + + bool float16::isNaN() { + if ((_value & 0x7C00) != 0x7C00) + return false; + if ((_value & 0x03FF) == 0x0000) + return false; + return true; + } + + bool float16::isInf() { return ((_value == 0x7C00) || (_value == 0xFC00)); } + */ + ////////////////////////////////////////////////////////// + // + // CORE CONVERSION + // + float f16tof32(ushort _value) + { + //ushort sgn; + ushort man; + int exp; + float f; + + //Debug.Log($"{_value}"); + + bool sgn = (_value & 0x8000) > 0; + exp = (_value & 0x7C00) >> 10; + man = (ushort)(_value & 0x03FF); + + //Debug.Log($"{sgn} {exp} {man}"); + + // ZERO + if ((_value & 0x7FFF) == 0) + { + return sgn ? -0 : 0; + } + // NAN & INF + if (exp == 0x001F) + { + if (man == 0) + return sgn ? float.NegativeInfinity : float.PositiveInfinity; //-INFINITY : INFINITY; + else + return float.NaN; // NAN; + } + + // SUBNORMAL/NORMAL + if (exp == 0) + f = 0; + else + f = 1; + + // PROCESS MANTISSE + 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) + { + f = f * (float)Math.Pow(2.0f, -13); // 5.96046447754e-8; + } + //Debug.Log($"{f}"); + return sgn ? -f : f; + } + + 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) + { + 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) + { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // denormalized float32 does not fit in float16 + if (exp == 0x00) + { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // handle infinity & NAN + if (exp == 0x00FF) + { + if (man != 0) + return 0xFE00; // NAN + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + + // normal numbers + exp = exp - 127 + 15; + // overflow does not fit => INF + if (exp > 30) + { + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + // subnormal numbers + if (exp < -38) + { + return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ? + } + if (exp <= 0) // subnormal + { + man >>= (exp + 14); + // rounding + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | man); + return man; + } + + // normal + // TODO rounding + exp <<= 10; + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | exp | man); + return (ushort)(exp | man); + } + + //This function is faulty!!!! + ushort f32tof16(float f) + { + //uint t = *(uint*)&f; + //uint t = (uint)BitConverter.SingleToInt32Bits(f); + uint t = SingleToInt32Bits(f); + // man bits = 10; but we keep 11 for rounding + ushort man = (ushort)((t & 0x007FFFFF) >> 12); + short exp = (short)((t & 0x7F800000) >> 23); + bool sgn = (t & 0x80000000) != 0; + + // handle 0 + if ((t & 0x7FFFFFFF) == 0) + { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // denormalized float32 does not fit in float16 + if (exp == 0x00) + { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // handle infinity & NAN + if (exp == 0x00FF) + { + if (man != 0) + return 0xFE00; // NAN + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + + // normal numbers + exp = (short)(exp - 127 + 15); + // overflow does not fit => INF + if (exp > 30) + { + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + // subnormal numbers + if (exp < -38) + { + return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ? + } + if (exp <= 0) // subnormal + { + man >>= (exp + 14); + // rounding + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | man); + return man; + } + + // normal + // TODO rounding + exp <<= 10; + man++; + man >>= 1; + ushort uexp = (ushort)exp; + if (sgn) + return (ushort)(0x8000 | uexp | man); + return (ushort)(uexp | man); + } + + // -- END OF FILE -- + } + +} \ No newline at end of file diff --git a/test/AngleTest.cs b/test/AngleTest.cs new file mode 100644 index 0000000..34b107a --- /dev/null +++ b/test/AngleTest.cs @@ -0,0 +1,171 @@ +#if !UNITY_5_6_OR_NEWER +using NUnit.Framework; + +namespace LinearAlgebra.Test +{ + public class Tests + { + [SetUp] + public void Setup() + { + } + + [Test] + public void Test_Normalize() + { + float r = 0; + + r = Angle.Normalize(90); + Assert.AreEqual(r, 90, "Normalize 90"); + + r = Angle.Normalize(-90); + Assert.AreEqual(r, -90, "Normalize -90"); + + r = Angle.Normalize(270); + Assert.AreEqual(r, -90, "Normalize 270"); + + r = Angle.Normalize(270 + 360); + Assert.AreEqual(r, -90, "Normalize 270+360"); + + r = Angle.Normalize(-270); + Assert.AreEqual(r, 90, "Normalize -270"); + + r = Angle.Normalize(-270 - 360); + Assert.AreEqual(r, 90, "Normalize -270-360"); + + r = Angle.Normalize(0); + Assert.AreEqual(r, 0, "Normalize 0"); + + r = Angle.Normalize(float.PositiveInfinity); + Assert.AreEqual(r, float.PositiveInfinity, "Normalize INFINITY"); + + r = Angle.Normalize(float.NegativeInfinity); + Assert.AreEqual(r, float.NegativeInfinity, "Normalize INFINITY"); + } + + [Test] + public void Clamp() + { + float r = 0; + + r = Angle.Clamp(1, 0, 2); + Assert.AreEqual(r, 1, "Clamp 1 0 2"); + + r = Angle.Clamp(-1, 0, 2); + Assert.AreEqual(r, 0, "Clamp -1 0 2"); + + r = Angle.Clamp(3, 0, 2); + Assert.AreEqual(r, 2, "Clamp 3 0 2"); + + r = Angle.Clamp(1, 0, 0); + Assert.AreEqual(r, 0, "Clamp 1 0 0"); + + r = Angle.Clamp(0, 0, 0); + Assert.AreEqual(r, 0, "Clamp 0 0 0"); + + r = Angle.Clamp(0, 1, -1); + Assert.AreEqual(r, 1, "Clamp 0 1 -1"); + + r = Angle.Clamp(1, 0, float.PositiveInfinity); + Assert.AreEqual(r, 1, "Clamp 1 0 INFINITY"); + + r = Angle.Clamp(1, float.NegativeInfinity, 1); + Assert.AreEqual(r, 1, "Clamp 1 -INFINITY 1"); + } + + [Test] + public void Difference() + { + float r = 0; + + r = Angle.Difference(0, 90); + Assert.AreEqual(r, 90, "Difference 0 90"); + + r = Angle.Difference(0, -90); + Assert.AreEqual(r, -90, "Difference 0 -90"); + + r = Angle.Difference(0, 270); + Assert.AreEqual(r, -90, "Difference 0 270"); + + r = Angle.Difference(0, -270); + Assert.AreEqual(r, 90, "Difference 0 -270"); + + r = Angle.Difference(90, 0); + Assert.AreEqual(r, -90, "Difference 90 0"); + + r = Angle.Difference(-90, 0); + Assert.AreEqual(r, 90, "Difference -90 0"); + + r = Angle.Difference(0, 0); + Assert.AreEqual(r, 0, "Difference 0 0"); + + r = Angle.Difference(90, 90); + Assert.AreEqual(r, 0, "Difference 90 90"); + + r = Angle.Difference(0, float.PositiveInfinity); + Assert.AreEqual(r, float.PositiveInfinity, "Difference 0 INFINITY"); + + r = Angle.Difference(0, float.NegativeInfinity); + Assert.AreEqual(r, float.NegativeInfinity, "Difference 0 -INFINITY"); + + r = Angle.Difference(float.NegativeInfinity, float.PositiveInfinity); + Assert.AreEqual(r, float.PositiveInfinity, "Difference -INFINITY INFINITY"); + } + + [Test] + public void MoveTowards() + { + float r = 0; + + r = Angle.MoveTowards(0, 90, 30); + Assert.AreEqual(r, 30, "MoveTowards 0 90 30"); + + r = Angle.MoveTowards(0, 90, 90); + Assert.AreEqual(r, 90, "MoveTowards 0 90 90"); + + r = Angle.MoveTowards(0, 90, 180); + Assert.AreEqual(r, 90, "MoveTowards 0 90 180"); + + r = Angle.MoveTowards(0, 90, 270); + Assert.AreEqual(r, 90, "MoveTowrads 0 90 270"); + + r = Angle.MoveTowards(0, 90, -30); + Assert.AreEqual(r, -30, "MoveTowards 0 90 -30"); + + r = Angle.MoveTowards(0, -90, -30); + Assert.AreEqual(r, 30, "MoveTowards 0 -90 -30"); + + r = Angle.MoveTowards(0, -90, -90); + Assert.AreEqual(r, 90, "MoveTowards 0 -90 -90"); + + r = Angle.MoveTowards(0, -90, -180); + Assert.AreEqual(r, 180, "MoveTowards 0 -90 -180"); + + r = Angle.MoveTowards(0, -90, -270); + Assert.AreEqual(r, 270, "MoveTowrads 0 -90 -270"); + + r = Angle.MoveTowards(0, 90, 0); + Assert.AreEqual(r, 0, "MoveTowards 0 90 0"); + + r = Angle.MoveTowards(0, 0, 0); + Assert.AreEqual(r, 0, "MoveTowards 0 0 0"); + + r = Angle.MoveTowards(0, 0, 30); + Assert.AreEqual(r, 0, "MoveTowrads 0 0 30"); + + r = Angle.MoveTowards(0, 90, float.PositiveInfinity); + Assert.AreEqual(r, 90, "MoveTowards 0 90 INFINITY"); + + r = Angle.MoveTowards(0, float.PositiveInfinity, 30); + Assert.AreEqual(r, 30, "MoveTowrads 0 INFINITY 30"); + + r = Angle.MoveTowards(0, -90, float.NegativeInfinity); + Assert.AreEqual(r, float.PositiveInfinity, "MoveTowards 0 -90 -INFINITY"); + + r = Angle.MoveTowards(0, float.NegativeInfinity, -30); + Assert.AreEqual(r, 30, "MoveTowrads 0 -INFINITY -30"); + + } + } +} +#endif \ No newline at end of file diff --git a/test/LinearAlgebra_Test.csproj b/test/LinearAlgebra_Test.csproj new file mode 100644 index 0000000..3ee2230 --- /dev/null +++ b/test/LinearAlgebra_Test.csproj @@ -0,0 +1,19 @@ + + + + net5.0 + false + true + + + + + + + + + + + + + From 874f747c7798c742bc7a198f5671db085e6ffa82 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 7 Apr 2025 09:36:22 +0200 Subject: [PATCH 02/20] Migrating from ControlCore to RoboidControl --- src/Matrix.cs | 185 ++++++++++++++++++++++++++++++++++++---------- src/Quaternion.cs | 14 ++-- 2 files changed, 154 insertions(+), 45 deletions(-) diff --git a/src/Matrix.cs b/src/Matrix.cs index 1f0542e..8ee1d0d 100644 --- a/src/Matrix.cs +++ b/src/Matrix.cs @@ -10,9 +10,9 @@ namespace LinearAlgebra public readonly struct Slice { - public uint start { get; } - public uint stop { get; } - public Slice(uint start, uint stop) + public int start { get; } + public int stop { get; } + public Slice(int start, int stop) { this.start = start; this.stop = stop; @@ -23,10 +23,10 @@ namespace LinearAlgebra { public float[,] data { get; } - public uint nRows => (uint)data.GetLength(0); - public uint nCols => (uint)data.GetLength(1); + public int nRows => data.GetLength(0); + public int nCols => data.GetLength(1); - public Matrix2(uint nRows, uint nCols) + public Matrix2(int nRows, int nCols) { this.data = new float[nRows, nCols]; } @@ -46,7 +46,7 @@ namespace LinearAlgebra return new Matrix2(data); } - public static Matrix2 Zero(uint nRows, uint nCols) + public static Matrix2 Zero(int nRows, int nCols) { return new Matrix2(nRows, nCols); } @@ -60,11 +60,11 @@ namespace LinearAlgebra return new Matrix2(result); } - public static Matrix2 Identity(uint size) + public static Matrix2 Identity(int size) { return Diagonal(1, size); } - public static Matrix2 Identity(uint nRows, uint nCols) + public static Matrix2 Identity(int nRows, int nCols) { Matrix2 m = Zero(nRows, nCols); m.FillDiagonal(1); @@ -78,7 +78,7 @@ namespace LinearAlgebra resultData[ix, ix] = v.data[ix]; return new Matrix2(resultData); } - public static Matrix2 Diagonal(float f, uint size) + public static Matrix2 Diagonal(float f, int size) { float[,] resultData = new float[size, size]; for (int ix = 0; ix < size; ix++) @@ -87,13 +87,13 @@ namespace LinearAlgebra } public void FillDiagonal(Matrix1 v) { - uint n = Math.Min(Math.Min(this.nRows, this.nCols), v.size); + int n = (int)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); + int n = Math.Min(this.nRows, this.nCols); for (int ix = 0; ix < n; ix++) this.data[ix, ix] = f; } @@ -108,9 +108,17 @@ namespace LinearAlgebra return new Matrix2(result); } + public Matrix1 GetRow(int rowIx) { + float[] row = new float[this.nCols]; + for (int colIx = 0; colIx < this.nCols; colIx++) { + row[colIx] = this.data[rowIx, colIx]; + } + return new Matrix1(row); + } + #if UNITY_5_3_OR_NEWER public Vector3Float GetRow3(int rowIx) { - uint cols = this.nCols; + int cols = this.nCols; Vector3Float row = new() { x = this.data[rowIx, 0], y = this.data[rowIx, 1], @@ -131,6 +139,14 @@ namespace LinearAlgebra this.data[rowIx, 2] = v.z; } + public void SwapRows(int row1, int row2) { + for (uint ix = 0; ix < this.nCols; ix++) { + float temp = this.data[row1, ix]; + this.data[row1, ix] = this.data[row2, ix]; + this.data[row2, ix] = temp; + } + } + public Matrix1 GetColumn(int colIx) { float[] column = new float[this.nRows]; @@ -140,6 +156,15 @@ namespace LinearAlgebra } return new Matrix1(column); } + public void SetColumn(int colIx, Matrix1 v) { + for (uint ix = 0; ix < v.size; ix++) + this.data[ix, colIx] = v.data[ix]; + } + public void SetColumn(int colIx, Vector3Float v) { + this.data[0, colIx] = v.x; + this.data[1, colIx] = v.y; + this.data[2, colIx] = v.z; + } public static bool AllClose(Matrix2 A, Matrix2 B, float atol = 1e-08f) { @@ -302,18 +327,37 @@ namespace LinearAlgebra return new Matrix2(result); } + public Matrix2 GetRows(Slice slice) { + return GetRows(slice.start, slice.stop); + } + public Matrix2 GetRows(int from, int 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 (int colIx = 0; colIx < this.nCols; colIx++) + result[resultRowIx, colIx] = this.data[rowIx, colIx]; + + resultRowIx++; + } + + return new Matrix2(result); + } + public Matrix2 Slice(Slice slice) { return Slice(slice.start, slice.stop); } - public Matrix2 Slice(uint from, uint to) + public Matrix2 Slice(int from, int 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 rowIx = from; rowIx < to; rowIx++) { for (int colIx = 0; colIx < this.nCols; colIx++) { @@ -328,39 +372,42 @@ namespace LinearAlgebra { return Slice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop)); } - - public Matrix2 Slice((uint start, uint stop) rowRange, (uint start, uint stop) colRange) + public Matrix2 Slice((int start, int stop) rowRange, (int start, int 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++) + int resultRowIx = 0; + int resultColIx = 0; + for (int i = rowRange.start; i < rowRange.stop; i++) { - for (uint j = colRange.start; j < colRange.stop; j++) + for (int 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) { + UpdateSlice((slice.start, slice.stop), m); + } + public void UpdateSlice((int start, int stop) slice, Matrix2 m) { + // if (slice.start == slice.stop) + // Console.WriteLine("WARNING: no data is updates when start equals stop in a slice!"); int mRowIx = 0; - for (uint rowIx = slice.start; rowIx < slice.stop; rowIx++, mRowIx++) - { + for (int 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) + public void UpdateSlice((int start, int stop) rowRange, (int start, int stop) colRange, Matrix2 m) { - for (uint i = rowRange.start; i < rowRange.stop; i++) + for (int i = rowRange.start; i < rowRange.stop; i++) { - for (uint j = colRange.start; j < colRange.stop; j++) + for (int j = colRange.start; j < colRange.stop; j++) this.data[i, j] = m.data[i - rowRange.start, j - colRange.start]; } } @@ -369,7 +416,7 @@ namespace LinearAlgebra { Matrix2 A = this; // unchecked - uint n = A.nRows; + int n = A.nRows; // Create an identity matrix of the same size as the original matrix float[,] augmentedMatrix = new float[n, 2 * n]; @@ -404,10 +451,10 @@ namespace LinearAlgebra } // Back substitution - for (uint i = n - 1; i >= 0; i--) + for (int i = n - 1; i >= 0; i--) { // Eliminate the column above the pivot - for (uint j = i - 1; j >= 0; j--) + for (int j = i - 1; j >= 0; j--) { float factor = augmentedMatrix[j, i]; for (int k = 0; k < 2 * n; k++) @@ -428,7 +475,7 @@ namespace LinearAlgebra public float Determinant() { - uint n = this.nRows; + int n = this.nRows; if (n != this.nCols) throw new System.ArgumentException("Matrix must be square."); @@ -448,7 +495,7 @@ namespace LinearAlgebra // Helper function to compute the minor of a matrix private Matrix2 Minor(int rowToRemove, int colToRemove) { - uint n = this.nRows; + int n = this.nRows; float[,] minor = new float[n - 1, n - 1]; int r = 0, c = 0; @@ -469,15 +516,46 @@ namespace LinearAlgebra return new Matrix2(minor); } + + public static Matrix2 DeleteRows(Matrix2 A, Slice rowRange) { + float[,] result = new float[A.nRows - (rowRange.stop - rowRange.start), A.nCols]; + + int resultRowIx = 0; + for (int i = 0; i < A.nRows; i++) { + if (i >= rowRange.start && i < rowRange.stop) + continue; + + for (int j = 0; j < A.nCols; j++) + result[resultRowIx, j] = A.data[i, j]; + + resultRowIx++; + } + return new Matrix2(result); + } + + internal static Matrix2 DeleteColumns(Matrix2 A, Slice colRange) { + float[,] result = new float[A.nRows, A.nCols - (colRange.stop - colRange.start)]; + + for (int i = 0; i < A.nRows; i++) { + int resultColIx = 0; + for (int j = 0; j < A.nCols; j++) { + if (j >= colRange.start && j < colRange.stop) + continue; + + result[i, resultColIx++] = A.data[i, j]; + } + } + return new Matrix2(result); + } } public class Matrix1 { public float[] data { get; } - public uint size => (uint)data.GetLength(0); + public int size => data.GetLength(0); - public Matrix1(uint size) + public Matrix1(int size) { this.data = new float[size]; } @@ -487,7 +565,7 @@ namespace LinearAlgebra this.data = data; } - public static Matrix1 Zero(uint size) + public static Matrix1 Zero(int size) { return new Matrix1(size); } @@ -604,6 +682,18 @@ namespace LinearAlgebra return result; } + 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 static Matrix1 operator *(Matrix1 A, float f) { float[] result = new float[A.size]; @@ -618,18 +708,35 @@ namespace LinearAlgebra return A * f; } + 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) { + float[] result = new float[A.size]; + + for (int i = 0; i < A.size; i++) + result[i] = f / A.data[i]; + + return new Matrix1(result); + } + public Matrix1 Slice(Slice range) { return Slice(range.start, range.stop); } - public Matrix1 Slice(uint from, uint to) + public Matrix1 Slice(int from, int 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++) + for (int ix = from; ix < to; ix++) result[resultIx++] = this.data[ix]; return new Matrix1(result); @@ -637,7 +744,7 @@ namespace LinearAlgebra public void UpdateSlice(Slice slice, Matrix1 v) { int vIx = 0; - for (uint ix = slice.start; ix < slice.stop; ix++, vIx++) + for (int ix = slice.start; ix < slice.stop; ix++, vIx++) this.data[ix] = v.data[vIx]; } } diff --git a/src/Quaternion.cs b/src/Quaternion.cs index f481ef4..cd7e9f4 100644 --- a/src/Quaternion.cs +++ b/src/Quaternion.cs @@ -3,24 +3,26 @@ using System; using Quaternion = UnityEngine.Quaternion; #endif -namespace LinearAlgebra -{ +namespace LinearAlgebra { - public class QuaternionOf - { + 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; this.w = w; } + public static Quaternion Reflect(Quaternion q) { + return new(-q.x, -q.y, -q.z, q.w); + } + + #if UNITY_5_3_OR_NEWER public static Matrix2 ToRotationMatrix(Quaternion q) { float w = q.x, x = q.y, y = q.z, z = q.w; From 2427229186840112bf726cd6189df99ba561adb2 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 7 Apr 2025 09:45:34 +0200 Subject: [PATCH 03/20] Cleanup --- src/Decomposition.cs | 287 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 287 insertions(+) create mode 100644 src/Decomposition.cs diff --git a/src/Decomposition.cs b/src/Decomposition.cs new file mode 100644 index 0000000..ddaf434 --- /dev/null +++ b/src/Decomposition.cs @@ -0,0 +1,287 @@ +using System; +namespace LinearAlgebra { + class QR { + // QR Decomposition of a matrix A + public static (Matrix2 Q, Matrix2 R) Decomposition(Matrix2 A) { + int nRows = A.nRows; + int nCols = A.nCols; + + float[,] Q = new float[nRows, nCols]; + float[,] R = new float[nCols, nCols]; + + // Perform Gram-Schmidt orthogonalization + for (uint colIx = 0; colIx < nCols; colIx++) { + + // Step 1: v = column(ix) of A + float[] v = new float[nRows]; + for (int rowIx = 0; rowIx < nRows; rowIx++) + v[rowIx] = A.data[rowIx, colIx]; + + // Step 2: Subtract projections of v onto previous q's (orthogonalize) + for (uint colIx2 = 0; colIx2 < colIx; colIx2++) { + float dotProd = 0; + for (int i = 0; i < nRows; i++) + dotProd += Q[i, colIx2] * v[i]; + for (int i = 0; i < nRows; i++) + v[i] -= dotProd * Q[i, colIx2]; + } + + // Step 3: Normalize v to get column(ix) of Q + float norm = 0; + for (int rowIx = 0; rowIx < nRows; rowIx++) + norm += v[rowIx] * v[rowIx]; + norm = (float)Math.Sqrt(norm); + + for (int rowIx = 0; rowIx < nRows; rowIx++) + Q[rowIx, colIx] = v[rowIx] / norm; + + // Store the coefficients of R + for (int colIx2 = 0; colIx2 <= colIx; colIx2++) { + R[colIx2, colIx] = 0; + for (int k = 0; k < nRows; k++) + R[colIx2, colIx] += Q[k, colIx2] * A.data[k, colIx]; + } + } + return (new Matrix2(Q), new Matrix2(R)); + } + + // Reduced QR Decomposition of a matrix A + public static (Matrix2 Q, Matrix2 R) ReducedDecomposition(Matrix2 A) { + int nRows = A.nRows; + int nCols = A.nCols; + + float[,] Q = new float[nRows, nCols]; + float[,] R = new float[nCols, nCols]; + + // Perform Gram-Schmidt orthogonalization + for (int colIx = 0; colIx < nCols; colIx++) { + + // Step 1: v = column(colIx) of A + float[] columnIx = new float[nRows]; + bool isZeroColumn = true; + for (int rowIx = 0; rowIx < nRows; rowIx++) { + columnIx[rowIx] = A.data[rowIx, colIx]; + if (columnIx[rowIx] != 0) + isZeroColumn = false; + } + if (isZeroColumn) { + for (int rowIx = 0; rowIx < nRows; rowIx++) + Q[rowIx, colIx] = 0; + // Set corresponding R element to 0 + R[colIx, colIx] = 0; + + Console.WriteLine($"zero column {colIx}"); + + continue; + } + + // Step 2: Subtract projections of v onto previous q's (orthogonalize) + for (int colIx2 = 0; colIx2 < colIx; colIx2++) { + // Compute the dot product of v and column(colIx2) of Q + float dotProduct = 0; + for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++) + dotProduct += columnIx[rowIx2] * Q[rowIx2, colIx2]; + // Subtract the projection from v + for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++) + columnIx[rowIx2] -= dotProduct * Q[rowIx2, colIx2]; + } + + // Step 3: Normalize v to get column(colIx) of Q + float norm = 0; + for (int rowIx = 0; rowIx < nRows; rowIx++) + norm += columnIx[rowIx] * columnIx[rowIx]; + if (norm == 0) + throw new Exception("invalid value"); + + norm = (float)Math.Sqrt(norm); + + for (int rowIx = 0; rowIx < nRows; rowIx++) + Q[rowIx, colIx] = columnIx[rowIx] / norm; + + // Here is where it deviates from the Full QR Decomposition ! + + // Step 4: Compute the row(colIx) of R + for (int colIx2 = colIx; colIx2 < nCols; colIx2++) { + float dotProduct = 0; + for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++) + dotProduct += Q[rowIx2, colIx] * A.data[rowIx2, colIx2]; + R[colIx, colIx2] = dotProduct; + } + } + if (!float.IsFinite(R[0, 0])) + throw new Exception("invalid value"); + + return (new Matrix2(Q), new Matrix2(R)); + } + } + + class SVD { + // According to ChatGPT, Mathnet uses Golub-Reinsch SVD algorithm + // 1. Bidiagonalization: The input matrix AA is reduced to a bidiagonal form using Golub-Kahan bidiagonalization. + // This process involves applying a sequence of Householder reflections to AA to create a bidiagonal matrix. + // This step reduces the complexity by making the matrix simpler while retaining the essential structure needed for SVD. + // + // 2. Diagonalization: Once the matrix is in bidiagonal form, + // the singular values are computed using an iterative process + // (typically involving QR factorization or Jacobi rotations) until convergence. + // This process diagonalizes the bidiagonal matrix and allows extraction of the singular values. + // + // 3. Computing UU and VTVT: After obtaining the singular values, + // the left singular vectors UU and right singular vectors VTVT are computed + // using the accumulated transformations (such as Householder reflections) from the bidiagonalization step. + + // Bidiagnolizations through Householder transformations + public static (Matrix2 U1, Matrix2 B, Matrix2 V1) Bidiagonalization(Matrix2 A) { + int m = A.nRows; // Rows of A + int n = A.nCols; // Columns of A + float[,] U1 = new float[m, m]; // Left orthogonal matrix + float[,] V1 = new float[n, n]; // Right orthogonal matrix + float[,] B = A.Clone().data; // Copy A to B for transformation + + // Initialize U1 and V1 as identity matrices + for (int i = 0; i < m; i++) + U1[i, i] = 1; + for (int i = 0; i < n; i++) + V1[i, i] = 1; + + // Perform Householder reflections to create a bidiagonal matrix B + for (int j = 0; j < n; j++) { + // Step 1: Construct the Householder vector y + float[] y = new float[m - j]; + for (int i = j; i < m; i++) + y[i - j] = B[i, j]; + + // Step 2: Compute the norm and scalar alpha + float norm = 0; + for (int i = 0; i < y.Length; i++) + norm += y[i] * y[i]; + norm = (float)Math.Sqrt(norm); + + if (B[j, j] > 0) + norm = -norm; + + float alpha = (float)Math.Sqrt(0.5 * (norm * (norm - B[j, j]))); + float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j]))); + + // Step 3: Apply the reflection to zero out below diagonal + for (int k = j; k < n; k++) { + float dot = 0; + for (int i = j; i < m; i++) + dot += y[i - j] * B[i, k]; + dot /= r; + + for (int i = j; i < m; i++) + B[i, k] -= 2 * dot * y[i - j]; + } + + // Step 4: Update U1 with the Householder reflection (U1 * Householder) + for (int i = j; i < m; i++) + U1[i, j] = y[i - j] / alpha; + + // Step 5: Update V1 (storing the Householder vector y) + // Correct indexing: we only need to store part of y in V1 from index j to n + for (int i = j; i < n; i++) + V1[j, i] = B[j, i]; + + // Repeat steps for further columns if necessary + } + return (new Matrix2(U1), new Matrix2(B), new Matrix2(V1)); + } + + public static Matrix2 Bidiagonalize(Matrix2 A) { + int m = A.nRows; // Rows of A + int n = A.nCols; // Columns of A + float[,] B = A.Clone().data; // Copy A to B for transformation + + // Perform Householder reflections to create a bidiagonal matrix B + for (int j = 0; j < n; j++) { + // Step 1: Construct the Householder vector y + float[] y = new float[m - j]; + for (int i = j; i < m; i++) + y[i - j] = B[i, j]; + + // Step 2: Compute the norm and scalar alpha + float norm = 0; + for (int i = 0; i < y.Length; i++) + norm += y[i] * y[i]; + norm = (float)Math.Sqrt(norm); + + if (B[j, j] > 0) + norm = -norm; + + float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j]))); + + // Step 3: Apply the reflection to zero out below diagonal + for (int k = j; k < n; k++) { + float dot = 0; + for (int i = j; i < m; i++) + dot += y[i - j] * B[i, k]; + dot /= r; + + for (int i = j; i < m; i++) + B[i, k] -= 2 * dot * y[i - j]; + } + + // Repeat steps for further columns if necessary + } + return new Matrix2(B); + } + + // QR Iteration for diagonalization of a bidiagonal matrix B + public static (Matrix1 singularValues, Matrix2 U, Matrix2 Vt) QRIteration(Matrix2 B) { + int m = B.nRows; + int n = B.nCols; + + Matrix2 U = new(m, m); // Left singular vectors (U) + Matrix2 Vt = new(n, n); // Right singular vectors (V^T) + float[] singularValues = new float[Math.Min(m, n)]; // Singular values + + // Initialize U and Vt as identity matrices + for (int i = 0; i < m; i++) + U.data[i, i] = 1; + for (int i = 0; i < n; i++) + Vt.data[i, i] = 1; + + // Perform QR iterations + float tolerance = 1e-7f; //1e-12f; for double + bool converged = false; + while (!converged) { + // Perform QR decomposition on the matrix B + (Matrix2 Q, Matrix2 R) = QR.Decomposition(B); + + // Update B to be the product Q * R //R * Q + B = R * Q; + + // Accumulate the transformations in U and Vt + U *= Q; + Vt *= R; + + // Check convergence by looking at the off-diagonal elements of B + converged = true; + for (int i = 0; i < m - 1; i++) { + for (int j = i + 1; j < n; j++) { + if (Math.Abs(B.data[i, j]) > tolerance) { + converged = false; + break; + } + } + } + } + + // Extract singular values (diagonal elements of B) + for (int i = 0; i < Math.Min(m, n); i++) + singularValues[i] = B.data[i, i]; + + return (new Matrix1(singularValues), U, Vt); + } + + public static (Matrix2 U, Matrix1 S, Matrix2 Vt) Decomposition(Matrix2 A) { + if (A.nRows != A.nCols) + throw new ArgumentException("SVD: matrix A has to be square."); + + Matrix2 B = Bidiagonalize(A); + (Matrix1 S, Matrix2 U, Matrix2 Vt) = QRIteration(B); + return (U, S, Vt); + } + } +} \ No newline at end of file From 8a2c83342a7ea7a1e2450324fd7e2de87b5cffdc Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 17 Apr 2025 09:33:17 +0200 Subject: [PATCH 04/20] Fixes --- src/Direction.cs | 4 ---- src/Matrix.cs | 2 -- src/Quaternion.cs | 3 +-- src/SwingTwist.cs | 2 -- 4 files changed, 1 insertion(+), 10 deletions(-) diff --git a/src/Direction.cs b/src/Direction.cs index f479744..6039bd5 100644 --- a/src/Direction.cs +++ b/src/Direction.cs @@ -6,10 +6,6 @@ using Vector3Float = UnityEngine.Vector3; namespace LinearAlgebra { - public class Direction - { -namespace LinearAlgebra { - /// /// A direction in 3D space /// diff --git a/src/Matrix.cs b/src/Matrix.cs index b50eb16..5196d48 100644 --- a/src/Matrix.cs +++ b/src/Matrix.cs @@ -84,8 +84,6 @@ namespace LinearAlgebra { 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); public void FillDiagonal(float f) { int n = Math.Min(this.nRows, this.nCols); diff --git a/src/Quaternion.cs b/src/Quaternion.cs index cd7e9f4..52dd26b 100644 --- a/src/Quaternion.cs +++ b/src/Quaternion.cs @@ -18,12 +18,11 @@ namespace LinearAlgebra { this.w = w; } +#if UNITY_5_3_OR_NEWER public static Quaternion Reflect(Quaternion q) { return new(-q.x, -q.y, -q.z, q.w); } - -#if UNITY_5_3_OR_NEWER public static Matrix2 ToRotationMatrix(Quaternion q) { float w = q.x, x = q.y, y = q.z, z = q.w; diff --git a/src/SwingTwist.cs b/src/SwingTwist.cs index fe9a1b6..22eb0bb 100644 --- a/src/SwingTwist.cs +++ b/src/SwingTwist.cs @@ -3,8 +3,6 @@ using System.Numerics; using Quaternion = UnityEngine.Quaternion; #endif -namespace LinearAlgebra -{ namespace LinearAlgebra { public class SwingTwist { From 0b2071ab6694534bf057316b6dc424f102a28075 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 17 Apr 2025 09:38:15 +0200 Subject: [PATCH 05/20] Compatibility Fixes --- src/Spherical.cs | 49 ++++++++++++++++++++++--------------------- test/SphericalTest.cs | 4 +++- 2 files changed, 28 insertions(+), 25 deletions(-) diff --git a/src/Spherical.cs b/src/Spherical.cs index ad6d9b8..456646f 100644 --- a/src/Spherical.cs +++ b/src/Spherical.cs @@ -1,6 +1,7 @@ using System; #if UNITY_5_3_OR_NEWER -using Vector3Float = UnityEngine.Vector3; +//using Vector3Float = UnityEngine.Vector3; +using Vector3 = UnityEngine.Vector3; #endif namespace LinearAlgebra { @@ -66,16 +67,16 @@ namespace LinearAlgebra { public readonly static Spherical forward = new(1, Direction.forward); - public static Spherical FromVector3Float(Vector3Float v) { - float distance = v.magnitude; - if (distance == 0.0f) - return Spherical.zero; - 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; - return Spherical.Degrees(distance, horizontalAngle, verticalAngle); - } - } + // public static Spherical FromVector3Float(Vector3Float v) { + // float distance = v.magnitude; + // if (distance == 0.0f) + // return Spherical.zero; + // 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; + // return Spherical.Degrees(distance, horizontalAngle, verticalAngle); + // } + // } public static Spherical FromVector3(Vector3 v) { float distance = v.magnitude; @@ -88,21 +89,21 @@ namespace LinearAlgebra { } } - public Vector3Float ToVector3Float() { - float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad; - float horizontalRad = this.direction.horizontal * Angle.Deg2Rad; - float cosVertical = (float)Math.Cos(verticalRad); - float sinVertical = (float)Math.Sin(verticalRad); - float cosHorizontal = (float)Math.Cos(horizontalRad); - float sinHorizontal = (float)Math.Sin(horizontalRad); + // public Vector3Float ToVector3Float() { + // float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad; + // float horizontalRad = this.direction.horizontal * Angle.Deg2Rad; + // float cosVertical = (float)Math.Cos(verticalRad); + // float sinVertical = (float)Math.Sin(verticalRad); + // float cosHorizontal = (float)Math.Cos(horizontalRad); + // float sinHorizontal = (float)Math.Sin(horizontalRad); - float x = this.distance * sinVertical * sinHorizontal; - float y = this.distance * cosVertical; - float z = this.distance * sinVertical * cosHorizontal; + // float x = this.distance * sinVertical * sinHorizontal; + // float y = this.distance * cosVertical; + // float z = this.distance * sinVertical * cosHorizontal; - Vector3Float v = new(x, y, z); - return v; - } + // Vector3Float v = new(x, y, z); + // return v; + // } public Vector3 ToVector3() { float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad; diff --git a/test/SphericalTest.cs b/test/SphericalTest.cs index d9cca1d..3ede4f4 100644 --- a/test/SphericalTest.cs +++ b/test/SphericalTest.cs @@ -1,3 +1,4 @@ +#if !UNITY_5_6_OR_NEWER using NUnit.Framework; namespace LinearAlgebra.Test { @@ -25,4 +26,5 @@ namespace LinearAlgebra.Test { Assert.AreEqual(v1.distance, r.distance, "Addition(0,0,0)"); } } -} \ No newline at end of file +} +#endif \ No newline at end of file From 244027fe5022d6890cc3b064fd480e1595592b31 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Thu, 24 Apr 2025 14:13:54 +0200 Subject: [PATCH 06/20] Added RoboidControl repo --- .editorconfig | 19 + .gitignore | 6 + src/Angle.cs | 110 ++++++ src/Decomposition.cs | 287 ++++++++++++++ src/Direction.cs | 83 ++++ src/Float.cs | 41 ++ src/LinearAlgebra.csproj | 14 + src/Matrix.cs | 689 +++++++++++++++++++++++++++++++++ src/Quat32.cs | 87 +++++ src/Quaternion.cs | 81 ++++ src/Spherical.cs | 134 +++++++ src/SwingTwist.cs | 41 ++ src/Vector2.cs | 363 +++++++++++++++++ src/Vector3.cs | 179 +++++++++ src/float16.cs | 322 +++++++++++++++ test/AngleTest.cs | 171 ++++++++ test/LinearAlgebra_Test.csproj | 19 + test/SphericalTest.cs | 30 ++ 18 files changed, 2676 insertions(+) create mode 100644 .editorconfig create mode 100644 .gitignore create mode 100644 src/Angle.cs create mode 100644 src/Decomposition.cs create mode 100644 src/Direction.cs create mode 100644 src/Float.cs create mode 100644 src/LinearAlgebra.csproj create mode 100644 src/Matrix.cs create mode 100644 src/Quat32.cs create mode 100644 src/Quaternion.cs create mode 100644 src/Spherical.cs create mode 100644 src/SwingTwist.cs create mode 100644 src/Vector2.cs create mode 100644 src/Vector3.cs create mode 100644 src/float16.cs create mode 100644 test/AngleTest.cs create mode 100644 test/LinearAlgebra_Test.csproj create mode 100644 test/SphericalTest.cs diff --git a/.editorconfig b/.editorconfig new file mode 100644 index 0000000..1ec7f97 --- /dev/null +++ b/.editorconfig @@ -0,0 +1,19 @@ +# EditorConfig is awesome: https://EditorConfig.org + +# top-most EditorConfig file +root = true + +[*] +indent_style = space +indent_size = 4 +end_of_line = crlf +charset = utf-8 +trim_trailing_whitespace = false +insert_final_newline = false +max_line_length = 80 + +[*.cs] +csharp_new_line_before_open_brace = none +# Suppress warnings everywhere +dotnet_diagnostic.IDE1006.severity = none +dotnet_diagnostic.IDE0130.severity = none \ No newline at end of file diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..f0b2f47 --- /dev/null +++ b/.gitignore @@ -0,0 +1,6 @@ +DoxyGen/DoxyWarnLogfile.txt +.vscode/settings.json +**bin +**obj +**.meta +*.sln diff --git a/src/Angle.cs b/src/Angle.cs new file mode 100644 index 0000000..f056e5a --- /dev/null +++ b/src/Angle.cs @@ -0,0 +1,110 @@ +using System; + +namespace LinearAlgebra { + + /// + /// %Angle utilities + /// + 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; + + public const float Rad2Deg = 360.0f / ((float)Math.PI * 2); //0.0174532924F; + public const float Deg2Rad = ((float)Math.PI * 2) / 360.0f; //57.29578F; + + /// + /// Clamp the angle between the given min and max values + /// + /// The angle to clamp + /// The minimum angle + /// The maximum angle + /// The clamped angle + /// Angles are normalized + public static float Clamp(float angle, float min, float max) { + float normalizedAngle = Normalize(angle); + return Float.Clamp(normalizedAngle, min, max); + } + + /// + /// Determine the angle difference, result is a normalized angle + /// + /// First first angle + /// The second angle + /// the angle between the two angles + /// Angle values should be degrees + public static float Difference(float a, float b) { + float r = Normalize(b - a); + return r; + } + + /// + /// Normalize an angle to the range -180 < angle <= 180 + /// + /// The angle to normalize + /// The normalized angle in interval (-180..180] + /// Angle values should be in degrees + public static float Normalize(float angle) { + if (float.IsInfinity(angle)) + return angle; + + while (angle <= -180) angle += 360; + while (angle > 180) angle -= 360; + return angle; + } + + /// + /// Rotate from one angle to the other with a maximum degrees + /// + /// Starting angle + /// Target angle + /// 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) { + float d = toAngle - fromAngle; + d = Normalize(d); + d = Math.Sign(d) * Float.Clamp(Math.Abs(d), 0, maxAngle); + return fromAngle + d; + } + + /// + /// Map interval of angles between vectors [0..Pi] to interval [0..1] + /// + /// The first vector + /// The second vector + /// The resulting factor in interval [0..1] + /// 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) { + return (1 - Vector2.Dot(v1, v2)) / 2; + } + + // Normalize all vector angles to the range -180 < angle < 180 + //public static Vector3 Normalize(Vector3 angles) { + // float x = Normalize(angles.x); + // float y = Normalize(angles.y); + // float z = Normalize(angles.z); + // return new Vector3(x, y, z); + //} + + // Returns the signed angle in degrees between from and to. + //public static float SignedAngle(Vector3 from, Vector3 to) { + // float angle = Vector3.Angle(from, to); + // Vector3 cross = Vector3.Cross(from, to); + // if (cross.y < 0) angle = -angle; + // return angle; + //} + + // Returns the signed angle in degrees between from and to. + //public static float SignedAngle(Vector2 from, Vector2 to) { + // float sign = Math.Sign(from.y * to.x - from.x * to.y); + // return Vector2.Angle(from, to) * sign; + //} + + //public static Quaternion ToQuaternion(Rotation orientation) { + // return new Quaternion(orientation.x, orientation.y, orientation.z, orientation.w); + //} + } +} \ No newline at end of file diff --git a/src/Decomposition.cs b/src/Decomposition.cs new file mode 100644 index 0000000..ddaf434 --- /dev/null +++ b/src/Decomposition.cs @@ -0,0 +1,287 @@ +using System; +namespace LinearAlgebra { + class QR { + // QR Decomposition of a matrix A + public static (Matrix2 Q, Matrix2 R) Decomposition(Matrix2 A) { + int nRows = A.nRows; + int nCols = A.nCols; + + float[,] Q = new float[nRows, nCols]; + float[,] R = new float[nCols, nCols]; + + // Perform Gram-Schmidt orthogonalization + for (uint colIx = 0; colIx < nCols; colIx++) { + + // Step 1: v = column(ix) of A + float[] v = new float[nRows]; + for (int rowIx = 0; rowIx < nRows; rowIx++) + v[rowIx] = A.data[rowIx, colIx]; + + // Step 2: Subtract projections of v onto previous q's (orthogonalize) + for (uint colIx2 = 0; colIx2 < colIx; colIx2++) { + float dotProd = 0; + for (int i = 0; i < nRows; i++) + dotProd += Q[i, colIx2] * v[i]; + for (int i = 0; i < nRows; i++) + v[i] -= dotProd * Q[i, colIx2]; + } + + // Step 3: Normalize v to get column(ix) of Q + float norm = 0; + for (int rowIx = 0; rowIx < nRows; rowIx++) + norm += v[rowIx] * v[rowIx]; + norm = (float)Math.Sqrt(norm); + + for (int rowIx = 0; rowIx < nRows; rowIx++) + Q[rowIx, colIx] = v[rowIx] / norm; + + // Store the coefficients of R + for (int colIx2 = 0; colIx2 <= colIx; colIx2++) { + R[colIx2, colIx] = 0; + for (int k = 0; k < nRows; k++) + R[colIx2, colIx] += Q[k, colIx2] * A.data[k, colIx]; + } + } + return (new Matrix2(Q), new Matrix2(R)); + } + + // Reduced QR Decomposition of a matrix A + public static (Matrix2 Q, Matrix2 R) ReducedDecomposition(Matrix2 A) { + int nRows = A.nRows; + int nCols = A.nCols; + + float[,] Q = new float[nRows, nCols]; + float[,] R = new float[nCols, nCols]; + + // Perform Gram-Schmidt orthogonalization + for (int colIx = 0; colIx < nCols; colIx++) { + + // Step 1: v = column(colIx) of A + float[] columnIx = new float[nRows]; + bool isZeroColumn = true; + for (int rowIx = 0; rowIx < nRows; rowIx++) { + columnIx[rowIx] = A.data[rowIx, colIx]; + if (columnIx[rowIx] != 0) + isZeroColumn = false; + } + if (isZeroColumn) { + for (int rowIx = 0; rowIx < nRows; rowIx++) + Q[rowIx, colIx] = 0; + // Set corresponding R element to 0 + R[colIx, colIx] = 0; + + Console.WriteLine($"zero column {colIx}"); + + continue; + } + + // Step 2: Subtract projections of v onto previous q's (orthogonalize) + for (int colIx2 = 0; colIx2 < colIx; colIx2++) { + // Compute the dot product of v and column(colIx2) of Q + float dotProduct = 0; + for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++) + dotProduct += columnIx[rowIx2] * Q[rowIx2, colIx2]; + // Subtract the projection from v + for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++) + columnIx[rowIx2] -= dotProduct * Q[rowIx2, colIx2]; + } + + // Step 3: Normalize v to get column(colIx) of Q + float norm = 0; + for (int rowIx = 0; rowIx < nRows; rowIx++) + norm += columnIx[rowIx] * columnIx[rowIx]; + if (norm == 0) + throw new Exception("invalid value"); + + norm = (float)Math.Sqrt(norm); + + for (int rowIx = 0; rowIx < nRows; rowIx++) + Q[rowIx, colIx] = columnIx[rowIx] / norm; + + // Here is where it deviates from the Full QR Decomposition ! + + // Step 4: Compute the row(colIx) of R + for (int colIx2 = colIx; colIx2 < nCols; colIx2++) { + float dotProduct = 0; + for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++) + dotProduct += Q[rowIx2, colIx] * A.data[rowIx2, colIx2]; + R[colIx, colIx2] = dotProduct; + } + } + if (!float.IsFinite(R[0, 0])) + throw new Exception("invalid value"); + + return (new Matrix2(Q), new Matrix2(R)); + } + } + + class SVD { + // According to ChatGPT, Mathnet uses Golub-Reinsch SVD algorithm + // 1. Bidiagonalization: The input matrix AA is reduced to a bidiagonal form using Golub-Kahan bidiagonalization. + // This process involves applying a sequence of Householder reflections to AA to create a bidiagonal matrix. + // This step reduces the complexity by making the matrix simpler while retaining the essential structure needed for SVD. + // + // 2. Diagonalization: Once the matrix is in bidiagonal form, + // the singular values are computed using an iterative process + // (typically involving QR factorization or Jacobi rotations) until convergence. + // This process diagonalizes the bidiagonal matrix and allows extraction of the singular values. + // + // 3. Computing UU and VTVT: After obtaining the singular values, + // the left singular vectors UU and right singular vectors VTVT are computed + // using the accumulated transformations (such as Householder reflections) from the bidiagonalization step. + + // Bidiagnolizations through Householder transformations + public static (Matrix2 U1, Matrix2 B, Matrix2 V1) Bidiagonalization(Matrix2 A) { + int m = A.nRows; // Rows of A + int n = A.nCols; // Columns of A + float[,] U1 = new float[m, m]; // Left orthogonal matrix + float[,] V1 = new float[n, n]; // Right orthogonal matrix + float[,] B = A.Clone().data; // Copy A to B for transformation + + // Initialize U1 and V1 as identity matrices + for (int i = 0; i < m; i++) + U1[i, i] = 1; + for (int i = 0; i < n; i++) + V1[i, i] = 1; + + // Perform Householder reflections to create a bidiagonal matrix B + for (int j = 0; j < n; j++) { + // Step 1: Construct the Householder vector y + float[] y = new float[m - j]; + for (int i = j; i < m; i++) + y[i - j] = B[i, j]; + + // Step 2: Compute the norm and scalar alpha + float norm = 0; + for (int i = 0; i < y.Length; i++) + norm += y[i] * y[i]; + norm = (float)Math.Sqrt(norm); + + if (B[j, j] > 0) + norm = -norm; + + float alpha = (float)Math.Sqrt(0.5 * (norm * (norm - B[j, j]))); + float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j]))); + + // Step 3: Apply the reflection to zero out below diagonal + for (int k = j; k < n; k++) { + float dot = 0; + for (int i = j; i < m; i++) + dot += y[i - j] * B[i, k]; + dot /= r; + + for (int i = j; i < m; i++) + B[i, k] -= 2 * dot * y[i - j]; + } + + // Step 4: Update U1 with the Householder reflection (U1 * Householder) + for (int i = j; i < m; i++) + U1[i, j] = y[i - j] / alpha; + + // Step 5: Update V1 (storing the Householder vector y) + // Correct indexing: we only need to store part of y in V1 from index j to n + for (int i = j; i < n; i++) + V1[j, i] = B[j, i]; + + // Repeat steps for further columns if necessary + } + return (new Matrix2(U1), new Matrix2(B), new Matrix2(V1)); + } + + public static Matrix2 Bidiagonalize(Matrix2 A) { + int m = A.nRows; // Rows of A + int n = A.nCols; // Columns of A + float[,] B = A.Clone().data; // Copy A to B for transformation + + // Perform Householder reflections to create a bidiagonal matrix B + for (int j = 0; j < n; j++) { + // Step 1: Construct the Householder vector y + float[] y = new float[m - j]; + for (int i = j; i < m; i++) + y[i - j] = B[i, j]; + + // Step 2: Compute the norm and scalar alpha + float norm = 0; + for (int i = 0; i < y.Length; i++) + norm += y[i] * y[i]; + norm = (float)Math.Sqrt(norm); + + if (B[j, j] > 0) + norm = -norm; + + float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j]))); + + // Step 3: Apply the reflection to zero out below diagonal + for (int k = j; k < n; k++) { + float dot = 0; + for (int i = j; i < m; i++) + dot += y[i - j] * B[i, k]; + dot /= r; + + for (int i = j; i < m; i++) + B[i, k] -= 2 * dot * y[i - j]; + } + + // Repeat steps for further columns if necessary + } + return new Matrix2(B); + } + + // QR Iteration for diagonalization of a bidiagonal matrix B + public static (Matrix1 singularValues, Matrix2 U, Matrix2 Vt) QRIteration(Matrix2 B) { + int m = B.nRows; + int n = B.nCols; + + Matrix2 U = new(m, m); // Left singular vectors (U) + Matrix2 Vt = new(n, n); // Right singular vectors (V^T) + float[] singularValues = new float[Math.Min(m, n)]; // Singular values + + // Initialize U and Vt as identity matrices + for (int i = 0; i < m; i++) + U.data[i, i] = 1; + for (int i = 0; i < n; i++) + Vt.data[i, i] = 1; + + // Perform QR iterations + float tolerance = 1e-7f; //1e-12f; for double + bool converged = false; + while (!converged) { + // Perform QR decomposition on the matrix B + (Matrix2 Q, Matrix2 R) = QR.Decomposition(B); + + // Update B to be the product Q * R //R * Q + B = R * Q; + + // Accumulate the transformations in U and Vt + U *= Q; + Vt *= R; + + // Check convergence by looking at the off-diagonal elements of B + converged = true; + for (int i = 0; i < m - 1; i++) { + for (int j = i + 1; j < n; j++) { + if (Math.Abs(B.data[i, j]) > tolerance) { + converged = false; + break; + } + } + } + } + + // Extract singular values (diagonal elements of B) + for (int i = 0; i < Math.Min(m, n); i++) + singularValues[i] = B.data[i, i]; + + return (new Matrix1(singularValues), U, Vt); + } + + public static (Matrix2 U, Matrix1 S, Matrix2 Vt) Decomposition(Matrix2 A) { + if (A.nRows != A.nCols) + throw new ArgumentException("SVD: matrix A has to be square."); + + Matrix2 B = Bidiagonalize(A); + (Matrix1 S, Matrix2 U, Matrix2 Vt) = QRIteration(B); + return (U, S, Vt); + } + } +} \ No newline at end of file diff --git a/src/Direction.cs b/src/Direction.cs new file mode 100644 index 0000000..6039bd5 --- /dev/null +++ b/src/Direction.cs @@ -0,0 +1,83 @@ +using System; +#if UNITY_5_3_OR_NEWER +using Vector3Float = UnityEngine.Vector3; +#endif + +namespace LinearAlgebra +{ + + /// + /// A direction in 3D space + /// + /// A direction is represented using two angles: + /// * The horizontal angle ranging from -180 (inclusive) to 180 (exclusive) + /// degrees which is a rotation in the horizontal plane + /// * A vertical angle ranging from -90 (inclusive) to 90 (exclusive) degrees + /// which is the rotation in the up/down direction applied after the horizontal + /// rotation has been applied. + /// The angles are automatically normalized to stay within the abovenmentioned + /// ranges. + public class Direction { + public float horizontal; + public float vertical; + + public Direction() { + horizontal = 0; + vertical = 0; + } + // public Direction(float horizontal, float vertical) { + // this.horizontal = horizontal; + // this.vertical = vertical; + // //Normalize(); + // } + + public static Direction Degrees(float horizontal, float vertical) { + Direction d = new() { + horizontal = horizontal, + vertical = vertical + }; + //Normalize(); + return d; + } + public static Direction Radians(float horizontal, float vertical) { + Direction d = new() { + horizontal = horizontal * Angle.Rad2Deg, + vertical = vertical * Angle.Rad2Deg + }; + //Normalize(); + return d; + } + + public readonly static Direction forward = Degrees(0, 0); + public readonly static Direction backward = Degrees(-180, 0); + public readonly static Direction up = Degrees(0, 90); + public readonly static Direction down = Degrees(0, -90); + public readonly static Direction left = Degrees(-90, 0); + public readonly static Direction right = Degrees(90, 0); + + public void Normalize() { + if (this.vertical > 90 || this.vertical < -90) { + this.horizontal += 180; + this.vertical = 180 - this.vertical; + } + } + + public Vector3Float ToVector3() + { + float verticalRad = (Angle.pi / 2) - this.vertical * Angle.Deg2Rad; + float horizontalRad = this.horizontal * Angle.Deg2Rad; + float cosVertical = (float)Math.Cos(verticalRad); + float sinVertical = (float)Math.Sin(verticalRad); + float cosHorizontal = (float)Math.Cos(horizontalRad); + float sinHorizontal = (float)Math.Sin(horizontalRad); + + float x = sinVertical * sinHorizontal; + float y = cosVertical; + float z = sinVertical * cosHorizontal; + + Vector3Float v = new(x, y, z); + return v; + } + } + +} \ No newline at end of file diff --git a/src/Float.cs b/src/Float.cs new file mode 100644 index 0000000..2217b84 --- /dev/null +++ b/src/Float.cs @@ -0,0 +1,41 @@ +namespace LinearAlgebra { + + /// + /// Float number utilities + /// + public class Float { + /// + /// The precision of float numbers + /// + public const float epsilon = 1E-05f; + /// + /// The square of the float number precision + /// + public const float sqrEpsilon = 1e-10f; + + /// + /// Clamp the value between the given minimum and maximum values + /// + /// The value to clamp + /// The minimum value + /// The maximum value + /// The clamped value + public static float Clamp(float f, float min, float max) { + if (f < min) + return min; + if (f > max) + return max; + return f; + } + + /// + /// Clamp the value between to the interval [0..1] + /// + /// The value to clamp + /// The clamped value + public static float Clamp01(float f) { + return Clamp(f, 0, 1); + } + } + +} \ No newline at end of file diff --git a/src/LinearAlgebra.csproj b/src/LinearAlgebra.csproj new file mode 100644 index 0000000..14d3947 --- /dev/null +++ b/src/LinearAlgebra.csproj @@ -0,0 +1,14 @@ + + + + false + false + net5.0 + + + + + + + + diff --git a/src/Matrix.cs b/src/Matrix.cs new file mode 100644 index 0000000..5196d48 --- /dev/null +++ b/src/Matrix.cs @@ -0,0 +1,689 @@ +using System; +#if UNITY_5_3_OR_NEWER +using Vector3Float = UnityEngine.Vector3; +using Vector2Float = UnityEngine.Vector2; +using Quaternion = UnityEngine.Quaternion; +#endif + +namespace LinearAlgebra { + + public readonly struct Slice + { + public int start { get; } + public int stop { get; } + public Slice(int start, int stop) + { + this.start = start; + this.stop = stop; + } + } + + public class Matrix2 { + public float[,] data { get; } + + public int nRows => data.GetLength(0); + public int nCols => data.GetLength(1); + + public Matrix2(int nRows, int 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]; + } + return new Matrix2(data); + } + + public static Matrix2 Zero(int nRows, int 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(int size) + { + return Diagonal(1, size); + } + public static Matrix2 Identity(int nRows, int 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, int 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) + { + int n = (int)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) + { + int 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); + } + + public Matrix1 GetRow(int rowIx) { + float[] row = new float[this.nCols]; + for (int colIx = 0; colIx < this.nCols; colIx++) { + row[colIx] = this.data[rowIx, colIx]; + } + return new Matrix1(row); + } + +#if UNITY_5_3_OR_NEWER + public Vector3Float GetRow3(int rowIx) { + int cols = this.nCols; + Vector3Float row = new() { + x = this.data[rowIx, 0], + y = this.data[rowIx, 1], + z = this.data[rowIx, 2] + }; + 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 void SwapRows(int row1, int row2) { + for (uint ix = 0; ix < this.nCols; ix++) { + float temp = this.data[row1, ix]; + this.data[row1, ix] = this.data[row2, ix]; + this.data[row2, ix] = temp; + } + } + + 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 void SetColumn(int colIx, Matrix1 v) { + for (uint ix = 0; ix < v.size; ix++) + this.data[ix, colIx] = v.data[ix]; + } + public void SetColumn(int colIx, Vector3Float v) { + this.data[0, colIx] = v.x; + this.data[1, colIx] = v.y; + this.data[2, colIx] = v.z; + } + + 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 GetRows(Slice slice) { + return GetRows(slice.start, slice.stop); + } + public Matrix2 GetRows(int from, int 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 (int colIx = 0; colIx < this.nCols; colIx++) + result[resultRowIx, colIx] = this.data[rowIx, colIx]; + + resultRowIx++; + } + + return new Matrix2(result); + } + + public Matrix2 Slice(Slice slice) + { + return Slice(slice.start, slice.stop); + } + public Matrix2 Slice(int from, int 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 (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((int start, int stop) rowRange, (int start, int stop) colRange) + { + float[,] result = new float[rowRange.stop - rowRange.start, colRange.stop - colRange.start]; + + int resultRowIx = 0; + int resultColIx = 0; + for (int i = rowRange.start; i < rowRange.stop; i++) + { + for (int 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) { + UpdateSlice((slice.start, slice.stop), m); + } + public void UpdateSlice((int start, int stop) slice, Matrix2 m) { + // if (slice.start == slice.stop) + // Console.WriteLine("WARNING: no data is updates when start equals stop in a slice!"); + int mRowIx = 0; + for (int 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((int start, int stop) rowRange, (int start, int stop) colRange, Matrix2 m) + { + for (int i = rowRange.start; i < rowRange.stop; i++) + { + for (int 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 + int 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 + } + } + + // 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]; + } + } + + // Back substitution + for (int i = n - 1; i >= 0; i--) + { + // Eliminate the column above the pivot + for (int 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]; + } + } + + // 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); + } + + public float Determinant() + { + int 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) + { + int 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 static Matrix2 DeleteRows(Matrix2 A, Slice rowRange) { + float[,] result = new float[A.nRows - (rowRange.stop - rowRange.start), A.nCols]; + + int resultRowIx = 0; + for (int i = 0; i < A.nRows; i++) { + if (i >= rowRange.start && i < rowRange.stop) + continue; + + for (int j = 0; j < A.nCols; j++) + result[resultRowIx, j] = A.data[i, j]; + + resultRowIx++; + } + return new Matrix2(result); + } + + internal static Matrix2 DeleteColumns(Matrix2 A, Slice colRange) { + float[,] result = new float[A.nRows, A.nCols - (colRange.stop - colRange.start)]; + + for (int i = 0; i < A.nRows; i++) { + int resultColIx = 0; + for (int j = 0; j < A.nCols; j++) { + if (j >= colRange.start && j < colRange.stop) + continue; + + result[i, resultColIx++] = A.data[i, j]; + } + } + return new Matrix2(result); + } + } + + public class Matrix1 + { + public float[] data { get; } + + public int size => data.GetLength(0); + + public Matrix1(int size) + { + this.data = new float[size]; + } + + public Matrix1(float[] data) { + this.data = data; + } + + public static Matrix1 Zero(int 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); + } + +#if UNITY_5_3_OR_NEWER + 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); + } +#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 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 { + get { + 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]); + } + } +#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 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, 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 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 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) { + float[] result = new float[A.size]; + + for (int i = 0; i < A.size; i++) + result[i] = f / A.data[i]; + + return new Matrix1(result); + } + + public Matrix1 Slice(Slice range) + { + return Slice(range.start, range.stop); + } + public Matrix1 Slice(int from, int 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 (int 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++) + this.data[ix] = v.data[vIx]; + } + } + +} \ No newline at end of file diff --git a/src/Quat32.cs b/src/Quat32.cs new file mode 100644 index 0000000..f13266d --- /dev/null +++ b/src/Quat32.cs @@ -0,0 +1,87 @@ +using System; + +namespace LinearAlgebra { + public class Quat32 { + public float x; + public float y; + public float z; + public float w; + + public Quat32() { + this.x = 0; + this.y = 0; + this.z = 0; + this.w = 1; + } + + public Quat32(float x, float y, float z, float w) { + this.x = x; + this.y = y; + this.z = z; + this.w = w; + } + + public static Quat32 FromSwingTwist(SwingTwist s) { + Quat32 q32 = Quat32.Euler(-s.swing.vertical, s.swing.horizontal, s.twist); + return q32; + } + + public static Quat32 Euler(float yaw, float pitch, float roll) { + float rollOver2 = roll * Angle.Deg2Rad * 0.5f; + float sinRollOver2 = (float)Math.Sin((float)rollOver2); + float cosRollOver2 = (float)Math.Cos((float)rollOver2); + float pitchOver2 = pitch * 0.5f; + float sinPitchOver2 = (float)Math.Sin((float)pitchOver2); + float cosPitchOver2 = (float)Math.Cos((float)pitchOver2); + float yawOver2 = yaw * 0.5f; + float sinYawOver2 = (float)Math.Sin((float)yawOver2); + float cosYawOver2 = (float)Math.Cos((float)yawOver2); + Quat32 result = new Quat32() { + w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + + sinYawOver2 * sinPitchOver2 * sinRollOver2, + x = sinYawOver2 * cosPitchOver2 * cosRollOver2 + + cosYawOver2 * sinPitchOver2 * sinRollOver2, + y = cosYawOver2 * sinPitchOver2 * cosRollOver2 - + sinYawOver2 * cosPitchOver2 * sinRollOver2, + z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - + sinYawOver2 * sinPitchOver2 * cosRollOver2 + }; + return result; + } + + public void ToAngles(out float right, out float up, out float forward) { + float test = this.x * this.y + this.z * this.w; + if (test > 0.499f) { // singularity at north pole + right = 0; + up = 2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg; + forward = 90; + return; + //return Vector3(0, 2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, 90); + } + else if (test < -0.499f) { // singularity at south pole + right = 0; + up = -2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg; + forward = -90; + return; + //return Vector3(0, -2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, -90); + } + else { + float sqx = this.x * this.x; + float sqy = this.y * this.y; + float sqz = this.z * this.z; + + right = (float)Math.Atan2(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * Angle.Rad2Deg; + up = (float)Math.Atan2(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * Angle.Rad2Deg; + forward = (float)Math.Asin(2 * test) * Angle.Rad2Deg; + return; + // return Vector3( + // atan2f(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * + // Rad2Deg, + // atan2f(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * + // Rad2Deg, + // asinf(2 * test) * Angle.Rad2Deg); + } + } + + } +} \ No newline at end of file diff --git a/src/Quaternion.cs b/src/Quaternion.cs new file mode 100644 index 0000000..52dd26b --- /dev/null +++ b/src/Quaternion.cs @@ -0,0 +1,81 @@ +using System; +#if UNITY_5_3_OR_NEWER +using Quaternion = UnityEngine.Quaternion; +#endif + +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) { + this.x = x; + this.y = y; + this.z = z; + this.w = w; + } + +#if UNITY_5_3_OR_NEWER + public static Quaternion Reflect(Quaternion q) { + return new(-q.x, -q.y, -q.z, q.w); + } + + public static Matrix2 ToRotationMatrix(Quaternion q) { + float w = q.x, x = q.y, y = q.z, z = q.w; + + 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); + } + + 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); + } +#endif + } + + // 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 diff --git a/src/Spherical.cs b/src/Spherical.cs new file mode 100644 index 0000000..456646f --- /dev/null +++ b/src/Spherical.cs @@ -0,0 +1,134 @@ +using System; +#if UNITY_5_3_OR_NEWER +//using Vector3Float = UnityEngine.Vector3; +using Vector3 = UnityEngine.Vector3; +#endif + +namespace LinearAlgebra { + /// + /// A spherical vector + /// + public class Spherical { + + /// + /// Create a default vector with zero distance + /// + public Spherical() { + this.distance = 0; + this.direction = new Direction(); + } + + /// + /// Create a spherical vector + /// + /// The distance in meters + /// The direction of the vector + public Spherical(float distance, Direction direction) { + this.distance = distance; + this.direction = direction; + } + + /// + /// Create spherical vector. All given angles are in degrees + /// + /// The distance in meters + /// The horizontal angle in degrees + /// The vertical angle in degrees + /// + public static Spherical Degrees(float distance, float horizontal, float vertical) { + Direction direction = Direction.Degrees(horizontal, vertical); + Spherical s = new(distance, direction); + return s; + } + + public static Spherical Radians(float distance, float horizontal, float vertical) { + Direction direction = Direction.Radians(horizontal, vertical); + Spherical s = new(distance, direction); + return s; + } + + /// + /// The distance in meters + /// + /// @remark The distance should never be negative + public float distance; + /// + /// The direction of the vector + /// + public Direction direction; + + /// + /// A spherical vector with zero degree angles and distance + /// + public readonly static Spherical zero = new(0, Direction.forward); + /// + /// A normalized forward-oriented vector + /// + public readonly static Spherical forward = new(1, Direction.forward); + + + // public static Spherical FromVector3Float(Vector3Float v) { + // float distance = v.magnitude; + // if (distance == 0.0f) + // return Spherical.zero; + // 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; + // return Spherical.Degrees(distance, horizontalAngle, verticalAngle); + // } + // } + + public static Spherical FromVector3(Vector3 v) { + float distance = v.magnitude; + if (distance == 0.0f) + return Spherical.zero; + 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; + return Spherical.Degrees(distance, horizontalAngle, verticalAngle); + } + } + + // public Vector3Float ToVector3Float() { + // float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad; + // float horizontalRad = this.direction.horizontal * Angle.Deg2Rad; + // float cosVertical = (float)Math.Cos(verticalRad); + // float sinVertical = (float)Math.Sin(verticalRad); + // float cosHorizontal = (float)Math.Cos(horizontalRad); + // float sinHorizontal = (float)Math.Sin(horizontalRad); + + // float x = this.distance * sinVertical * sinHorizontal; + // float y = this.distance * cosVertical; + // float z = this.distance * sinVertical * cosHorizontal; + + // Vector3Float v = new(x, y, z); + // return v; + // } + + public Vector3 ToVector3() { + float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad; + float horizontalRad = this.direction.horizontal * Angle.Deg2Rad; + float cosVertical = (float)Math.Cos(verticalRad); + float sinVertical = (float)Math.Sin(verticalRad); + float cosHorizontal = (float)Math.Cos(horizontalRad); + float sinHorizontal = (float)Math.Sin(horizontalRad); + + float x = this.distance * sinVertical * sinHorizontal; + float y = this.distance * cosVertical; + float z = this.distance * sinVertical * cosHorizontal; + + Vector3 v = new(x, y, z); + return v; + } + + public static Spherical operator +(Spherical s1, Spherical s2) { + // let's do it the easy way... + Vector3 v1 = s1.ToVector3(); + Vector3 v2 = s2.ToVector3(); + Vector3 v = v1 + v2; + Spherical r = FromVector3(v); + return r; + } + + } +} \ No newline at end of file diff --git a/src/SwingTwist.cs b/src/SwingTwist.cs new file mode 100644 index 0000000..22eb0bb --- /dev/null +++ b/src/SwingTwist.cs @@ -0,0 +1,41 @@ +using System.Numerics; +#if UNITY_5_3_OR_NEWER +using Quaternion = UnityEngine.Quaternion; +#endif + +namespace LinearAlgebra { + + public class SwingTwist { + public Direction swing; + public float twist; + + public static readonly SwingTwist zero = new SwingTwist(0, 0, 0); + + public SwingTwist(Direction swing, float twist) { + this.swing = swing; + this.twist = twist; + } + public SwingTwist(float horizontalSwing, float verticalSwing, float twist) { + this.swing = Direction.Degrees(horizontalSwing, verticalSwing); + this.swing.Normalize(); + this.twist = twist; + } + public static SwingTwist FromQuat32(Quat32 q32) { + // UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w); + // SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z); + q32.ToAngles(out float right, out float up, out float forward); + SwingTwist r = new SwingTwist(up, right, forward); + return r; + } + +#if UNITY_5_3_OR_NEWER + public Quaternion ToQuaternion() { + Quaternion q = Quaternion.Euler(-this.swing.vertical, + this.swing.horizontal, + this.twist); + return q; + } +#endif + } + +} \ No newline at end of file diff --git a/src/Vector2.cs b/src/Vector2.cs new file mode 100644 index 0000000..1840a7a --- /dev/null +++ b/src/Vector2.cs @@ -0,0 +1,363 @@ +using System; +using System.Numerics; + +namespace LinearAlgebra { + + public class Vector2Of where T : IComparable { + public T x; + public T y; + + public Vector2Of(T x, T y) { + this.x = x; + this.y = y; + } + } + + public class Vector2Int : Vector2Of { + public Vector2Int(int x, int y) : base(x, y) { } + + public static Vector2Int operator -(Vector2Int v1, Vector2Int v2) { + return new Vector2Int(v1.x - v2.x, v1.y - v2.y); + } + + public float magnitude { + get { + return (float)Math.Sqrt(this.x * this.x + this.y * this.y); + } + } + + public static float Distance(Vector2Int v1, Vector2Int v2) { + return (v1 - v2).magnitude; + } + } + + public class Vector2Float : Vector2Of { + public Vector2Float(float x, float y) : base(x, y) { } + + public static Vector2Float operator -(Vector2Float v1, Vector2Float v2) { + return new Vector2Float(v1.x - v2.x, v1.y - v2.y); + } + + public float magnitude { + get { + return (float)Math.Sqrt(this.x * this.x + this.y * this.y); + } + } + + public static float Distance(Vector2Float v1, Vector2Float v2) { + return (v1 - v2).magnitude; + } + } + + /// + /// 2-dimensional vectors + /// + public struct Vector2 : IEquatable { + + /// + /// The right axis of the vector + /// + public float x; // left/right + /// + /// The upward/forward axis of the vector + /// + public float y; // forward/backward + // directions are to be inline with Vector3 as much as possible... + + /// + /// Create a new 2-dimensional vector + /// + /// x axis value + /// y axis value + public Vector2(float x, float y) { + this.x = x; + this.y = y; + } + + /// + /// A vector with zero for all axis + /// + public static readonly Vector2 zero = new Vector2(0, 0); + /// + /// A vector with values (1, 1) + /// + public static readonly Vector2 one = new Vector2(1, 1); + /// + /// A vector with values (0, 1) + /// + public static readonly Vector2 up = new Vector2(0, 1); + /// + /// A vector with values (0, -1) + /// + public static readonly Vector2 down = new Vector2(0, -1); + /// + /// A vector with values (0, 1) + /// + public static readonly Vector2 forward = new Vector2(0, 1); + /// + /// A vector with values (0, -1) + /// + public static readonly Vector2 back = new Vector2(0, -1); + /// + /// A vector3 with values (-1, 0) + /// + public static readonly Vector2 left = new Vector2(-1, 0); + /// + /// A vector with values (1, 0) + /// + public static readonly Vector2 right = new Vector2(1, 0); + + /// + /// The squared length of this vector + /// + /// The squared length + /// 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 { + float d = x * x + y * y; + return d; + } + } + + /// + /// The length of this vector + /// + /// The length of this vector + public float magnitude { + get { + float d = (float)Math.Sqrt(x * x + y * y); + return d; + } + } + + /// + /// Convert the vector to a length of a 1 + /// + /// The vector with length 1 + public Vector2 normalized { + get { + float l = magnitude; + Vector2 v = zero; + if (l > Float.epsilon) + v = this / l; + return v; + } + } + + /// + /// Add two vectors + /// + /// The first vector + /// The second vector + /// The result of adding the two vectors + public static Vector2 operator +(Vector2 v1, Vector2 v2) { + Vector2 v = new Vector2(v1.x + v2.x, v1.y + v2.y); + return v; + } + + /// + /// Subtract two vectors + /// + /// The first vector + /// The second vector + /// The result of adding the two vectors + public static Vector2 operator -(Vector2 v1, Vector2 v2) { + Vector2 v = new Vector2(v1.x - v2.x, v1.y - v2.y); + return v; + } + + /// + /// Negate the vector + /// + /// The vector to negate + /// The negated vector + /// This will result in a vector pointing in the opposite direction + public static Vector2 operator -(Vector2 v1) { + Vector2 v = new Vector2(-v1.x, -v1.y); + return v; + } + + /// + /// Scale a vector uniformly up + /// + /// The vector to scale + /// 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) { + Vector2 v = new Vector2(v1.x * f, v1.y * f); + return v; + } + + /// + /// Scale a vector uniformly up + /// + /// The scaling factor + /// 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) { + Vector2 v = new Vector2(f * v1.x, f * v1.y); + return v; + } + + /// + /// Scale a vector uniformly down + /// + /// The vector to scale + /// 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) { + Vector2 v = new Vector2(v1.x / f, v1.y / f); + return v; + } + + /// + /// Tests if the vector has equal values as the given vector + /// + /// The vector to compare to + /// true if the vector values are equal + public bool Equals(Vector2 v1) => x == v1.x && y == v1.y; + + /// + /// Tests if the vector is equal to the given object + /// + /// 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) { + if (!(obj is Vector2 v)) + return false; + + return (x == v.x && y == v.y); + } + + /// + /// Tests if the two vectors have equal values + /// + /// The first vector + /// The second vector + /// truewhen the vectors have equal values + /// 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) { + return (v1.x == v2.x && v1.y == v2.y); + } + + /// + /// Tests if two vectors have different values + /// + /// The first vector + /// The second vector + /// truewhen the vectors have different values + /// 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) { + return (v1.x != v2.x || v1.y != v2.y); + } + + /// + /// Get an hash code for the vector + /// + /// The hash code + public override int GetHashCode() { + return (x, y).GetHashCode(); + } + + /// + /// Get the distance between two vectors + /// + /// The first vector + /// The second vector + /// The distance between the two vectors + 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); + return d; + } + + /// + /// The dot product of two vectors + /// + /// The first vector + /// The second vector + /// The dot product of the two vectors + public static float Dot(Vector2 v1, Vector2 v2) { + return v1.x * v2.x + v1.y * v2.y; + } + + /// + /// Lerp between two vectors + /// + /// The from vector + /// The to vector + /// The interpolation distance [0..1] + /// The lerped vector + /// 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) { + Vector2 v = v1 + (v2 - v1) * f; + return v; + } + + /// + /// Calculate the signed angle between two vectors. + /// + /// The starting vector + /// The ending vector + /// The axis to rotate around + /// The signed angle in degrees + 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; + + float sqrMagFrom = from.sqrMagnitude; + float sqrMagTo = to.sqrMagnitude; + + if (sqrMagFrom == 0 || sqrMagTo == 0) + return 0; + //if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo)) + // return nanf(""); + + float angleFrom = (float)Math.Atan2(from.y, from.x); + float angleTo = (float)Math.Atan2(to.y, to.x); + return (angleTo - angleFrom) * Angle.Rad2Deg; + } + + /// + /// Rotates the vector with the given angle + /// + /// The vector to rotate + /// The angle in degrees + /// + 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() { + x = (cos * tx) - (sin * ty), + y = (sin * tx) + (cos * ty) + }; + return v; + } + + /// + /// Map interval of angles between vectors [0..Pi] to interval [0..1] + /// + /// The first vector + /// 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) { + return (1 - Vector2.Dot(v1, v2)) / 2; + } + } +} \ No newline at end of file diff --git a/src/Vector3.cs b/src/Vector3.cs new file mode 100644 index 0000000..7994dcb --- /dev/null +++ b/src/Vector3.cs @@ -0,0 +1,179 @@ +#if !UNITY_5_3_OR_NEWER +using System; + +namespace LinearAlgebra { + public class Vector3Of { + public T x; + public T y; + public T z; + + public Vector3Of(T x, T y, T z) { + this.x = x; + this.y = y; + this.z = z; + } + + // public uint magnitude { + // get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); + // } + } + + public class Vector3Int : Vector3Of { + public Vector3Int(int x, int y, int z) : base(x, y, z) { } + } + public class Vector3Float : Vector3Of { + public Vector3Float(float x, float y, float z) : base(x, y, z) { } + + public float magnitude { + get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); + } + } + + /// + /// 3-dimensional vectors + /// + /// This uses the right-handed coordinate system. + public struct Vector3 : IEquatable { + + /// + /// The right axis of the vector + /// + public float x; //> left/right + /// + /// The upward axis of the vector + /// + public float y; //> up/down + /// + /// The forward axis of the vector + /// + public float z; //> forward/backward + + /// + /// Create a new 3-dimensional vector + /// + /// x axis value + /// y axis value + /// z axis value + public Vector3(float x, float y, float z) { + this.x = x; + this.y = y; + this.z = z; + } + + /// + /// A vector with zero for all axis + /// + public static readonly Vector3 zero = new Vector3(0, 0, 0); + /// + /// A vector with one for all axis + /// + public static readonly Vector3 one = new Vector3(1, 1, 1); + /// + /// A vector3 with values (-1, 0, 0) + /// + public static readonly Vector3 left = new Vector3(-1, 0, 0); + /// + /// A vector with values (1, 0, 0) + /// + public static readonly Vector3 right = new Vector3(1, 0, 0); + /// + /// A vector with values (0, -1, 0) + /// + public static readonly Vector3 down = new Vector3(0, -1, 0); + /// + /// A vector with values (0, 1, 0) + /// + public static readonly Vector3 up = new Vector3(0, 1, 0); + /// + /// A vector with values (0, 0, -1) + /// + public static readonly Vector3 back = new Vector3(0, -1, 0); + /// + /// A vector with values (0, 0, 1) + /// + public static readonly Vector3 forward = new Vector3(0, 1, 0); + + public readonly float magnitude { + get { + float d = (float)Math.Sqrt(x * x + y * y + z * z); + return d; + } + } + + public Vector3 normalized { + get { + float l = magnitude; + Vector3 v = zero; + if (l > Float.epsilon) + v = this / l; + return v; + } + } + + 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) { + 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 v = new Vector3(-v1.x, -v1.y, -v1.z); + return v; + } + + 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) { + Vector3 v = new Vector3(d * v1.x, d * v1.y, d * v1.z); + return v; + } + + 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) { + 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) { + return (v1.x == v2.x && v1.y == v2.y && v1.z == v2.z); + } + + 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() { + return (x, y, z).GetHashCode(); + } + + public static float Distance(Vector3 v1, Vector3 v2) { + return (v2 - v1).magnitude; + } + + 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) { + Vector3 v = v1 + (v2 - v1) * f; + return v; + } + + } +} +#endif \ No newline at end of file diff --git a/src/float16.cs b/src/float16.cs new file mode 100644 index 0000000..4b58cdd --- /dev/null +++ b/src/float16.cs @@ -0,0 +1,322 @@ +using System; + +namespace LinearAlgebra { + + public class float16 { + // + // FILE: float16.cpp + // AUTHOR: Rob Tillaart + // VERSION: 0.1.8 + // PURPOSE: library for Float16s for Arduino + // URL: http://en.wikipedia.org/wiki/Half-precision_floating-point_format + + ushort _value; + + public float16() { _value = 0; } + + public float16(float f) { + //_value = f32tof16(f); + _value = F32ToF16__(f); + } + + public float toFloat() { + return f16tof32(_value); + } + + public ushort GetBinary() { return _value; } + public void SetBinary(ushort value) { _value = value; } + + ////////////////////////////////////////////////////////// + // + // EQUALITIES + // + /* + bool float16::operator ==(const float16 &f) { return (_value == f._value); } + + bool float16::operator !=(const float16 &f) { return (_value != f._value); } + + bool float16::operator >(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value < f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value > f._value; + } + + bool float16::operator >=(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value <= f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value >= f._value; + } + + bool float16::operator <(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value > f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value < f._value; + } + + bool float16::operator <=(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value >= f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value <= f._value; + } + + ////////////////////////////////////////////////////////// + // + // NEGATION + // + float16 float16::operator -() { + float16 f16; + f16.setBinary(_value ^ 0x8000); + return f16; + } + + ////////////////////////////////////////////////////////// + // + // MATH + // + float16 float16::operator +(const float16 &f) { + return float16(this->toDouble() + f.toDouble()); + } + + float16 float16::operator -(const float16 &f) { + return float16(this->toDouble() - f.toDouble()); + } + + float16 float16::operator *(const float16 &f) { + return float16(this->toDouble() * f.toDouble()); + } + + float16 float16::operator /(const float16 &f) { + return float16(this->toDouble() / f.toDouble()); + } + + float16 & float16::operator+=(const float16 &f) { + *this = this->toDouble() + f.toDouble(); + return *this; + } + + float16 & float16::operator-=(const float16 &f) { + *this = this->toDouble() - f.toDouble(); + return *this; + } + + float16 & float16::operator*=(const float16 &f) { + *this = this->toDouble() * f.toDouble(); + return *this; + } + + float16 & float16::operator/=(const float16 &f) { + *this = this->toDouble() / f.toDouble(); + return *this; + } + + ////////////////////////////////////////////////////////// + // + // MATH HELPER FUNCTIONS + // + int float16::sign() { + if (_value & 0x8000) + return -1; + if (_value & 0xFFFF) + return 1; + return 0; + } + + bool float16::isZero() { return ((_value & 0x7FFF) == 0x0000); } + + bool float16::isNaN() { + if ((_value & 0x7C00) != 0x7C00) + return false; + if ((_value & 0x03FF) == 0x0000) + return false; + return true; + } + + bool float16::isInf() { return ((_value == 0x7C00) || (_value == 0xFC00)); } + */ + ////////////////////////////////////////////////////////// + // + // CORE CONVERSION + // + float f16tof32(ushort _value) { + //ushort sgn; + ushort man; + int exp; + float f; + + //Debug.Log($"{_value}"); + + bool sgn = (_value & 0x8000) > 0; + exp = (_value & 0x7C00) >> 10; + man = (ushort)(_value & 0x03FF); + + //Debug.Log($"{sgn} {exp} {man}"); + + // ZERO + if ((_value & 0x7FFF) == 0) { + return sgn ? -0 : 0; + } + // NAN & INF + if (exp == 0x001F) { + if (man == 0) + return sgn ? float.NegativeInfinity : float.PositiveInfinity; //-INFINITY : INFINITY; + else + return float.NaN; // NAN; + } + + // SUBNORMAL/NORMAL + if (exp == 0) + f = 0; + else + f = 1; + + // PROCESS MANTISSE + 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) { + f = f * (float)Math.Pow(2.0f, -13); // 5.96046447754e-8; + } + //Debug.Log($"{f}"); + return sgn ? -f : f; + } + + 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) { + 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) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // denormalized float32 does not fit in float16 + if (exp == 0x00) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // handle infinity & NAN + if (exp == 0x00FF) { + if (man != 0) + return 0xFE00; // NAN + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + + // normal numbers + exp = exp - 127 + 15; + // overflow does not fit => INF + if (exp > 30) { + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + // subnormal numbers + if (exp < -38) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ? + } + if (exp <= 0) // subnormal + { + man >>= (exp + 14); + // rounding + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | man); + return man; + } + + // normal + // TODO rounding + exp <<= 10; + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | exp | man); + return (ushort)(exp | man); + } + + //This function is faulty!!!! + ushort f32tof16(float f) { + //uint t = *(uint*)&f; + //uint t = (uint)BitConverter.SingleToInt32Bits(f); + uint t = SingleToInt32Bits(f); + // man bits = 10; but we keep 11 for rounding + ushort man = (ushort)((t & 0x007FFFFF) >> 12); + short exp = (short)((t & 0x7F800000) >> 23); + bool sgn = (t & 0x80000000) != 0; + + // handle 0 + if ((t & 0x7FFFFFFF) == 0) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // denormalized float32 does not fit in float16 + if (exp == 0x00) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // handle infinity & NAN + if (exp == 0x00FF) { + if (man != 0) + return 0xFE00; // NAN + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + + // normal numbers + exp = (short)(exp - 127 + 15); + // overflow does not fit => INF + if (exp > 30) { + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + // subnormal numbers + if (exp < -38) { + return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ? + } + if (exp <= 0) // subnormal + { + man >>= (exp + 14); + // rounding + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | man); + return man; + } + + // normal + // TODO rounding + exp <<= 10; + man++; + man >>= 1; + ushort uexp = (ushort)exp; + if (sgn) + return (ushort)(0x8000 | uexp | man); + return (ushort)(uexp | man); + } + + // -- END OF FILE -- + } + +} \ No newline at end of file diff --git a/test/AngleTest.cs b/test/AngleTest.cs new file mode 100644 index 0000000..c248465 --- /dev/null +++ b/test/AngleTest.cs @@ -0,0 +1,171 @@ +#if !UNITY_5_6_OR_NEWER +using NUnit.Framework; + +namespace LinearAlgebra.Test +{ + public class Tests + { + [SetUp] + public void Setup() + { + } + + [Test] + public void Normalize() + { + float r = 0; + + r = Angle.Normalize(90); + Assert.AreEqual(r, 90, "Normalize 90"); + + r = Angle.Normalize(-90); + Assert.AreEqual(r, -90, "Normalize -90"); + + r = Angle.Normalize(270); + Assert.AreEqual(r, -90, "Normalize 270"); + + r = Angle.Normalize(270 + 360); + Assert.AreEqual(r, -90, "Normalize 270+360"); + + r = Angle.Normalize(-270); + Assert.AreEqual(r, 90, "Normalize -270"); + + r = Angle.Normalize(-270 - 360); + Assert.AreEqual(r, 90, "Normalize -270-360"); + + r = Angle.Normalize(0); + Assert.AreEqual(r, 0, "Normalize 0"); + + r = Angle.Normalize(float.PositiveInfinity); + Assert.AreEqual(r, float.PositiveInfinity, "Normalize INFINITY"); + + r = Angle.Normalize(float.NegativeInfinity); + Assert.AreEqual(r, float.NegativeInfinity, "Normalize INFINITY"); + } + + [Test] + public void Clamp() + { + float r = 0; + + r = Angle.Clamp(1, 0, 2); + Assert.AreEqual(r, 1, "Clamp 1 0 2"); + + r = Angle.Clamp(-1, 0, 2); + Assert.AreEqual(r, 0, "Clamp -1 0 2"); + + r = Angle.Clamp(3, 0, 2); + Assert.AreEqual(r, 2, "Clamp 3 0 2"); + + r = Angle.Clamp(1, 0, 0); + Assert.AreEqual(r, 0, "Clamp 1 0 0"); + + r = Angle.Clamp(0, 0, 0); + Assert.AreEqual(r, 0, "Clamp 0 0 0"); + + r = Angle.Clamp(0, 1, -1); + Assert.AreEqual(r, 1, "Clamp 0 1 -1"); + + r = Angle.Clamp(1, 0, float.PositiveInfinity); + Assert.AreEqual(r, 1, "Clamp 1 0 INFINITY"); + + r = Angle.Clamp(1, float.NegativeInfinity, 1); + Assert.AreEqual(r, 1, "Clamp 1 -INFINITY 1"); + } + + [Test] + public void Difference() + { + float r = 0; + + r = Angle.Difference(0, 90); + Assert.AreEqual(r, 90, "Difference 0 90"); + + r = Angle.Difference(0, -90); + Assert.AreEqual(r, -90, "Difference 0 -90"); + + r = Angle.Difference(0, 270); + Assert.AreEqual(r, -90, "Difference 0 270"); + + r = Angle.Difference(0, -270); + Assert.AreEqual(r, 90, "Difference 0 -270"); + + r = Angle.Difference(90, 0); + Assert.AreEqual(r, -90, "Difference 90 0"); + + r = Angle.Difference(-90, 0); + Assert.AreEqual(r, 90, "Difference -90 0"); + + r = Angle.Difference(0, 0); + Assert.AreEqual(r, 0, "Difference 0 0"); + + r = Angle.Difference(90, 90); + Assert.AreEqual(r, 0, "Difference 90 90"); + + r = Angle.Difference(0, float.PositiveInfinity); + Assert.AreEqual(r, float.PositiveInfinity, "Difference 0 INFINITY"); + + r = Angle.Difference(0, float.NegativeInfinity); + Assert.AreEqual(r, float.NegativeInfinity, "Difference 0 -INFINITY"); + + r = Angle.Difference(float.NegativeInfinity, float.PositiveInfinity); + Assert.AreEqual(r, float.PositiveInfinity, "Difference -INFINITY INFINITY"); + } + + [Test] + public void MoveTowards() + { + float r = 0; + + r = Angle.MoveTowards(0, 90, 30); + Assert.AreEqual(r, 30, "MoveTowards 0 90 30"); + + r = Angle.MoveTowards(0, 90, 90); + Assert.AreEqual(r, 90, "MoveTowards 0 90 90"); + + r = Angle.MoveTowards(0, 90, 180); + Assert.AreEqual(r, 90, "MoveTowards 0 90 180"); + + r = Angle.MoveTowards(0, 90, 270); + Assert.AreEqual(r, 90, "MoveTowrads 0 90 270"); + + r = Angle.MoveTowards(0, 90, -30); + Assert.AreEqual(r, -30, "MoveTowards 0 90 -30"); + + r = Angle.MoveTowards(0, -90, -30); + Assert.AreEqual(r, 30, "MoveTowards 0 -90 -30"); + + r = Angle.MoveTowards(0, -90, -90); + Assert.AreEqual(r, 90, "MoveTowards 0 -90 -90"); + + r = Angle.MoveTowards(0, -90, -180); + Assert.AreEqual(r, 180, "MoveTowards 0 -90 -180"); + + r = Angle.MoveTowards(0, -90, -270); + Assert.AreEqual(r, 270, "MoveTowrads 0 -90 -270"); + + r = Angle.MoveTowards(0, 90, 0); + Assert.AreEqual(r, 0, "MoveTowards 0 90 0"); + + r = Angle.MoveTowards(0, 0, 0); + Assert.AreEqual(r, 0, "MoveTowards 0 0 0"); + + r = Angle.MoveTowards(0, 0, 30); + Assert.AreEqual(r, 0, "MoveTowrads 0 0 30"); + + r = Angle.MoveTowards(0, 90, float.PositiveInfinity); + Assert.AreEqual(r, 90, "MoveTowards 0 90 INFINITY"); + + r = Angle.MoveTowards(0, float.PositiveInfinity, 30); + Assert.AreEqual(r, 30, "MoveTowrads 0 INFINITY 30"); + + r = Angle.MoveTowards(0, -90, float.NegativeInfinity); + Assert.AreEqual(r, float.PositiveInfinity, "MoveTowards 0 -90 -INFINITY"); + + r = Angle.MoveTowards(0, float.NegativeInfinity, -30); + Assert.AreEqual(r, 30, "MoveTowrads 0 -INFINITY -30"); + + } + } +} +#endif \ No newline at end of file diff --git a/test/LinearAlgebra_Test.csproj b/test/LinearAlgebra_Test.csproj new file mode 100644 index 0000000..3ee2230 --- /dev/null +++ b/test/LinearAlgebra_Test.csproj @@ -0,0 +1,19 @@ + + + + net5.0 + false + true + + + + + + + + + + + + + diff --git a/test/SphericalTest.cs b/test/SphericalTest.cs new file mode 100644 index 0000000..3ede4f4 --- /dev/null +++ b/test/SphericalTest.cs @@ -0,0 +1,30 @@ +#if !UNITY_5_6_OR_NEWER +using NUnit.Framework; + +namespace LinearAlgebra.Test { + public class SphericalTest { + [SetUp] + public void Setup() { + } + + [Test] + public void FromVector3() { + Vector3 v = new(0, 0, 1); + Spherical s = Spherical.FromVector3(v); + Assert.AreEqual(1.0f, s.distance, "s.distance 0 0 1"); + Assert.AreEqual(0.0f, s.direction.horizontal, "s.hor 0 0 1"); + Assert.AreEqual(0.0f, s.direction.vertical, "s.vert 0 0 1"); + } + + [Test] + public void Addition() { + Spherical v1 = Spherical.Degrees(1, 45, 0); + Spherical v2 = Spherical.zero; + Spherical r = Spherical.zero; + + r = v1 + v2; + Assert.AreEqual(v1.distance, r.distance, "Addition(0,0,0)"); + } + } +} +#endif \ No newline at end of file From 9b2a04746d3b7254f33b451a9e35597db305bd65 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 26 May 2025 18:25:31 +0200 Subject: [PATCH 07/20] Updated Angle to match C++/Python --- src/Angle.cs | 177 ++++++++++++++++++++++++++----- src/Direction.cs | 6 +- src/Spherical.cs | 4 +- test/AngleTest.cs | 236 ++++++++++++++++++++++++------------------ test/DirectionTest.cs | 17 +++ test/SphericalTest.cs | 27 ++++- 6 files changed, 336 insertions(+), 131 deletions(-) create mode 100644 test/DirectionTest.cs diff --git a/src/Angle.cs b/src/Angle.cs index f056e5a..1490983 100644 --- a/src/Angle.cs +++ b/src/Angle.cs @@ -1,17 +1,128 @@ using System; +using System.Reflection.Emit; namespace LinearAlgebra { - /// - /// %Angle utilities - /// - 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; - + public class Angle { public const float Rad2Deg = 360.0f / ((float)Math.PI * 2); //0.0174532924F; - public const float Deg2Rad = ((float)Math.PI * 2) / 360.0f; //57.29578F; + public const float Deg2Rad = (float)Math.PI * 2 / 360.0f; //57.29578F; + + private Angle(float degrees) { + this.value = degrees; + } + private readonly float value = 0; + + public static readonly Angle zero = new(0); + + public static Angle Degrees(float degrees) { + // Reduce it to (-180..180] + if (float.IsFinite(degrees)) { + while (degrees < -180) + degrees += 360; + while (degrees >= 180) + degrees -= 360; + } + return new Angle(degrees); + } + + public static Angle Radians(float radians) { + // Reduce it to (-pi..pi] + if (float.IsFinite(radians)) { + while (radians <= -Math.PI) + radians += 2 * (float)Math.PI; + while (radians > Math.PI) + radians -= 2 * (float)Math.PI; + } + + return new Angle(radians * Rad2Deg); + + } + + public static Angle Revolutions(float revolutions) { + // reduce it to (-0.5 .. 0.5] + if (float.IsFinite(revolutions)) { + // Get the integer part + int integerPart = (int)revolutions; + + // Get the decimal part + revolutions -= integerPart; + if (revolutions < -0.5) + revolutions += 1; + if (revolutions >= 0.5) + revolutions -= 1; + } + return new Angle(revolutions * 360); + } + + public float inDegrees { + get { return this.value; } + } + + public float inRadians { + get { return this.value * Deg2Rad; } + } + + public float inRevolutions { + get { return this.value / 360.0f; } + } + + /// + /// Get the sign of the angle + /// + /// The angle + /// -1 when the angle is negative, 1 when it is positive and 0 in all other cases + public static int Sign(Angle a) { + if (a.value < 0) + return -1; + if (a.value > 0) + return 1; + return 0; + } + + /// + /// Returns the magnitude of the angle + /// + /// The angle + /// The positive magnitude of the angle + /// Negative values are negated to get a positive result + public static Angle Abs(Angle a) { + if (Sign(a) < 0) + return -a; + else + return a; + } + + /// + /// Negate the angle + /// + /// The angle + /// The negated angle + /// The negation of -180 is still -180 because the range is (-180..180] + public static Angle operator -(Angle a) { + Angle r = new(-a.value); + return r; + } + + /// + /// Subtract two angles + /// + /// Angle 1 + /// Angle 2 + /// The result of the subtraction + public static Angle operator -(Angle a1, Angle a2) { + Angle r = new(a1.value - a2.value); + return r; + } + /// + /// Add two angles + /// + /// Angle 1 + /// Angle 2 + /// The result of the addition + public static Angle operator +(Angle a1, Angle a2) { + Angle r = new(a1.value + a2.value); + return r; + } /// /// Clamp the angle between the given min and max values @@ -21,11 +132,38 @@ namespace LinearAlgebra { /// The maximum angle /// The clamped angle /// Angles are normalized - public static float Clamp(float angle, float min, float max) { - float normalizedAngle = Normalize(angle); - return Float.Clamp(normalizedAngle, min, max); + public static float Clamp(Angle angle, Angle min, Angle max) { + return Float.Clamp(angle.inDegrees, min.inDegrees, max.inDegrees); } + /// + /// Rotate from one angle to the other with a maximum degrees + /// + /// Starting angle + /// Target angle + /// Maximum angle to rotate + /// The resulting angle + /// This function is compatible with radian and degrees angles + public static Angle MoveTowards(Angle fromAngle, Angle toAngle, float maxDegrees) { + maxDegrees = Math.Max(0, maxDegrees); // filter out negative distances + Angle d = toAngle - fromAngle; + float dDegrees = Abs(d).inDegrees; + d = Degrees(Float.Clamp(dDegrees, 0, maxDegrees)); + if (Sign(d) < 0) + d = -d; + return fromAngle + d; + } + } + + /// + /// %Angle utilities + /// + public static class Angles { + 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; + + /// /// Determine the angle difference, result is a normalized angle /// @@ -53,21 +191,6 @@ namespace LinearAlgebra { return angle; } - /// - /// Rotate from one angle to the other with a maximum degrees - /// - /// Starting angle - /// Target angle - /// 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) { - float d = toAngle - fromAngle; - d = Normalize(d); - d = Math.Sign(d) * Float.Clamp(Math.Abs(d), 0, maxAngle); - return fromAngle + d; - } - /// /// Map interval of angles between vectors [0..Pi] to interval [0..1] /// diff --git a/src/Direction.cs b/src/Direction.cs index 6039bd5..297eff9 100644 --- a/src/Direction.cs +++ b/src/Direction.cs @@ -25,6 +25,10 @@ namespace LinearAlgebra horizontal = 0; vertical = 0; } + public Direction(Angle horizontal, Angle vertical) { + this.horizontal = horizontal.inDegrees; + + } // public Direction(float horizontal, float vertical) { // this.horizontal = horizontal; // this.vertical = vertical; @@ -64,7 +68,7 @@ namespace LinearAlgebra public Vector3Float ToVector3() { - float verticalRad = (Angle.pi / 2) - this.vertical * Angle.Deg2Rad; + float verticalRad = ((float)Math.PI / 2) - this.vertical * Angle.Deg2Rad; float horizontalRad = this.horizontal * Angle.Deg2Rad; float cosVertical = (float)Math.Cos(verticalRad); float sinVertical = (float)Math.Sin(verticalRad); diff --git a/src/Spherical.cs b/src/Spherical.cs index 456646f..183aa01 100644 --- a/src/Spherical.cs +++ b/src/Spherical.cs @@ -83,7 +83,7 @@ namespace LinearAlgebra { if (distance == 0.0f) return Spherical.zero; else { - float verticalAngle = (float)((Angle.pi / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg); + float verticalAngle = (float)(Math.PI / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg; float horizontalAngle = (float)Math.Atan2(v.x, v.z) * Angle.Rad2Deg; return Spherical.Degrees(distance, horizontalAngle, verticalAngle); } @@ -106,7 +106,7 @@ namespace LinearAlgebra { // } public Vector3 ToVector3() { - float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad; + float verticalRad = (float)(Math.PI / 2 - this.direction.vertical) * Angle.Deg2Rad; float horizontalRad = this.direction.horizontal * Angle.Deg2Rad; float cosVertical = (float)Math.Cos(verticalRad); float sinVertical = (float)Math.Sin(verticalRad); diff --git a/test/AngleTest.cs b/test/AngleTest.cs index c248465..00e4981 100644 --- a/test/AngleTest.cs +++ b/test/AngleTest.cs @@ -1,169 +1,207 @@ #if !UNITY_5_6_OR_NEWER +using System; +using System.Formats.Asn1; using NUnit.Framework; -namespace LinearAlgebra.Test -{ - public class Tests - { +namespace LinearAlgebra.Test { + public class AngleTests { [SetUp] - public void Setup() - { + public void Setup() { } [Test] - public void Normalize() - { - float r = 0; + public void Construct() { + // Degrees + float angle = 0.0f; + Angle a = Angle.Degrees(angle); + Assert.AreEqual(angle, a.inDegrees); - r = Angle.Normalize(90); - Assert.AreEqual(r, 90, "Normalize 90"); + angle = -180.0f; + a = Angle.Degrees(angle); + Assert.AreEqual(angle, a.inDegrees); - r = Angle.Normalize(-90); - Assert.AreEqual(r, -90, "Normalize -90"); + angle = 270.0f; + a = Angle.Degrees(angle); + Assert.AreEqual(-90, a.inDegrees); - r = Angle.Normalize(270); - Assert.AreEqual(r, -90, "Normalize 270"); + // Radians + angle = 0.0f; + a = Angle.Radians(angle); + Assert.AreEqual(angle, a.inRadians); - r = Angle.Normalize(270 + 360); - Assert.AreEqual(r, -90, "Normalize 270+360"); + angle = (float)-Math.PI; + a = Angle.Radians(angle); + Assert.AreEqual(angle, a.inRadians); - r = Angle.Normalize(-270); - Assert.AreEqual(r, 90, "Normalize -270"); + angle = (float)Math.PI * 1.5f; + a = Angle.Radians(angle); + Assert.AreEqual(-Math.PI * 0.5f, a.inRadians); - r = Angle.Normalize(-270 - 360); - Assert.AreEqual(r, 90, "Normalize -270-360"); + // Revolutions + angle = 0.0f; + a = Angle.Revolutions(angle); + Assert.AreEqual(angle, a.inRevolutions); - r = Angle.Normalize(0); - Assert.AreEqual(r, 0, "Normalize 0"); + angle = -0.5f; + a = Angle.Revolutions(angle); + Assert.AreEqual(angle, a.inRevolutions); - r = Angle.Normalize(float.PositiveInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "Normalize INFINITY"); + angle = 0.75f; + a = Angle.Revolutions(angle); + Assert.AreEqual(-0.25f, a.inRevolutions); - r = Angle.Normalize(float.NegativeInfinity); - Assert.AreEqual(r, float.NegativeInfinity, "Normalize INFINITY"); } + // [Test] + // public void Normalize() { + // float r = 0; + + // r = Angle.Normalize(90); + // Assert.AreEqual(r, 90, "Normalize 90"); + + // r = Angle.Normalize(-90); + // Assert.AreEqual(r, -90, "Normalize -90"); + + // r = Angle.Normalize(270); + // Assert.AreEqual(r, -90, "Normalize 270"); + + // r = Angle.Normalize(270 + 360); + // Assert.AreEqual(r, -90, "Normalize 270+360"); + + // r = Angle.Normalize(-270); + // Assert.AreEqual(r, 90, "Normalize -270"); + + // r = Angle.Normalize(-270 - 360); + // Assert.AreEqual(r, 90, "Normalize -270-360"); + + // r = Angle.Normalize(0); + // Assert.AreEqual(r, 0, "Normalize 0"); + + // r = Angle.Normalize(float.PositiveInfinity); + // Assert.AreEqual(r, float.PositiveInfinity, "Normalize INFINITY"); + + // r = Angle.Normalize(float.NegativeInfinity); + // Assert.AreEqual(r, float.NegativeInfinity, "Normalize INFINITY"); + // } + [Test] - public void Clamp() - { + public void Clamp() { float r = 0; - r = Angle.Clamp(1, 0, 2); + r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(2)); Assert.AreEqual(r, 1, "Clamp 1 0 2"); - r = Angle.Clamp(-1, 0, 2); + r = Angle.Clamp(Angle.Degrees(-1), Angle.Degrees(0), Angle.Degrees(2)); Assert.AreEqual(r, 0, "Clamp -1 0 2"); - r = Angle.Clamp(3, 0, 2); + r = Angle.Clamp(Angle.Degrees(3), Angle.Degrees(0), Angle.Degrees(2)); Assert.AreEqual(r, 2, "Clamp 3 0 2"); - r = Angle.Clamp(1, 0, 0); + r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(0)); Assert.AreEqual(r, 0, "Clamp 1 0 0"); - r = Angle.Clamp(0, 0, 0); + r = Angle.Clamp(Angle.Degrees(0), Angle.Degrees(0), Angle.Degrees(0)); Assert.AreEqual(r, 0, "Clamp 0 0 0"); - r = Angle.Clamp(0, 1, -1); + r = Angle.Clamp(Angle.Degrees(0), Angle.Degrees(1), Angle.Degrees(-1)); Assert.AreEqual(r, 1, "Clamp 0 1 -1"); - r = Angle.Clamp(1, 0, float.PositiveInfinity); + r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(float.PositiveInfinity)); Assert.AreEqual(r, 1, "Clamp 1 0 INFINITY"); - r = Angle.Clamp(1, float.NegativeInfinity, 1); + r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(float.NegativeInfinity), Angle.Degrees(1)); Assert.AreEqual(r, 1, "Clamp 1 -INFINITY 1"); } - [Test] - public void Difference() - { - float r = 0; + // [Test] + // public void Difference() { + // float r = 0; - r = Angle.Difference(0, 90); - Assert.AreEqual(r, 90, "Difference 0 90"); + // r = Angle.Difference(0, 90); + // Assert.AreEqual(r, 90, "Difference 0 90"); - r = Angle.Difference(0, -90); - Assert.AreEqual(r, -90, "Difference 0 -90"); + // r = Angle.Difference(0, -90); + // Assert.AreEqual(r, -90, "Difference 0 -90"); - r = Angle.Difference(0, 270); - Assert.AreEqual(r, -90, "Difference 0 270"); + // r = Angle.Difference(0, 270); + // Assert.AreEqual(r, -90, "Difference 0 270"); - r = Angle.Difference(0, -270); - Assert.AreEqual(r, 90, "Difference 0 -270"); + // r = Angle.Difference(0, -270); + // Assert.AreEqual(r, 90, "Difference 0 -270"); - r = Angle.Difference(90, 0); - Assert.AreEqual(r, -90, "Difference 90 0"); + // r = Angle.Difference(90, 0); + // Assert.AreEqual(r, -90, "Difference 90 0"); - r = Angle.Difference(-90, 0); - Assert.AreEqual(r, 90, "Difference -90 0"); + // r = Angle.Difference(-90, 0); + // Assert.AreEqual(r, 90, "Difference -90 0"); - r = Angle.Difference(0, 0); - Assert.AreEqual(r, 0, "Difference 0 0"); + // r = Angle.Difference(0, 0); + // Assert.AreEqual(r, 0, "Difference 0 0"); - r = Angle.Difference(90, 90); - Assert.AreEqual(r, 0, "Difference 90 90"); + // r = Angle.Difference(90, 90); + // Assert.AreEqual(r, 0, "Difference 90 90"); - r = Angle.Difference(0, float.PositiveInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "Difference 0 INFINITY"); + // r = Angle.Difference(0, float.PositiveInfinity); + // Assert.AreEqual(r, float.PositiveInfinity, "Difference 0 INFINITY"); - r = Angle.Difference(0, float.NegativeInfinity); - Assert.AreEqual(r, float.NegativeInfinity, "Difference 0 -INFINITY"); + // r = Angle.Difference(0, float.NegativeInfinity); + // Assert.AreEqual(r, float.NegativeInfinity, "Difference 0 -INFINITY"); - r = Angle.Difference(float.NegativeInfinity, float.PositiveInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "Difference -INFINITY INFINITY"); - } + // r = Angle.Difference(float.NegativeInfinity, float.PositiveInfinity); + // Assert.AreEqual(r, float.PositiveInfinity, "Difference -INFINITY INFINITY"); + // } [Test] - public void MoveTowards() - { - float r = 0; + public void MoveTowards() { + Angle r = Angle.zero; - r = Angle.MoveTowards(0, 90, 30); - Assert.AreEqual(r, 30, "MoveTowards 0 90 30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 30); + Assert.AreEqual(r.inDegrees, 30, "MoveTowards 0 90 30"); - r = Angle.MoveTowards(0, 90, 90); - Assert.AreEqual(r, 90, "MoveTowards 0 90 90"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 90); + Assert.AreEqual(r.inDegrees, 90, "MoveTowards 0 90 90"); - r = Angle.MoveTowards(0, 90, 180); - Assert.AreEqual(r, 90, "MoveTowards 0 90 180"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 180); + Assert.AreEqual(r.inDegrees, 90, "MoveTowards 0 90 180"); - r = Angle.MoveTowards(0, 90, 270); - Assert.AreEqual(r, 90, "MoveTowrads 0 90 270"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 270); + Assert.AreEqual(r.inDegrees, 90, "MoveTowrads 0 90 270"); - r = Angle.MoveTowards(0, 90, -30); - Assert.AreEqual(r, -30, "MoveTowards 0 90 -30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), -30); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 90 -30"); - r = Angle.MoveTowards(0, -90, -30); - Assert.AreEqual(r, 30, "MoveTowards 0 -90 -30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -30); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -30"); - r = Angle.MoveTowards(0, -90, -90); - Assert.AreEqual(r, 90, "MoveTowards 0 -90 -90"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -90); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -90"); - r = Angle.MoveTowards(0, -90, -180); - Assert.AreEqual(r, 180, "MoveTowards 0 -90 -180"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -180); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -180"); - r = Angle.MoveTowards(0, -90, -270); - Assert.AreEqual(r, 270, "MoveTowrads 0 -90 -270"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -270); + Assert.AreEqual(r.inDegrees, 0, "MoveTowrads 0 -90 -270"); - r = Angle.MoveTowards(0, 90, 0); - Assert.AreEqual(r, 0, "MoveTowards 0 90 0"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 0); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 90 0"); - r = Angle.MoveTowards(0, 0, 0); - Assert.AreEqual(r, 0, "MoveTowards 0 0 0"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(0), 0); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 0 0"); - r = Angle.MoveTowards(0, 0, 30); - Assert.AreEqual(r, 0, "MoveTowrads 0 0 30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(0), 30); + Assert.AreEqual(r.inDegrees, 0, "MoveTowrads 0 0 30"); - r = Angle.MoveTowards(0, 90, float.PositiveInfinity); - Assert.AreEqual(r, 90, "MoveTowards 0 90 INFINITY"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), float.PositiveInfinity); + Assert.AreEqual(r.inDegrees, 90, "MoveTowards 0 90 INFINITY"); - r = Angle.MoveTowards(0, float.PositiveInfinity, 30); - Assert.AreEqual(r, 30, "MoveTowrads 0 INFINITY 30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(float.PositiveInfinity), 30); + Assert.AreEqual(r.inDegrees, 30, "MoveTowrads 0 INFINITY 30"); - r = Angle.MoveTowards(0, -90, float.NegativeInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "MoveTowards 0 -90 -INFINITY"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), float.NegativeInfinity); + Assert.AreEqual(r.inDegrees, 0, "MoveTowards 0 -90 -INFINITY"); - r = Angle.MoveTowards(0, float.NegativeInfinity, -30); - Assert.AreEqual(r, 30, "MoveTowrads 0 -INFINITY -30"); + r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(float.NegativeInfinity), -30); + Assert.AreEqual(r.inDegrees, 0, "MoveTowrads 0 -INFINITY -30"); } } diff --git a/test/DirectionTest.cs b/test/DirectionTest.cs new file mode 100644 index 0000000..e31af4c --- /dev/null +++ b/test/DirectionTest.cs @@ -0,0 +1,17 @@ +using NUnit.Framework; + +namespace LinearAlgebra.Test { + public class DirectionTest { + [SetUp] + public void Setup() { + } + + [Test] + public void Compare() { + Direction d = Direction.Degrees(45, 135); + bool r; + r = d == new Direction(Angle.Degrees(45), Angle.Degrees(135)); + Assert.True(r); + } + }; +} \ No newline at end of file diff --git a/test/SphericalTest.cs b/test/SphericalTest.cs index 3ede4f4..8539dc0 100644 --- a/test/SphericalTest.cs +++ b/test/SphericalTest.cs @@ -1,4 +1,5 @@ #if !UNITY_5_6_OR_NEWER +using System; using NUnit.Framework; namespace LinearAlgebra.Test { @@ -13,7 +14,19 @@ namespace LinearAlgebra.Test { Spherical s = Spherical.FromVector3(v); Assert.AreEqual(1.0f, s.distance, "s.distance 0 0 1"); Assert.AreEqual(0.0f, s.direction.horizontal, "s.hor 0 0 1"); - Assert.AreEqual(0.0f, s.direction.vertical, "s.vert 0 0 1"); + Assert.AreEqual(0.0f, s.direction.vertical, 1.0E-05F, "s.vert 0 0 1"); + + v = new(0, 1, 0); + s = Spherical.FromVector3(v); + Assert.AreEqual(1.0f, s.distance, "s.distance 0 1 0"); + Assert.AreEqual(0.0f, s.direction.horizontal, "s.hor 0 1 0"); + Assert.AreEqual(90.0f, s.direction.vertical, "s.vert 0 1 0"); + + v = new(1, 0, 0); + s = Spherical.FromVector3(v); + Assert.AreEqual(1.0f, s.distance, "s.distance 1 0 0"); + Assert.AreEqual(90.0f, s.direction.horizontal, "s.hor 1 0 0"); + Assert.AreEqual(0.0f, s.direction.vertical, 1.0E-05F, "s.vert 1 0 0"); } [Test] @@ -23,7 +36,17 @@ namespace LinearAlgebra.Test { Spherical r = Spherical.zero; r = v1 + v2; - Assert.AreEqual(v1.distance, r.distance, "Addition(0,0,0)"); + Assert.AreEqual(v1.distance, r.distance, 1.0E-05F, "Addition(0,0,0)"); + + r = v1; + r += v2; + Assert.AreEqual(v1.distance, r.distance, 1.0E-05F, "Addition(0,0,0)"); + + v2 = Spherical.Degrees(1, 0, 90); + r = v1 + v2; + Assert.AreEqual(Math.Sqrt(2), r.distance, 1.0E-05F, "Addition(1 0 90)"); + Assert.AreEqual(45.0f, r.direction.horizontal, "Addition(1 0 90)"); + Assert.AreEqual(45.0f, r.direction.vertical, "Addition(1 0 90)"); } } } From bd53b71cd3e9e06b2c62e19173148273670d8d2d Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 27 May 2025 11:08:53 +0200 Subject: [PATCH 08/20] Added Unity Configurator --- Unity/Editor/Configurator.cs | 88 ++++++++++++++++++++++++++++++++++++ 1 file changed, 88 insertions(+) create mode 100644 Unity/Editor/Configurator.cs diff --git a/Unity/Editor/Configurator.cs b/Unity/Editor/Configurator.cs new file mode 100644 index 0000000..25942ee --- /dev/null +++ b/Unity/Editor/Configurator.cs @@ -0,0 +1,88 @@ +using System.Collections.Generic; +using UnityEngine; +using UnityEditor; +using UnityEditor.Callbacks; +using UnityEditor.PackageManager.Requests; +using UnityEditor.PackageManager; + +namespace RoboidControl.Unity +{ + [InitializeOnLoad] + public class ConfigurationCheck + { + static ConfigurationCheck() + { + RetrievePackageList(); + } + + protected static ListRequest request; + public static List packageNameList; + + public static void RetrievePackageList() + { + request = Client.List(); // List packages installed for the Project + EditorApplication.update += Progress; + } + + public static void Progress() + { + if (request.IsCompleted) + { + if (request.Status == StatusCode.Success) + { + packageNameList = new List(); + foreach (UnityEditor.PackageManager.PackageInfo package in request.Result) + packageNameList.Add(package.name); + + DidReloadScripts(); + } + else if (request.Status >= StatusCode.Failure) + Debug.Log(request.Error.message); + + EditorApplication.update -= Progress; + } + } + + //[DidReloadScripts] + protected static void DidReloadScripts() + { + if (packageNameList == null) + return; + CheckExtension( + packageNameList.Contains("com.unity.cloud.gltfast"), "GLTF"); + } + + protected static void CheckExtension(bool enabled, string define) + { + if (enabled) + GlobalDefine(define); + else + GlobalUndefine(define); + } + + public static void GlobalDefine(string name) + { + //Debug.Log("Define " + name); + string scriptDefines = PlayerSettings.GetScriptingDefineSymbolsForGroup(EditorUserBuildSettings.selectedBuildTargetGroup); + if (!scriptDefines.Contains(name)) + { + string newScriptDefines = scriptDefines + " " + name; + if (EditorUserBuildSettings.selectedBuildTargetGroup != 0) + PlayerSettings.SetScriptingDefineSymbolsForGroup(EditorUserBuildSettings.selectedBuildTargetGroup, newScriptDefines); + } + } + + public static void GlobalUndefine(string name) + { + //Debug.Log("Undefine " + name); + string scriptDefines = PlayerSettings.GetScriptingDefineSymbolsForGroup(EditorUserBuildSettings.selectedBuildTargetGroup); + if (scriptDefines.Contains(name)) + { + int playMakerIndex = scriptDefines.IndexOf(name); + string newScriptDefines = scriptDefines.Remove(playMakerIndex, name.Length); + PlayerSettings.SetScriptingDefineSymbolsForGroup(EditorUserBuildSettings.selectedBuildTargetGroup, newScriptDefines); + } + + } + } +} \ No newline at end of file From d9d64ebc8fd6d50a2f9c22973d187c23086aa501 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 28 May 2025 12:10:10 +0200 Subject: [PATCH 09/20] Simplified constructors --- Examples/BB2B/BB2B.cs | 2 +- Examples/BB2B/BB2B_Encoder.cs | 6 +- Examples/BB2B/Program.cs | 2 +- README.md | 18 +++++ src/Participant.cs | 12 ++++ src/Participants/SiteServer.cs | 6 +- src/Thing.cs | 69 ++++++++++++++----- .../{EncoderMotor.cs => ControlledMotor.cs} | 8 +-- src/Things/DifferentialDrive.cs | 13 ++-- src/Things/DigitalSensor.cs | 4 +- src/Things/DistanceSensor.cs | 8 ++- src/Things/Motor.cs | 4 +- src/Things/RelativeEncoder.cs | 7 ++ src/Things/TemperatureSensor.cs | 5 +- src/Things/TouchSensor.cs | 4 +- test/UnitTest1.cs | 2 +- 16 files changed, 126 insertions(+), 44 deletions(-) rename src/Things/{EncoderMotor.cs => ControlledMotor.cs} (91%) diff --git a/Examples/BB2B/BB2B.cs b/Examples/BB2B/BB2B.cs index 4f1c3cf..2a398ca 100644 --- a/Examples/BB2B/BB2B.cs +++ b/Examples/BB2B/BB2B.cs @@ -9,7 +9,7 @@ namespace RoboidControl { readonly TouchSensor touchRight; const float speed = 0.5f; - public BB2B(Participant owner) : base(owner) { + public BB2B(Thing parent = default) : base(parent) { this.name = "BB2B"; this.SetMotors(new Motor(this), new Motor(this)); this.SetDriveDimensions(0.064f, 0.128f); diff --git a/Examples/BB2B/BB2B_Encoder.cs b/Examples/BB2B/BB2B_Encoder.cs index f2b91df..21787c6 100644 --- a/Examples/BB2B/BB2B_Encoder.cs +++ b/Examples/BB2B/BB2B_Encoder.cs @@ -9,15 +9,15 @@ namespace RoboidControl { readonly TouchSensor touchRight; const float speed = 180.0f; // wheel rotation speed in degrees - public BB2B_Encoder(Participant owner) : base(owner) { + public BB2B_Encoder(Thing parent) : base(parent) { this.name = "BB2B"; this.SetDriveDimensions(0.064f, 0.128f); // Update the basic motors to motors with encoder - EncoderMotor leftMotor = new(this, new RelativeEncoder()) { + ControlledMotor leftMotor = new(this, new RelativeEncoder()) { position = new Spherical(0.064f, Direction.left) }; - EncoderMotor rightMotor = new(this, new RelativeEncoder()) { + ControlledMotor rightMotor = new(this, new RelativeEncoder()) { position = new Spherical(0.064f, Direction.right) }; this.SetMotors(leftMotor, rightMotor); diff --git a/Examples/BB2B/Program.cs b/Examples/BB2B/Program.cs index 10b0c67..7550097 100644 --- a/Examples/BB2B/Program.cs +++ b/Examples/BB2B/Program.cs @@ -3,7 +3,7 @@ using RoboidControl; class Program { static void Main() { - BB2B bb2b = new(ParticipantUDP.Isolated()); + BB2B bb2b = new(); while (true) { bb2b.Update(); diff --git a/README.md b/README.md index 7af367d..b576be8 100644 --- a/README.md +++ b/README.md @@ -11,3 +11,21 @@ The documentation for Roboid Control for C# is found at https://docs.roboidcontr - RoboidControl::Thing - RoboidControl::Participant + +# Get Started + +## Unity + +The Unity environment can use the same RoboidControl code as every other C# code, but needs a *starter* wrapper around it to make the things visibile. For example, to start the BB2B example in Unity one needs to write a BB2B_Starter.cs component as follows: +``` +using RoboidControl.Unity; + +public class BB2B_Starter : SiteServer { + void Start() { + new RoboidControl.BB2B(); + } +} +``` +This component then should be attached to a GameObject in the scene. + +It is possible to create a Site Server in Unity by just adding the `SiteServer` Component to a GameObject in the scene. When this is run, other roboids will be able to connect to this site then. \ No newline at end of file diff --git a/src/Participant.cs b/src/Participant.cs index 4318467..a936b87 100644 --- a/src/Participant.cs +++ b/src/Participant.cs @@ -3,6 +3,7 @@ using System.Collections.Generic; using System.Collections.Concurrent; using System.Net; using System.Net.Sockets; +using System.Reflection.Metadata; namespace RoboidControl { @@ -14,6 +15,13 @@ namespace RoboidControl { /// It also maintains the communcation information to contact the participant. /// It is used as a basis for the local participant, but also as a reference to remote participants. public class Participant { + + private Participant() { + this.root = Thing.CreateRoot(this); + this.root.name = "Root"; + this.Add(this.root); + } + /// /// Create a new participant with the given communcation info /// @@ -26,6 +34,10 @@ namespace RoboidControl { this.udpClient = localParticipant.udpClient; } + public static readonly Participant localParticipant = new(); + + public Thing root = null; + /// /// The Ip Address of a participant. When the participant is local, this contains 0.0.0.0 /// diff --git a/src/Participants/SiteServer.cs b/src/Participants/SiteServer.cs index 8c38572..519f425 100644 --- a/src/Participants/SiteServer.cs +++ b/src/Participants/SiteServer.cs @@ -132,9 +132,9 @@ namespace RoboidControl { protected virtual Thing ProcessNewThing(Participant sender, ThingMsg msg) { return msg.thingType switch { - Thing.Type.TouchSensor => new TouchSensor(sender, msg.thingId), - Thing.Type.DifferentialDrive => new DifferentialDrive(sender, msg.thingId), - _ => new Thing(sender, msg.thingType, msg.thingId), + //Thing.Type.TouchSensor => new TouchSensor(sender, msg.thingId), + //Thing.Type.DifferentialDrive => new DifferentialDrive(sender, msg.thingId), + _ => Thing.CreateRemote(sender, msg.thingType, msg.thingId) }; } diff --git a/src/Thing.cs b/src/Thing.cs index af90a11..b9f9b29 100644 --- a/src/Thing.cs +++ b/src/Thing.cs @@ -2,6 +2,7 @@ using System; using System.Collections.Generic; using System.Collections.Concurrent; using LinearAlgebra; +using Microsoft.VisualStudio.TestPlatform.ObjectModel.DataCollection; namespace RoboidControl { @@ -26,10 +27,11 @@ namespace RoboidControl { public const byte ControlledMotor = 0x06; public const byte UncontrolledMotor = 0x07; public const byte Servo = 0x08; - public const byte IncrementalEncoder = 0x19; + public const byte RelativeEncoder = 0x19; // Other + public const byte Root = 0x10; public const byte Roboid = 0x09; - public const byte HUmanoid = 0x0A; + public const byte Humanoid = 0x0A; public const byte ExternalSensor = 0x0B; public const byte Animator = 0x0C; public const byte DifferentialDrive = 0x0D; @@ -37,6 +39,29 @@ namespace RoboidControl { #region Init + private Thing(Participant owner) { + this.type = Type.Root; + + this.positionUpdated = true; + this.orientationUpdated = true; + this.hierarchyChanged = true; + + this.owner = owner; + this.parent = null; + } + + public static Thing CreateRoot(Participant owner) { + return new Thing(owner); + } + + static Thing localRoot { + get { + Participant participant = Participant.localParticipant; + return participant.root; + } + } + + /* /// /// Create a new thing without communication abilities /// @@ -66,6 +91,7 @@ namespace RoboidControl { this.owner.updateQueue.Enqueue(e); } } + */ /// /// Create a new child thing @@ -75,26 +101,33 @@ namespace RoboidControl { /// The ID of the thing, leave out or set to zero to generate an ID /// Invoke a OnNewThing event when the thing has been created /// The owner will be the same as the owner of the parent thing + /* public Thing(Thing parent, byte thingType = Type.Undetermined, byte thingId = 0, bool invokeEvent = true) : this(parent.owner, thingType, thingId, invokeEvent) { this.parent = parent; } + */ + public Thing(byte thingType = Type.Undetermined, Thing parent = default) { + this.type = thingType; - // /// - // /// Create a new thing for the given participant - // /// - // /// The participant owning the thing - // /// The network ID of the thing - // /// The ID of the thing - // /// The type of thing - // public Thing(Participant owner, byte networkId, byte thingId, byte thingType = 0) { - // this.owner = owner; - // this.id = thingId; - // this.type = thingType; - // this.networkId = networkId; - // // Console.Write($"New thing added to {owner}"); - // this.owner.Add(this); - // InvokeNewThing(this); - // } + this.positionUpdated = true; + this.orientationUpdated = true; + this.hierarchyChanged = true; + + if (parent == default) + this.parent = Participant.localParticipant.root; + else + this.parent = parent; + + this.owner = parent.owner; + this.owner.Add(this, true); + } + + public static Thing CreateRemote(Participant owner, byte thingType, byte thingId) { + Thing remoteThing = new(thingType, owner.root) { + id = thingId + }; + return remoteThing; + } /// /// Function which can be used to create components in external engines. diff --git a/src/Things/EncoderMotor.cs b/src/Things/ControlledMotor.cs similarity index 91% rename from src/Things/EncoderMotor.cs rename to src/Things/ControlledMotor.cs index 01d6ac6..fb5ad79 100644 --- a/src/Things/EncoderMotor.cs +++ b/src/Things/ControlledMotor.cs @@ -3,12 +3,12 @@ namespace RoboidControl { /// @brief A motor with speed control /// It uses a feedback loop from an encoder to regulate the speed /// The speed is measured in revolutions per second. - class EncoderMotor : Motor { - public EncoderMotor(Thing parent, RelativeEncoder encoder) : base(parent) { + public class ControlledMotor : Motor { + public ControlledMotor(Thing parent, RelativeEncoder encoder) : base(parent) { this.encoder = encoder; } // Upgrade an existing motor with an encoder - public EncoderMotor(Motor motor, RelativeEncoder encoder) : base(motor.parent) { + public ControlledMotor(Motor motor, RelativeEncoder encoder) : base(motor.parent) { this.motor = motor; this.encoder = encoder; } @@ -50,7 +50,7 @@ namespace RoboidControl { float pidP = 0.1F; float pidD = 0.0F; - float pidI = 0.0F; + //float pidI = 0.0F; public override void Update(ulong currentTimeMs, bool recurse = false) { float actualSpeed = this.encoder.angularSpeed; diff --git a/src/Things/DifferentialDrive.cs b/src/Things/DifferentialDrive.cs index 45203a8..271ea0d 100644 --- a/src/Things/DifferentialDrive.cs +++ b/src/Things/DifferentialDrive.cs @@ -6,11 +6,13 @@ namespace RoboidControl { /// /// @sa @link https://en.wikipedia.org/wiki/Differential_wheeled_robot @endlink public class DifferentialDrive : Thing { + /* /// /// Create a differential drive without communication abilities /// Invoke a OnNewThing event when the thing has been created - public DifferentialDrive(bool invokeEvent = true) : base(Type.DifferentialDrive, invokeEvent) { } + public DifferentialDrive() : base(Type.DifferentialDrive) { } + /// /// Create a differential drive for a participant /// @@ -28,15 +30,16 @@ namespace RoboidControl { // sendBinary = true; // owner.Send(new BinaryMsg(owner.networkId, this)); // this.updateQueue.Enqueue(new UpdateEvent(BinaryMsg.Id)); - } + */ + /// /// Create a new child differential drive /// /// The parent thing /// The ID of the thing, leave out or set to zero to generate an ID /// Invoke a OnNewThing event when the thing has been created - public DifferentialDrive(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.DifferentialDrive, thingId, invokeEvent) { } + public DifferentialDrive(Thing parent) : base(Type.DifferentialDrive, parent) { } /// @brief Configures the dimensions of the drive /// @param wheelDiameter The diameter of the wheels in meters @@ -101,9 +104,9 @@ namespace RoboidControl { /// Positive moves the robot in the forward direction. public void SetWheelAngularVelocity(float angularSpeedLeft, float angularSpeedRight) { // This only works when the motor is a motor with encoder - if (this.leftWheel is EncoderMotor leftMotor) + if (this.leftWheel is ControlledMotor leftMotor) leftMotor.targetAngularSpeed = angularSpeedLeft; - if (this.rightWheel is EncoderMotor rightMotor) + if (this.rightWheel is ControlledMotor rightMotor) rightMotor.targetAngularSpeed = angularSpeedRight; } diff --git a/src/Things/DigitalSensor.cs b/src/Things/DigitalSensor.cs index 2993580..3d1a19c 100644 --- a/src/Things/DigitalSensor.cs +++ b/src/Things/DigitalSensor.cs @@ -6,6 +6,7 @@ namespace RoboidControl { /// A sensor which can detect touches /// public class DigitalSensor : Thing { + /* /// /// Create a digital sensor without communication abilities /// @@ -18,13 +19,14 @@ namespace RoboidControl { /// The ID of the thing, leave out or set to zero to generate an ID /// Invoke a OnNewThing event when the thing has been created public DigitalSensor(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.Switch, thingId, invokeEvent) { } + */ /// /// Create a new child digital sensor /// /// The parent thing /// The ID of the thing, leave out or set to zero to generate an ID /// Invoke a OnNewThing event when the thing has been created - public DigitalSensor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.Switch, thingId, invokeEvent) { } + public DigitalSensor(Thing parent) : base(Type.Switch, parent) { } /// /// Value which is true when the sensor is touching something, false otherwise diff --git a/src/Things/DistanceSensor.cs b/src/Things/DistanceSensor.cs index ad5f785..def58f9 100644 --- a/src/Things/DistanceSensor.cs +++ b/src/Things/DistanceSensor.cs @@ -9,19 +9,23 @@ namespace RoboidControl { /// public float distance = 0; + /* /// /// Constructor for a new distance sensor /// /// The participant for which the sensor is needed public DistanceSensor(Participant participant) : base(participant, Type.Undetermined) { } + /// /// Create a distance sensor with the given ID /// /// The participant for with the sensor is needed /// The network ID of the sensor /// The ID of the thing - public DistanceSensor(Participant owner, byte thingId) : base(owner, Type.TemperatureSensor, thingId) { - } + public DistanceSensor(Participant owner, byte thingId) : base(owner, Type.TemperatureSensor, thingId) {} + */ + public DistanceSensor(Thing parent): base(Type.DistanceSensor, parent) {} + #if UNITY_5_3_OR_NEWER /// @copydoc Passer::RoboidControl::Thing::CreateComponent diff --git a/src/Things/Motor.cs b/src/Things/Motor.cs index 1b09654..e4ecad8 100644 --- a/src/Things/Motor.cs +++ b/src/Things/Motor.cs @@ -3,8 +3,8 @@ using LinearAlgebra; namespace RoboidControl { public class Motor : Thing { - public Motor(bool invokeEvent = true) : base(Type.UncontrolledMotor, invokeEvent) { } - public Motor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.UncontrolledMotor, thingId, invokeEvent) { } + //public Motor(bool invokeEvent = true) : base(Type.UncontrolledMotor, invokeEvent) { } + public Motor(Thing parent) : base(Type.UncontrolledMotor, parent) { } /// @brief Motor turning direction public enum Direction { diff --git a/src/Things/RelativeEncoder.cs b/src/Things/RelativeEncoder.cs index 5183f91..d3bc8b7 100644 --- a/src/Things/RelativeEncoder.cs +++ b/src/Things/RelativeEncoder.cs @@ -1,4 +1,6 @@ +using NUnit.Framework; + namespace RoboidControl { /// @brief An Incremental Encoder measures the rotations of an axle using a rotary @@ -9,7 +11,12 @@ namespace RoboidControl { /// full rotation /// @param distancePerRevolution The distance a wheel travels per full /// rotation + /* public RelativeEncoder(bool invokeEvent = true) : base(Type.IncrementalEncoder, invokeEvent) { } + */ + public RelativeEncoder(Thing parent = default) : base(Type.RelativeEncoder, parent) { + + } protected float _rotationSpeed = 0; /// @brief Get the rotation speed since the previous call diff --git a/src/Things/TemperatureSensor.cs b/src/Things/TemperatureSensor.cs index 6475d06..ecf548e 100644 --- a/src/Things/TemperatureSensor.cs +++ b/src/Things/TemperatureSensor.cs @@ -6,6 +6,7 @@ namespace RoboidControl { /// A temperature sensor /// public class TemperatureSensor : Thing { + /* /// /// Create a temperature sensor without communication abilities /// @@ -18,14 +19,14 @@ namespace RoboidControl { /// The ID of the thing /// Invoke a OnNewThing event when the thing has been created public TemperatureSensor(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.TemperatureSensor, thingId, invokeEvent) { } - + */ /// /// Create a new child temperature sensor /// /// The parent thing /// The ID of the thing, leave out or set to zero to generate an ID /// Invoke a OnNewThing event when the thing has been created - public TemperatureSensor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.TemperatureSensor, thingId, invokeEvent) { } + public TemperatureSensor(Thing parent) : base(Type.TemperatureSensor, parent) { } /// /// The measured temperature diff --git a/src/Things/TouchSensor.cs b/src/Things/TouchSensor.cs index 2198e6e..e2db699 100644 --- a/src/Things/TouchSensor.cs +++ b/src/Things/TouchSensor.cs @@ -6,6 +6,7 @@ namespace RoboidControl { /// A sensor which can detect touches /// public class TouchSensor : Thing { + /* /// /// Create a touch sensor without communication abilities /// @@ -18,13 +19,14 @@ namespace RoboidControl { /// The ID of the thing, leave out or set to zero to generate an ID /// Invoke a OnNewThing event when the thing has been created public TouchSensor(Participant owner, byte thingId = 0, bool invokeEvent = true) : base(owner, Type.TouchSensor, thingId, invokeEvent) { } + */ /// /// Create a new child touch sensor /// /// The parent thing /// The ID of the thing, leave out or set to zero to generate an ID /// Invoke a OnNewThing event when the thing has been created - public TouchSensor(Thing parent, byte thingId = 0, bool invokeEvent = true) : base(parent, Type.TouchSensor, thingId, invokeEvent) { + public TouchSensor(Thing parent) : base(Type.TouchSensor, parent) { this.name = "TouchSensor"; } diff --git a/test/UnitTest1.cs b/test/UnitTest1.cs index 46a3864..9fc8f79 100644 --- a/test/UnitTest1.cs +++ b/test/UnitTest1.cs @@ -65,7 +65,7 @@ namespace RoboidControl.test { public void Test_ThingMsg() { SiteServer siteServer = new(7681); ParticipantUDP participant = new("127.0.0.1", 7681, 7682); - Thing thing = new(participant) { + Thing thing = new() { name = "First Thing", modelUrl = "https://passer.life/extras/ant.jpg" }; From 795e4730d0c8cb9859f26152f896e3ef2c88b4f2 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 28 May 2025 12:16:53 +0200 Subject: [PATCH 10/20] Removed non-subtree(?) LinearAlgebra --- LinearAlgebra/.editorconfig | 19 - LinearAlgebra/.gitignore | 6 - LinearAlgebra/src/Angle.cs | 110 --- LinearAlgebra/src/Decomposition.cs | 287 -------- LinearAlgebra/src/Direction.cs | 83 --- LinearAlgebra/src/Float.cs | 41 -- LinearAlgebra/src/LinearAlgebra.csproj | 14 - LinearAlgebra/src/Matrix.cs | 689 ------------------- LinearAlgebra/src/Quat32.cs | 87 --- LinearAlgebra/src/Quaternion.cs | 81 --- LinearAlgebra/src/Spherical.cs | 134 ---- LinearAlgebra/src/SwingTwist.cs | 41 -- LinearAlgebra/src/Vector2.cs | 363 ---------- LinearAlgebra/src/Vector3.cs | 179 ----- LinearAlgebra/src/float16.cs | 322 --------- LinearAlgebra/test/AngleTest.cs | 171 ----- LinearAlgebra/test/LinearAlgebra_Test.csproj | 19 - LinearAlgebra/test/SphericalTest.cs | 30 - 18 files changed, 2676 deletions(-) delete mode 100644 LinearAlgebra/.editorconfig delete mode 100644 LinearAlgebra/.gitignore delete mode 100644 LinearAlgebra/src/Angle.cs delete mode 100644 LinearAlgebra/src/Decomposition.cs delete mode 100644 LinearAlgebra/src/Direction.cs delete mode 100644 LinearAlgebra/src/Float.cs delete mode 100644 LinearAlgebra/src/LinearAlgebra.csproj delete mode 100644 LinearAlgebra/src/Matrix.cs delete mode 100644 LinearAlgebra/src/Quat32.cs delete mode 100644 LinearAlgebra/src/Quaternion.cs delete mode 100644 LinearAlgebra/src/Spherical.cs delete mode 100644 LinearAlgebra/src/SwingTwist.cs delete mode 100644 LinearAlgebra/src/Vector2.cs delete mode 100644 LinearAlgebra/src/Vector3.cs delete mode 100644 LinearAlgebra/src/float16.cs delete mode 100644 LinearAlgebra/test/AngleTest.cs delete mode 100644 LinearAlgebra/test/LinearAlgebra_Test.csproj delete mode 100644 LinearAlgebra/test/SphericalTest.cs diff --git a/LinearAlgebra/.editorconfig b/LinearAlgebra/.editorconfig deleted file mode 100644 index 1ec7f97..0000000 --- a/LinearAlgebra/.editorconfig +++ /dev/null @@ -1,19 +0,0 @@ -# EditorConfig is awesome: https://EditorConfig.org - -# top-most EditorConfig file -root = true - -[*] -indent_style = space -indent_size = 4 -end_of_line = crlf -charset = utf-8 -trim_trailing_whitespace = false -insert_final_newline = false -max_line_length = 80 - -[*.cs] -csharp_new_line_before_open_brace = none -# Suppress warnings everywhere -dotnet_diagnostic.IDE1006.severity = none -dotnet_diagnostic.IDE0130.severity = none \ No newline at end of file diff --git a/LinearAlgebra/.gitignore b/LinearAlgebra/.gitignore deleted file mode 100644 index f0b2f47..0000000 --- a/LinearAlgebra/.gitignore +++ /dev/null @@ -1,6 +0,0 @@ -DoxyGen/DoxyWarnLogfile.txt -.vscode/settings.json -**bin -**obj -**.meta -*.sln diff --git a/LinearAlgebra/src/Angle.cs b/LinearAlgebra/src/Angle.cs deleted file mode 100644 index f056e5a..0000000 --- a/LinearAlgebra/src/Angle.cs +++ /dev/null @@ -1,110 +0,0 @@ -using System; - -namespace LinearAlgebra { - - /// - /// %Angle utilities - /// - 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; - - public const float Rad2Deg = 360.0f / ((float)Math.PI * 2); //0.0174532924F; - public const float Deg2Rad = ((float)Math.PI * 2) / 360.0f; //57.29578F; - - /// - /// Clamp the angle between the given min and max values - /// - /// The angle to clamp - /// The minimum angle - /// The maximum angle - /// The clamped angle - /// Angles are normalized - public static float Clamp(float angle, float min, float max) { - float normalizedAngle = Normalize(angle); - return Float.Clamp(normalizedAngle, min, max); - } - - /// - /// Determine the angle difference, result is a normalized angle - /// - /// First first angle - /// The second angle - /// the angle between the two angles - /// Angle values should be degrees - public static float Difference(float a, float b) { - float r = Normalize(b - a); - return r; - } - - /// - /// Normalize an angle to the range -180 < angle <= 180 - /// - /// The angle to normalize - /// The normalized angle in interval (-180..180] - /// Angle values should be in degrees - public static float Normalize(float angle) { - if (float.IsInfinity(angle)) - return angle; - - while (angle <= -180) angle += 360; - while (angle > 180) angle -= 360; - return angle; - } - - /// - /// Rotate from one angle to the other with a maximum degrees - /// - /// Starting angle - /// Target angle - /// 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) { - float d = toAngle - fromAngle; - d = Normalize(d); - d = Math.Sign(d) * Float.Clamp(Math.Abs(d), 0, maxAngle); - return fromAngle + d; - } - - /// - /// Map interval of angles between vectors [0..Pi] to interval [0..1] - /// - /// The first vector - /// The second vector - /// The resulting factor in interval [0..1] - /// 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) { - return (1 - Vector2.Dot(v1, v2)) / 2; - } - - // Normalize all vector angles to the range -180 < angle < 180 - //public static Vector3 Normalize(Vector3 angles) { - // float x = Normalize(angles.x); - // float y = Normalize(angles.y); - // float z = Normalize(angles.z); - // return new Vector3(x, y, z); - //} - - // Returns the signed angle in degrees between from and to. - //public static float SignedAngle(Vector3 from, Vector3 to) { - // float angle = Vector3.Angle(from, to); - // Vector3 cross = Vector3.Cross(from, to); - // if (cross.y < 0) angle = -angle; - // return angle; - //} - - // Returns the signed angle in degrees between from and to. - //public static float SignedAngle(Vector2 from, Vector2 to) { - // float sign = Math.Sign(from.y * to.x - from.x * to.y); - // return Vector2.Angle(from, to) * sign; - //} - - //public static Quaternion ToQuaternion(Rotation orientation) { - // return new Quaternion(orientation.x, orientation.y, orientation.z, orientation.w); - //} - } -} \ No newline at end of file diff --git a/LinearAlgebra/src/Decomposition.cs b/LinearAlgebra/src/Decomposition.cs deleted file mode 100644 index ddaf434..0000000 --- a/LinearAlgebra/src/Decomposition.cs +++ /dev/null @@ -1,287 +0,0 @@ -using System; -namespace LinearAlgebra { - class QR { - // QR Decomposition of a matrix A - public static (Matrix2 Q, Matrix2 R) Decomposition(Matrix2 A) { - int nRows = A.nRows; - int nCols = A.nCols; - - float[,] Q = new float[nRows, nCols]; - float[,] R = new float[nCols, nCols]; - - // Perform Gram-Schmidt orthogonalization - for (uint colIx = 0; colIx < nCols; colIx++) { - - // Step 1: v = column(ix) of A - float[] v = new float[nRows]; - for (int rowIx = 0; rowIx < nRows; rowIx++) - v[rowIx] = A.data[rowIx, colIx]; - - // Step 2: Subtract projections of v onto previous q's (orthogonalize) - for (uint colIx2 = 0; colIx2 < colIx; colIx2++) { - float dotProd = 0; - for (int i = 0; i < nRows; i++) - dotProd += Q[i, colIx2] * v[i]; - for (int i = 0; i < nRows; i++) - v[i] -= dotProd * Q[i, colIx2]; - } - - // Step 3: Normalize v to get column(ix) of Q - float norm = 0; - for (int rowIx = 0; rowIx < nRows; rowIx++) - norm += v[rowIx] * v[rowIx]; - norm = (float)Math.Sqrt(norm); - - for (int rowIx = 0; rowIx < nRows; rowIx++) - Q[rowIx, colIx] = v[rowIx] / norm; - - // Store the coefficients of R - for (int colIx2 = 0; colIx2 <= colIx; colIx2++) { - R[colIx2, colIx] = 0; - for (int k = 0; k < nRows; k++) - R[colIx2, colIx] += Q[k, colIx2] * A.data[k, colIx]; - } - } - return (new Matrix2(Q), new Matrix2(R)); - } - - // Reduced QR Decomposition of a matrix A - public static (Matrix2 Q, Matrix2 R) ReducedDecomposition(Matrix2 A) { - int nRows = A.nRows; - int nCols = A.nCols; - - float[,] Q = new float[nRows, nCols]; - float[,] R = new float[nCols, nCols]; - - // Perform Gram-Schmidt orthogonalization - for (int colIx = 0; colIx < nCols; colIx++) { - - // Step 1: v = column(colIx) of A - float[] columnIx = new float[nRows]; - bool isZeroColumn = true; - for (int rowIx = 0; rowIx < nRows; rowIx++) { - columnIx[rowIx] = A.data[rowIx, colIx]; - if (columnIx[rowIx] != 0) - isZeroColumn = false; - } - if (isZeroColumn) { - for (int rowIx = 0; rowIx < nRows; rowIx++) - Q[rowIx, colIx] = 0; - // Set corresponding R element to 0 - R[colIx, colIx] = 0; - - Console.WriteLine($"zero column {colIx}"); - - continue; - } - - // Step 2: Subtract projections of v onto previous q's (orthogonalize) - for (int colIx2 = 0; colIx2 < colIx; colIx2++) { - // Compute the dot product of v and column(colIx2) of Q - float dotProduct = 0; - for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++) - dotProduct += columnIx[rowIx2] * Q[rowIx2, colIx2]; - // Subtract the projection from v - for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++) - columnIx[rowIx2] -= dotProduct * Q[rowIx2, colIx2]; - } - - // Step 3: Normalize v to get column(colIx) of Q - float norm = 0; - for (int rowIx = 0; rowIx < nRows; rowIx++) - norm += columnIx[rowIx] * columnIx[rowIx]; - if (norm == 0) - throw new Exception("invalid value"); - - norm = (float)Math.Sqrt(norm); - - for (int rowIx = 0; rowIx < nRows; rowIx++) - Q[rowIx, colIx] = columnIx[rowIx] / norm; - - // Here is where it deviates from the Full QR Decomposition ! - - // Step 4: Compute the row(colIx) of R - for (int colIx2 = colIx; colIx2 < nCols; colIx2++) { - float dotProduct = 0; - for (int rowIx2 = 0; rowIx2 < nRows; rowIx2++) - dotProduct += Q[rowIx2, colIx] * A.data[rowIx2, colIx2]; - R[colIx, colIx2] = dotProduct; - } - } - if (!float.IsFinite(R[0, 0])) - throw new Exception("invalid value"); - - return (new Matrix2(Q), new Matrix2(R)); - } - } - - class SVD { - // According to ChatGPT, Mathnet uses Golub-Reinsch SVD algorithm - // 1. Bidiagonalization: The input matrix AA is reduced to a bidiagonal form using Golub-Kahan bidiagonalization. - // This process involves applying a sequence of Householder reflections to AA to create a bidiagonal matrix. - // This step reduces the complexity by making the matrix simpler while retaining the essential structure needed for SVD. - // - // 2. Diagonalization: Once the matrix is in bidiagonal form, - // the singular values are computed using an iterative process - // (typically involving QR factorization or Jacobi rotations) until convergence. - // This process diagonalizes the bidiagonal matrix and allows extraction of the singular values. - // - // 3. Computing UU and VTVT: After obtaining the singular values, - // the left singular vectors UU and right singular vectors VTVT are computed - // using the accumulated transformations (such as Householder reflections) from the bidiagonalization step. - - // Bidiagnolizations through Householder transformations - public static (Matrix2 U1, Matrix2 B, Matrix2 V1) Bidiagonalization(Matrix2 A) { - int m = A.nRows; // Rows of A - int n = A.nCols; // Columns of A - float[,] U1 = new float[m, m]; // Left orthogonal matrix - float[,] V1 = new float[n, n]; // Right orthogonal matrix - float[,] B = A.Clone().data; // Copy A to B for transformation - - // Initialize U1 and V1 as identity matrices - for (int i = 0; i < m; i++) - U1[i, i] = 1; - for (int i = 0; i < n; i++) - V1[i, i] = 1; - - // Perform Householder reflections to create a bidiagonal matrix B - for (int j = 0; j < n; j++) { - // Step 1: Construct the Householder vector y - float[] y = new float[m - j]; - for (int i = j; i < m; i++) - y[i - j] = B[i, j]; - - // Step 2: Compute the norm and scalar alpha - float norm = 0; - for (int i = 0; i < y.Length; i++) - norm += y[i] * y[i]; - norm = (float)Math.Sqrt(norm); - - if (B[j, j] > 0) - norm = -norm; - - float alpha = (float)Math.Sqrt(0.5 * (norm * (norm - B[j, j]))); - float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j]))); - - // Step 3: Apply the reflection to zero out below diagonal - for (int k = j; k < n; k++) { - float dot = 0; - for (int i = j; i < m; i++) - dot += y[i - j] * B[i, k]; - dot /= r; - - for (int i = j; i < m; i++) - B[i, k] -= 2 * dot * y[i - j]; - } - - // Step 4: Update U1 with the Householder reflection (U1 * Householder) - for (int i = j; i < m; i++) - U1[i, j] = y[i - j] / alpha; - - // Step 5: Update V1 (storing the Householder vector y) - // Correct indexing: we only need to store part of y in V1 from index j to n - for (int i = j; i < n; i++) - V1[j, i] = B[j, i]; - - // Repeat steps for further columns if necessary - } - return (new Matrix2(U1), new Matrix2(B), new Matrix2(V1)); - } - - public static Matrix2 Bidiagonalize(Matrix2 A) { - int m = A.nRows; // Rows of A - int n = A.nCols; // Columns of A - float[,] B = A.Clone().data; // Copy A to B for transformation - - // Perform Householder reflections to create a bidiagonal matrix B - for (int j = 0; j < n; j++) { - // Step 1: Construct the Householder vector y - float[] y = new float[m - j]; - for (int i = j; i < m; i++) - y[i - j] = B[i, j]; - - // Step 2: Compute the norm and scalar alpha - float norm = 0; - for (int i = 0; i < y.Length; i++) - norm += y[i] * y[i]; - norm = (float)Math.Sqrt(norm); - - if (B[j, j] > 0) - norm = -norm; - - float r = (float)Math.Sqrt(0.5 * (norm * (norm + B[j, j]))); - - // Step 3: Apply the reflection to zero out below diagonal - for (int k = j; k < n; k++) { - float dot = 0; - for (int i = j; i < m; i++) - dot += y[i - j] * B[i, k]; - dot /= r; - - for (int i = j; i < m; i++) - B[i, k] -= 2 * dot * y[i - j]; - } - - // Repeat steps for further columns if necessary - } - return new Matrix2(B); - } - - // QR Iteration for diagonalization of a bidiagonal matrix B - public static (Matrix1 singularValues, Matrix2 U, Matrix2 Vt) QRIteration(Matrix2 B) { - int m = B.nRows; - int n = B.nCols; - - Matrix2 U = new(m, m); // Left singular vectors (U) - Matrix2 Vt = new(n, n); // Right singular vectors (V^T) - float[] singularValues = new float[Math.Min(m, n)]; // Singular values - - // Initialize U and Vt as identity matrices - for (int i = 0; i < m; i++) - U.data[i, i] = 1; - for (int i = 0; i < n; i++) - Vt.data[i, i] = 1; - - // Perform QR iterations - float tolerance = 1e-7f; //1e-12f; for double - bool converged = false; - while (!converged) { - // Perform QR decomposition on the matrix B - (Matrix2 Q, Matrix2 R) = QR.Decomposition(B); - - // Update B to be the product Q * R //R * Q - B = R * Q; - - // Accumulate the transformations in U and Vt - U *= Q; - Vt *= R; - - // Check convergence by looking at the off-diagonal elements of B - converged = true; - for (int i = 0; i < m - 1; i++) { - for (int j = i + 1; j < n; j++) { - if (Math.Abs(B.data[i, j]) > tolerance) { - converged = false; - break; - } - } - } - } - - // Extract singular values (diagonal elements of B) - for (int i = 0; i < Math.Min(m, n); i++) - singularValues[i] = B.data[i, i]; - - return (new Matrix1(singularValues), U, Vt); - } - - public static (Matrix2 U, Matrix1 S, Matrix2 Vt) Decomposition(Matrix2 A) { - if (A.nRows != A.nCols) - throw new ArgumentException("SVD: matrix A has to be square."); - - Matrix2 B = Bidiagonalize(A); - (Matrix1 S, Matrix2 U, Matrix2 Vt) = QRIteration(B); - return (U, S, Vt); - } - } -} \ No newline at end of file diff --git a/LinearAlgebra/src/Direction.cs b/LinearAlgebra/src/Direction.cs deleted file mode 100644 index 6039bd5..0000000 --- a/LinearAlgebra/src/Direction.cs +++ /dev/null @@ -1,83 +0,0 @@ -using System; -#if UNITY_5_3_OR_NEWER -using Vector3Float = UnityEngine.Vector3; -#endif - -namespace LinearAlgebra -{ - - /// - /// A direction in 3D space - /// - /// A direction is represented using two angles: - /// * The horizontal angle ranging from -180 (inclusive) to 180 (exclusive) - /// degrees which is a rotation in the horizontal plane - /// * A vertical angle ranging from -90 (inclusive) to 90 (exclusive) degrees - /// which is the rotation in the up/down direction applied after the horizontal - /// rotation has been applied. - /// The angles are automatically normalized to stay within the abovenmentioned - /// ranges. - public class Direction { - public float horizontal; - public float vertical; - - public Direction() { - horizontal = 0; - vertical = 0; - } - // public Direction(float horizontal, float vertical) { - // this.horizontal = horizontal; - // this.vertical = vertical; - // //Normalize(); - // } - - public static Direction Degrees(float horizontal, float vertical) { - Direction d = new() { - horizontal = horizontal, - vertical = vertical - }; - //Normalize(); - return d; - } - public static Direction Radians(float horizontal, float vertical) { - Direction d = new() { - horizontal = horizontal * Angle.Rad2Deg, - vertical = vertical * Angle.Rad2Deg - }; - //Normalize(); - return d; - } - - public readonly static Direction forward = Degrees(0, 0); - public readonly static Direction backward = Degrees(-180, 0); - public readonly static Direction up = Degrees(0, 90); - public readonly static Direction down = Degrees(0, -90); - public readonly static Direction left = Degrees(-90, 0); - public readonly static Direction right = Degrees(90, 0); - - public void Normalize() { - if (this.vertical > 90 || this.vertical < -90) { - this.horizontal += 180; - this.vertical = 180 - this.vertical; - } - } - - public Vector3Float ToVector3() - { - float verticalRad = (Angle.pi / 2) - this.vertical * Angle.Deg2Rad; - float horizontalRad = this.horizontal * Angle.Deg2Rad; - float cosVertical = (float)Math.Cos(verticalRad); - float sinVertical = (float)Math.Sin(verticalRad); - float cosHorizontal = (float)Math.Cos(horizontalRad); - float sinHorizontal = (float)Math.Sin(horizontalRad); - - float x = sinVertical * sinHorizontal; - float y = cosVertical; - float z = sinVertical * cosHorizontal; - - Vector3Float v = new(x, y, z); - return v; - } - } - -} \ No newline at end of file diff --git a/LinearAlgebra/src/Float.cs b/LinearAlgebra/src/Float.cs deleted file mode 100644 index 2217b84..0000000 --- a/LinearAlgebra/src/Float.cs +++ /dev/null @@ -1,41 +0,0 @@ -namespace LinearAlgebra { - - /// - /// Float number utilities - /// - public class Float { - /// - /// The precision of float numbers - /// - public const float epsilon = 1E-05f; - /// - /// The square of the float number precision - /// - public const float sqrEpsilon = 1e-10f; - - /// - /// Clamp the value between the given minimum and maximum values - /// - /// The value to clamp - /// The minimum value - /// The maximum value - /// The clamped value - public static float Clamp(float f, float min, float max) { - if (f < min) - return min; - if (f > max) - return max; - return f; - } - - /// - /// Clamp the value between to the interval [0..1] - /// - /// The value to clamp - /// The clamped value - public static float Clamp01(float f) { - return Clamp(f, 0, 1); - } - } - -} \ No newline at end of file diff --git a/LinearAlgebra/src/LinearAlgebra.csproj b/LinearAlgebra/src/LinearAlgebra.csproj deleted file mode 100644 index 14d3947..0000000 --- a/LinearAlgebra/src/LinearAlgebra.csproj +++ /dev/null @@ -1,14 +0,0 @@ - - - - false - false - net5.0 - - - - - - - - diff --git a/LinearAlgebra/src/Matrix.cs b/LinearAlgebra/src/Matrix.cs deleted file mode 100644 index 5196d48..0000000 --- a/LinearAlgebra/src/Matrix.cs +++ /dev/null @@ -1,689 +0,0 @@ -using System; -#if UNITY_5_3_OR_NEWER -using Vector3Float = UnityEngine.Vector3; -using Vector2Float = UnityEngine.Vector2; -using Quaternion = UnityEngine.Quaternion; -#endif - -namespace LinearAlgebra { - - public readonly struct Slice - { - public int start { get; } - public int stop { get; } - public Slice(int start, int stop) - { - this.start = start; - this.stop = stop; - } - } - - public class Matrix2 { - public float[,] data { get; } - - public int nRows => data.GetLength(0); - public int nCols => data.GetLength(1); - - public Matrix2(int nRows, int 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]; - } - return new Matrix2(data); - } - - public static Matrix2 Zero(int nRows, int 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(int size) - { - return Diagonal(1, size); - } - public static Matrix2 Identity(int nRows, int 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, int 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) - { - int n = (int)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) - { - int 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); - } - - public Matrix1 GetRow(int rowIx) { - float[] row = new float[this.nCols]; - for (int colIx = 0; colIx < this.nCols; colIx++) { - row[colIx] = this.data[rowIx, colIx]; - } - return new Matrix1(row); - } - -#if UNITY_5_3_OR_NEWER - public Vector3Float GetRow3(int rowIx) { - int cols = this.nCols; - Vector3Float row = new() { - x = this.data[rowIx, 0], - y = this.data[rowIx, 1], - z = this.data[rowIx, 2] - }; - 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 void SwapRows(int row1, int row2) { - for (uint ix = 0; ix < this.nCols; ix++) { - float temp = this.data[row1, ix]; - this.data[row1, ix] = this.data[row2, ix]; - this.data[row2, ix] = temp; - } - } - - 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 void SetColumn(int colIx, Matrix1 v) { - for (uint ix = 0; ix < v.size; ix++) - this.data[ix, colIx] = v.data[ix]; - } - public void SetColumn(int colIx, Vector3Float v) { - this.data[0, colIx] = v.x; - this.data[1, colIx] = v.y; - this.data[2, colIx] = v.z; - } - - 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 GetRows(Slice slice) { - return GetRows(slice.start, slice.stop); - } - public Matrix2 GetRows(int from, int 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 (int colIx = 0; colIx < this.nCols; colIx++) - result[resultRowIx, colIx] = this.data[rowIx, colIx]; - - resultRowIx++; - } - - return new Matrix2(result); - } - - public Matrix2 Slice(Slice slice) - { - return Slice(slice.start, slice.stop); - } - public Matrix2 Slice(int from, int 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 (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((int start, int stop) rowRange, (int start, int stop) colRange) - { - float[,] result = new float[rowRange.stop - rowRange.start, colRange.stop - colRange.start]; - - int resultRowIx = 0; - int resultColIx = 0; - for (int i = rowRange.start; i < rowRange.stop; i++) - { - for (int 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) { - UpdateSlice((slice.start, slice.stop), m); - } - public void UpdateSlice((int start, int stop) slice, Matrix2 m) { - // if (slice.start == slice.stop) - // Console.WriteLine("WARNING: no data is updates when start equals stop in a slice!"); - int mRowIx = 0; - for (int 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((int start, int stop) rowRange, (int start, int stop) colRange, Matrix2 m) - { - for (int i = rowRange.start; i < rowRange.stop; i++) - { - for (int 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 - int 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 - } - } - - // 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]; - } - } - - // Back substitution - for (int i = n - 1; i >= 0; i--) - { - // Eliminate the column above the pivot - for (int 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]; - } - } - - // 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); - } - - public float Determinant() - { - int 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) - { - int 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 static Matrix2 DeleteRows(Matrix2 A, Slice rowRange) { - float[,] result = new float[A.nRows - (rowRange.stop - rowRange.start), A.nCols]; - - int resultRowIx = 0; - for (int i = 0; i < A.nRows; i++) { - if (i >= rowRange.start && i < rowRange.stop) - continue; - - for (int j = 0; j < A.nCols; j++) - result[resultRowIx, j] = A.data[i, j]; - - resultRowIx++; - } - return new Matrix2(result); - } - - internal static Matrix2 DeleteColumns(Matrix2 A, Slice colRange) { - float[,] result = new float[A.nRows, A.nCols - (colRange.stop - colRange.start)]; - - for (int i = 0; i < A.nRows; i++) { - int resultColIx = 0; - for (int j = 0; j < A.nCols; j++) { - if (j >= colRange.start && j < colRange.stop) - continue; - - result[i, resultColIx++] = A.data[i, j]; - } - } - return new Matrix2(result); - } - } - - public class Matrix1 - { - public float[] data { get; } - - public int size => data.GetLength(0); - - public Matrix1(int size) - { - this.data = new float[size]; - } - - public Matrix1(float[] data) { - this.data = data; - } - - public static Matrix1 Zero(int 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); - } - -#if UNITY_5_3_OR_NEWER - 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); - } -#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 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 { - get { - 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]); - } - } -#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 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, 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 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 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) { - float[] result = new float[A.size]; - - for (int i = 0; i < A.size; i++) - result[i] = f / A.data[i]; - - return new Matrix1(result); - } - - public Matrix1 Slice(Slice range) - { - return Slice(range.start, range.stop); - } - public Matrix1 Slice(int from, int 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 (int 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++) - this.data[ix] = v.data[vIx]; - } - } - -} \ No newline at end of file diff --git a/LinearAlgebra/src/Quat32.cs b/LinearAlgebra/src/Quat32.cs deleted file mode 100644 index f13266d..0000000 --- a/LinearAlgebra/src/Quat32.cs +++ /dev/null @@ -1,87 +0,0 @@ -using System; - -namespace LinearAlgebra { - public class Quat32 { - public float x; - public float y; - public float z; - public float w; - - public Quat32() { - this.x = 0; - this.y = 0; - this.z = 0; - this.w = 1; - } - - public Quat32(float x, float y, float z, float w) { - this.x = x; - this.y = y; - this.z = z; - this.w = w; - } - - public static Quat32 FromSwingTwist(SwingTwist s) { - Quat32 q32 = Quat32.Euler(-s.swing.vertical, s.swing.horizontal, s.twist); - return q32; - } - - public static Quat32 Euler(float yaw, float pitch, float roll) { - float rollOver2 = roll * Angle.Deg2Rad * 0.5f; - float sinRollOver2 = (float)Math.Sin((float)rollOver2); - float cosRollOver2 = (float)Math.Cos((float)rollOver2); - float pitchOver2 = pitch * 0.5f; - float sinPitchOver2 = (float)Math.Sin((float)pitchOver2); - float cosPitchOver2 = (float)Math.Cos((float)pitchOver2); - float yawOver2 = yaw * 0.5f; - float sinYawOver2 = (float)Math.Sin((float)yawOver2); - float cosYawOver2 = (float)Math.Cos((float)yawOver2); - Quat32 result = new Quat32() { - w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + - sinYawOver2 * sinPitchOver2 * sinRollOver2, - x = sinYawOver2 * cosPitchOver2 * cosRollOver2 + - cosYawOver2 * sinPitchOver2 * sinRollOver2, - y = cosYawOver2 * sinPitchOver2 * cosRollOver2 - - sinYawOver2 * cosPitchOver2 * sinRollOver2, - z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - - sinYawOver2 * sinPitchOver2 * cosRollOver2 - }; - return result; - } - - public void ToAngles(out float right, out float up, out float forward) { - float test = this.x * this.y + this.z * this.w; - if (test > 0.499f) { // singularity at north pole - right = 0; - up = 2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg; - forward = 90; - return; - //return Vector3(0, 2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, 90); - } - else if (test < -0.499f) { // singularity at south pole - right = 0; - up = -2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg; - forward = -90; - return; - //return Vector3(0, -2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, -90); - } - else { - float sqx = this.x * this.x; - float sqy = this.y * this.y; - float sqz = this.z * this.z; - - right = (float)Math.Atan2(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * Angle.Rad2Deg; - up = (float)Math.Atan2(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * Angle.Rad2Deg; - forward = (float)Math.Asin(2 * test) * Angle.Rad2Deg; - return; - // return Vector3( - // atan2f(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * - // Rad2Deg, - // atan2f(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * - // Rad2Deg, - // asinf(2 * test) * Angle.Rad2Deg); - } - } - - } -} \ No newline at end of file diff --git a/LinearAlgebra/src/Quaternion.cs b/LinearAlgebra/src/Quaternion.cs deleted file mode 100644 index 52dd26b..0000000 --- a/LinearAlgebra/src/Quaternion.cs +++ /dev/null @@ -1,81 +0,0 @@ -using System; -#if UNITY_5_3_OR_NEWER -using Quaternion = UnityEngine.Quaternion; -#endif - -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) { - this.x = x; - this.y = y; - this.z = z; - this.w = w; - } - -#if UNITY_5_3_OR_NEWER - public static Quaternion Reflect(Quaternion q) { - return new(-q.x, -q.y, -q.z, q.w); - } - - public static Matrix2 ToRotationMatrix(Quaternion q) { - float w = q.x, x = q.y, y = q.z, z = q.w; - - 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); - } - - 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); - } -#endif - } - - // 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 diff --git a/LinearAlgebra/src/Spherical.cs b/LinearAlgebra/src/Spherical.cs deleted file mode 100644 index 456646f..0000000 --- a/LinearAlgebra/src/Spherical.cs +++ /dev/null @@ -1,134 +0,0 @@ -using System; -#if UNITY_5_3_OR_NEWER -//using Vector3Float = UnityEngine.Vector3; -using Vector3 = UnityEngine.Vector3; -#endif - -namespace LinearAlgebra { - /// - /// A spherical vector - /// - public class Spherical { - - /// - /// Create a default vector with zero distance - /// - public Spherical() { - this.distance = 0; - this.direction = new Direction(); - } - - /// - /// Create a spherical vector - /// - /// The distance in meters - /// The direction of the vector - public Spherical(float distance, Direction direction) { - this.distance = distance; - this.direction = direction; - } - - /// - /// Create spherical vector. All given angles are in degrees - /// - /// The distance in meters - /// The horizontal angle in degrees - /// The vertical angle in degrees - /// - public static Spherical Degrees(float distance, float horizontal, float vertical) { - Direction direction = Direction.Degrees(horizontal, vertical); - Spherical s = new(distance, direction); - return s; - } - - public static Spherical Radians(float distance, float horizontal, float vertical) { - Direction direction = Direction.Radians(horizontal, vertical); - Spherical s = new(distance, direction); - return s; - } - - /// - /// The distance in meters - /// - /// @remark The distance should never be negative - public float distance; - /// - /// The direction of the vector - /// - public Direction direction; - - /// - /// A spherical vector with zero degree angles and distance - /// - public readonly static Spherical zero = new(0, Direction.forward); - /// - /// A normalized forward-oriented vector - /// - public readonly static Spherical forward = new(1, Direction.forward); - - - // public static Spherical FromVector3Float(Vector3Float v) { - // float distance = v.magnitude; - // if (distance == 0.0f) - // return Spherical.zero; - // 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; - // return Spherical.Degrees(distance, horizontalAngle, verticalAngle); - // } - // } - - public static Spherical FromVector3(Vector3 v) { - float distance = v.magnitude; - if (distance == 0.0f) - return Spherical.zero; - 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; - return Spherical.Degrees(distance, horizontalAngle, verticalAngle); - } - } - - // public Vector3Float ToVector3Float() { - // float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad; - // float horizontalRad = this.direction.horizontal * Angle.Deg2Rad; - // float cosVertical = (float)Math.Cos(verticalRad); - // float sinVertical = (float)Math.Sin(verticalRad); - // float cosHorizontal = (float)Math.Cos(horizontalRad); - // float sinHorizontal = (float)Math.Sin(horizontalRad); - - // float x = this.distance * sinVertical * sinHorizontal; - // float y = this.distance * cosVertical; - // float z = this.distance * sinVertical * cosHorizontal; - - // Vector3Float v = new(x, y, z); - // return v; - // } - - public Vector3 ToVector3() { - float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad; - float horizontalRad = this.direction.horizontal * Angle.Deg2Rad; - float cosVertical = (float)Math.Cos(verticalRad); - float sinVertical = (float)Math.Sin(verticalRad); - float cosHorizontal = (float)Math.Cos(horizontalRad); - float sinHorizontal = (float)Math.Sin(horizontalRad); - - float x = this.distance * sinVertical * sinHorizontal; - float y = this.distance * cosVertical; - float z = this.distance * sinVertical * cosHorizontal; - - Vector3 v = new(x, y, z); - return v; - } - - public static Spherical operator +(Spherical s1, Spherical s2) { - // let's do it the easy way... - Vector3 v1 = s1.ToVector3(); - Vector3 v2 = s2.ToVector3(); - Vector3 v = v1 + v2; - Spherical r = FromVector3(v); - return r; - } - - } -} \ No newline at end of file diff --git a/LinearAlgebra/src/SwingTwist.cs b/LinearAlgebra/src/SwingTwist.cs deleted file mode 100644 index 22eb0bb..0000000 --- a/LinearAlgebra/src/SwingTwist.cs +++ /dev/null @@ -1,41 +0,0 @@ -using System.Numerics; -#if UNITY_5_3_OR_NEWER -using Quaternion = UnityEngine.Quaternion; -#endif - -namespace LinearAlgebra { - - public class SwingTwist { - public Direction swing; - public float twist; - - public static readonly SwingTwist zero = new SwingTwist(0, 0, 0); - - public SwingTwist(Direction swing, float twist) { - this.swing = swing; - this.twist = twist; - } - public SwingTwist(float horizontalSwing, float verticalSwing, float twist) { - this.swing = Direction.Degrees(horizontalSwing, verticalSwing); - this.swing.Normalize(); - this.twist = twist; - } - public static SwingTwist FromQuat32(Quat32 q32) { - // UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w); - // SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z); - q32.ToAngles(out float right, out float up, out float forward); - SwingTwist r = new SwingTwist(up, right, forward); - return r; - } - -#if UNITY_5_3_OR_NEWER - public Quaternion ToQuaternion() { - Quaternion q = Quaternion.Euler(-this.swing.vertical, - this.swing.horizontal, - this.twist); - return q; - } -#endif - } - -} \ No newline at end of file diff --git a/LinearAlgebra/src/Vector2.cs b/LinearAlgebra/src/Vector2.cs deleted file mode 100644 index 1840a7a..0000000 --- a/LinearAlgebra/src/Vector2.cs +++ /dev/null @@ -1,363 +0,0 @@ -using System; -using System.Numerics; - -namespace LinearAlgebra { - - public class Vector2Of where T : IComparable { - public T x; - public T y; - - public Vector2Of(T x, T y) { - this.x = x; - this.y = y; - } - } - - public class Vector2Int : Vector2Of { - public Vector2Int(int x, int y) : base(x, y) { } - - public static Vector2Int operator -(Vector2Int v1, Vector2Int v2) { - return new Vector2Int(v1.x - v2.x, v1.y - v2.y); - } - - public float magnitude { - get { - return (float)Math.Sqrt(this.x * this.x + this.y * this.y); - } - } - - public static float Distance(Vector2Int v1, Vector2Int v2) { - return (v1 - v2).magnitude; - } - } - - public class Vector2Float : Vector2Of { - public Vector2Float(float x, float y) : base(x, y) { } - - public static Vector2Float operator -(Vector2Float v1, Vector2Float v2) { - return new Vector2Float(v1.x - v2.x, v1.y - v2.y); - } - - public float magnitude { - get { - return (float)Math.Sqrt(this.x * this.x + this.y * this.y); - } - } - - public static float Distance(Vector2Float v1, Vector2Float v2) { - return (v1 - v2).magnitude; - } - } - - /// - /// 2-dimensional vectors - /// - public struct Vector2 : IEquatable { - - /// - /// The right axis of the vector - /// - public float x; // left/right - /// - /// The upward/forward axis of the vector - /// - public float y; // forward/backward - // directions are to be inline with Vector3 as much as possible... - - /// - /// Create a new 2-dimensional vector - /// - /// x axis value - /// y axis value - public Vector2(float x, float y) { - this.x = x; - this.y = y; - } - - /// - /// A vector with zero for all axis - /// - public static readonly Vector2 zero = new Vector2(0, 0); - /// - /// A vector with values (1, 1) - /// - public static readonly Vector2 one = new Vector2(1, 1); - /// - /// A vector with values (0, 1) - /// - public static readonly Vector2 up = new Vector2(0, 1); - /// - /// A vector with values (0, -1) - /// - public static readonly Vector2 down = new Vector2(0, -1); - /// - /// A vector with values (0, 1) - /// - public static readonly Vector2 forward = new Vector2(0, 1); - /// - /// A vector with values (0, -1) - /// - public static readonly Vector2 back = new Vector2(0, -1); - /// - /// A vector3 with values (-1, 0) - /// - public static readonly Vector2 left = new Vector2(-1, 0); - /// - /// A vector with values (1, 0) - /// - public static readonly Vector2 right = new Vector2(1, 0); - - /// - /// The squared length of this vector - /// - /// The squared length - /// 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 { - float d = x * x + y * y; - return d; - } - } - - /// - /// The length of this vector - /// - /// The length of this vector - public float magnitude { - get { - float d = (float)Math.Sqrt(x * x + y * y); - return d; - } - } - - /// - /// Convert the vector to a length of a 1 - /// - /// The vector with length 1 - public Vector2 normalized { - get { - float l = magnitude; - Vector2 v = zero; - if (l > Float.epsilon) - v = this / l; - return v; - } - } - - /// - /// Add two vectors - /// - /// The first vector - /// The second vector - /// The result of adding the two vectors - public static Vector2 operator +(Vector2 v1, Vector2 v2) { - Vector2 v = new Vector2(v1.x + v2.x, v1.y + v2.y); - return v; - } - - /// - /// Subtract two vectors - /// - /// The first vector - /// The second vector - /// The result of adding the two vectors - public static Vector2 operator -(Vector2 v1, Vector2 v2) { - Vector2 v = new Vector2(v1.x - v2.x, v1.y - v2.y); - return v; - } - - /// - /// Negate the vector - /// - /// The vector to negate - /// The negated vector - /// This will result in a vector pointing in the opposite direction - public static Vector2 operator -(Vector2 v1) { - Vector2 v = new Vector2(-v1.x, -v1.y); - return v; - } - - /// - /// Scale a vector uniformly up - /// - /// The vector to scale - /// 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) { - Vector2 v = new Vector2(v1.x * f, v1.y * f); - return v; - } - - /// - /// Scale a vector uniformly up - /// - /// The scaling factor - /// 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) { - Vector2 v = new Vector2(f * v1.x, f * v1.y); - return v; - } - - /// - /// Scale a vector uniformly down - /// - /// The vector to scale - /// 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) { - Vector2 v = new Vector2(v1.x / f, v1.y / f); - return v; - } - - /// - /// Tests if the vector has equal values as the given vector - /// - /// The vector to compare to - /// true if the vector values are equal - public bool Equals(Vector2 v1) => x == v1.x && y == v1.y; - - /// - /// Tests if the vector is equal to the given object - /// - /// 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) { - if (!(obj is Vector2 v)) - return false; - - return (x == v.x && y == v.y); - } - - /// - /// Tests if the two vectors have equal values - /// - /// The first vector - /// The second vector - /// truewhen the vectors have equal values - /// 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) { - return (v1.x == v2.x && v1.y == v2.y); - } - - /// - /// Tests if two vectors have different values - /// - /// The first vector - /// The second vector - /// truewhen the vectors have different values - /// 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) { - return (v1.x != v2.x || v1.y != v2.y); - } - - /// - /// Get an hash code for the vector - /// - /// The hash code - public override int GetHashCode() { - return (x, y).GetHashCode(); - } - - /// - /// Get the distance between two vectors - /// - /// The first vector - /// The second vector - /// The distance between the two vectors - 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); - return d; - } - - /// - /// The dot product of two vectors - /// - /// The first vector - /// The second vector - /// The dot product of the two vectors - public static float Dot(Vector2 v1, Vector2 v2) { - return v1.x * v2.x + v1.y * v2.y; - } - - /// - /// Lerp between two vectors - /// - /// The from vector - /// The to vector - /// The interpolation distance [0..1] - /// The lerped vector - /// 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) { - Vector2 v = v1 + (v2 - v1) * f; - return v; - } - - /// - /// Calculate the signed angle between two vectors. - /// - /// The starting vector - /// The ending vector - /// The axis to rotate around - /// The signed angle in degrees - 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; - - float sqrMagFrom = from.sqrMagnitude; - float sqrMagTo = to.sqrMagnitude; - - if (sqrMagFrom == 0 || sqrMagTo == 0) - return 0; - //if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo)) - // return nanf(""); - - float angleFrom = (float)Math.Atan2(from.y, from.x); - float angleTo = (float)Math.Atan2(to.y, to.x); - return (angleTo - angleFrom) * Angle.Rad2Deg; - } - - /// - /// Rotates the vector with the given angle - /// - /// The vector to rotate - /// The angle in degrees - /// - 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() { - x = (cos * tx) - (sin * ty), - y = (sin * tx) + (cos * ty) - }; - return v; - } - - /// - /// Map interval of angles between vectors [0..Pi] to interval [0..1] - /// - /// The first vector - /// 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) { - return (1 - Vector2.Dot(v1, v2)) / 2; - } - } -} \ No newline at end of file diff --git a/LinearAlgebra/src/Vector3.cs b/LinearAlgebra/src/Vector3.cs deleted file mode 100644 index 7994dcb..0000000 --- a/LinearAlgebra/src/Vector3.cs +++ /dev/null @@ -1,179 +0,0 @@ -#if !UNITY_5_3_OR_NEWER -using System; - -namespace LinearAlgebra { - public class Vector3Of { - public T x; - public T y; - public T z; - - public Vector3Of(T x, T y, T z) { - this.x = x; - this.y = y; - this.z = z; - } - - // public uint magnitude { - // get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); - // } - } - - public class Vector3Int : Vector3Of { - public Vector3Int(int x, int y, int z) : base(x, y, z) { } - } - public class Vector3Float : Vector3Of { - public Vector3Float(float x, float y, float z) : base(x, y, z) { } - - public float magnitude { - get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); - } - } - - /// - /// 3-dimensional vectors - /// - /// This uses the right-handed coordinate system. - public struct Vector3 : IEquatable { - - /// - /// The right axis of the vector - /// - public float x; //> left/right - /// - /// The upward axis of the vector - /// - public float y; //> up/down - /// - /// The forward axis of the vector - /// - public float z; //> forward/backward - - /// - /// Create a new 3-dimensional vector - /// - /// x axis value - /// y axis value - /// z axis value - public Vector3(float x, float y, float z) { - this.x = x; - this.y = y; - this.z = z; - } - - /// - /// A vector with zero for all axis - /// - public static readonly Vector3 zero = new Vector3(0, 0, 0); - /// - /// A vector with one for all axis - /// - public static readonly Vector3 one = new Vector3(1, 1, 1); - /// - /// A vector3 with values (-1, 0, 0) - /// - public static readonly Vector3 left = new Vector3(-1, 0, 0); - /// - /// A vector with values (1, 0, 0) - /// - public static readonly Vector3 right = new Vector3(1, 0, 0); - /// - /// A vector with values (0, -1, 0) - /// - public static readonly Vector3 down = new Vector3(0, -1, 0); - /// - /// A vector with values (0, 1, 0) - /// - public static readonly Vector3 up = new Vector3(0, 1, 0); - /// - /// A vector with values (0, 0, -1) - /// - public static readonly Vector3 back = new Vector3(0, -1, 0); - /// - /// A vector with values (0, 0, 1) - /// - public static readonly Vector3 forward = new Vector3(0, 1, 0); - - public readonly float magnitude { - get { - float d = (float)Math.Sqrt(x * x + y * y + z * z); - return d; - } - } - - public Vector3 normalized { - get { - float l = magnitude; - Vector3 v = zero; - if (l > Float.epsilon) - v = this / l; - return v; - } - } - - 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) { - 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 v = new Vector3(-v1.x, -v1.y, -v1.z); - return v; - } - - 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) { - Vector3 v = new Vector3(d * v1.x, d * v1.y, d * v1.z); - return v; - } - - 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) { - 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) { - return (v1.x == v2.x && v1.y == v2.y && v1.z == v2.z); - } - - 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() { - return (x, y, z).GetHashCode(); - } - - public static float Distance(Vector3 v1, Vector3 v2) { - return (v2 - v1).magnitude; - } - - 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) { - Vector3 v = v1 + (v2 - v1) * f; - return v; - } - - } -} -#endif \ No newline at end of file diff --git a/LinearAlgebra/src/float16.cs b/LinearAlgebra/src/float16.cs deleted file mode 100644 index 4b58cdd..0000000 --- a/LinearAlgebra/src/float16.cs +++ /dev/null @@ -1,322 +0,0 @@ -using System; - -namespace LinearAlgebra { - - public class float16 { - // - // FILE: float16.cpp - // AUTHOR: Rob Tillaart - // VERSION: 0.1.8 - // PURPOSE: library for Float16s for Arduino - // URL: http://en.wikipedia.org/wiki/Half-precision_floating-point_format - - ushort _value; - - public float16() { _value = 0; } - - public float16(float f) { - //_value = f32tof16(f); - _value = F32ToF16__(f); - } - - public float toFloat() { - return f16tof32(_value); - } - - public ushort GetBinary() { return _value; } - public void SetBinary(ushort value) { _value = value; } - - ////////////////////////////////////////////////////////// - // - // EQUALITIES - // - /* - bool float16::operator ==(const float16 &f) { return (_value == f._value); } - - bool float16::operator !=(const float16 &f) { return (_value != f._value); } - - bool float16::operator >(const float16 &f) { - if ((_value & 0x8000) && (f._value & 0x8000)) - return _value < f._value; - if (_value & 0x8000) - return false; - if (f._value & 0x8000) - return true; - return _value > f._value; - } - - bool float16::operator >=(const float16 &f) { - if ((_value & 0x8000) && (f._value & 0x8000)) - return _value <= f._value; - if (_value & 0x8000) - return false; - if (f._value & 0x8000) - return true; - return _value >= f._value; - } - - bool float16::operator <(const float16 &f) { - if ((_value & 0x8000) && (f._value & 0x8000)) - return _value > f._value; - if (_value & 0x8000) - return true; - if (f._value & 0x8000) - return false; - return _value < f._value; - } - - bool float16::operator <=(const float16 &f) { - if ((_value & 0x8000) && (f._value & 0x8000)) - return _value >= f._value; - if (_value & 0x8000) - return true; - if (f._value & 0x8000) - return false; - return _value <= f._value; - } - - ////////////////////////////////////////////////////////// - // - // NEGATION - // - float16 float16::operator -() { - float16 f16; - f16.setBinary(_value ^ 0x8000); - return f16; - } - - ////////////////////////////////////////////////////////// - // - // MATH - // - float16 float16::operator +(const float16 &f) { - return float16(this->toDouble() + f.toDouble()); - } - - float16 float16::operator -(const float16 &f) { - return float16(this->toDouble() - f.toDouble()); - } - - float16 float16::operator *(const float16 &f) { - return float16(this->toDouble() * f.toDouble()); - } - - float16 float16::operator /(const float16 &f) { - return float16(this->toDouble() / f.toDouble()); - } - - float16 & float16::operator+=(const float16 &f) { - *this = this->toDouble() + f.toDouble(); - return *this; - } - - float16 & float16::operator-=(const float16 &f) { - *this = this->toDouble() - f.toDouble(); - return *this; - } - - float16 & float16::operator*=(const float16 &f) { - *this = this->toDouble() * f.toDouble(); - return *this; - } - - float16 & float16::operator/=(const float16 &f) { - *this = this->toDouble() / f.toDouble(); - return *this; - } - - ////////////////////////////////////////////////////////// - // - // MATH HELPER FUNCTIONS - // - int float16::sign() { - if (_value & 0x8000) - return -1; - if (_value & 0xFFFF) - return 1; - return 0; - } - - bool float16::isZero() { return ((_value & 0x7FFF) == 0x0000); } - - bool float16::isNaN() { - if ((_value & 0x7C00) != 0x7C00) - return false; - if ((_value & 0x03FF) == 0x0000) - return false; - return true; - } - - bool float16::isInf() { return ((_value == 0x7C00) || (_value == 0xFC00)); } - */ - ////////////////////////////////////////////////////////// - // - // CORE CONVERSION - // - float f16tof32(ushort _value) { - //ushort sgn; - ushort man; - int exp; - float f; - - //Debug.Log($"{_value}"); - - bool sgn = (_value & 0x8000) > 0; - exp = (_value & 0x7C00) >> 10; - man = (ushort)(_value & 0x03FF); - - //Debug.Log($"{sgn} {exp} {man}"); - - // ZERO - if ((_value & 0x7FFF) == 0) { - return sgn ? -0 : 0; - } - // NAN & INF - if (exp == 0x001F) { - if (man == 0) - return sgn ? float.NegativeInfinity : float.PositiveInfinity; //-INFINITY : INFINITY; - else - return float.NaN; // NAN; - } - - // SUBNORMAL/NORMAL - if (exp == 0) - f = 0; - else - f = 1; - - // PROCESS MANTISSE - 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) { - f = f * (float)Math.Pow(2.0f, -13); // 5.96046447754e-8; - } - //Debug.Log($"{f}"); - return sgn ? -f : f; - } - - 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) { - 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) { - return sgn ? (ushort)0x8000 : (ushort)0x0000; - } - // denormalized float32 does not fit in float16 - if (exp == 0x00) { - return sgn ? (ushort)0x8000 : (ushort)0x0000; - } - // handle infinity & NAN - if (exp == 0x00FF) { - if (man != 0) - return 0xFE00; // NAN - return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF - } - - // normal numbers - exp = exp - 127 + 15; - // overflow does not fit => INF - if (exp > 30) { - return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF - } - // subnormal numbers - if (exp < -38) { - return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ? - } - if (exp <= 0) // subnormal - { - man >>= (exp + 14); - // rounding - man++; - man >>= 1; - if (sgn) - return (ushort)(0x8000 | man); - return man; - } - - // normal - // TODO rounding - exp <<= 10; - man++; - man >>= 1; - if (sgn) - return (ushort)(0x8000 | exp | man); - return (ushort)(exp | man); - } - - //This function is faulty!!!! - ushort f32tof16(float f) { - //uint t = *(uint*)&f; - //uint t = (uint)BitConverter.SingleToInt32Bits(f); - uint t = SingleToInt32Bits(f); - // man bits = 10; but we keep 11 for rounding - ushort man = (ushort)((t & 0x007FFFFF) >> 12); - short exp = (short)((t & 0x7F800000) >> 23); - bool sgn = (t & 0x80000000) != 0; - - // handle 0 - if ((t & 0x7FFFFFFF) == 0) { - return sgn ? (ushort)0x8000 : (ushort)0x0000; - } - // denormalized float32 does not fit in float16 - if (exp == 0x00) { - return sgn ? (ushort)0x8000 : (ushort)0x0000; - } - // handle infinity & NAN - if (exp == 0x00FF) { - if (man != 0) - return 0xFE00; // NAN - return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF - } - - // normal numbers - exp = (short)(exp - 127 + 15); - // overflow does not fit => INF - if (exp > 30) { - return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF - } - // subnormal numbers - if (exp < -38) { - return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ? - } - if (exp <= 0) // subnormal - { - man >>= (exp + 14); - // rounding - man++; - man >>= 1; - if (sgn) - return (ushort)(0x8000 | man); - return man; - } - - // normal - // TODO rounding - exp <<= 10; - man++; - man >>= 1; - ushort uexp = (ushort)exp; - if (sgn) - return (ushort)(0x8000 | uexp | man); - return (ushort)(uexp | man); - } - - // -- END OF FILE -- - } - -} \ No newline at end of file diff --git a/LinearAlgebra/test/AngleTest.cs b/LinearAlgebra/test/AngleTest.cs deleted file mode 100644 index c248465..0000000 --- a/LinearAlgebra/test/AngleTest.cs +++ /dev/null @@ -1,171 +0,0 @@ -#if !UNITY_5_6_OR_NEWER -using NUnit.Framework; - -namespace LinearAlgebra.Test -{ - public class Tests - { - [SetUp] - public void Setup() - { - } - - [Test] - public void Normalize() - { - float r = 0; - - r = Angle.Normalize(90); - Assert.AreEqual(r, 90, "Normalize 90"); - - r = Angle.Normalize(-90); - Assert.AreEqual(r, -90, "Normalize -90"); - - r = Angle.Normalize(270); - Assert.AreEqual(r, -90, "Normalize 270"); - - r = Angle.Normalize(270 + 360); - Assert.AreEqual(r, -90, "Normalize 270+360"); - - r = Angle.Normalize(-270); - Assert.AreEqual(r, 90, "Normalize -270"); - - r = Angle.Normalize(-270 - 360); - Assert.AreEqual(r, 90, "Normalize -270-360"); - - r = Angle.Normalize(0); - Assert.AreEqual(r, 0, "Normalize 0"); - - r = Angle.Normalize(float.PositiveInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "Normalize INFINITY"); - - r = Angle.Normalize(float.NegativeInfinity); - Assert.AreEqual(r, float.NegativeInfinity, "Normalize INFINITY"); - } - - [Test] - public void Clamp() - { - float r = 0; - - r = Angle.Clamp(1, 0, 2); - Assert.AreEqual(r, 1, "Clamp 1 0 2"); - - r = Angle.Clamp(-1, 0, 2); - Assert.AreEqual(r, 0, "Clamp -1 0 2"); - - r = Angle.Clamp(3, 0, 2); - Assert.AreEqual(r, 2, "Clamp 3 0 2"); - - r = Angle.Clamp(1, 0, 0); - Assert.AreEqual(r, 0, "Clamp 1 0 0"); - - r = Angle.Clamp(0, 0, 0); - Assert.AreEqual(r, 0, "Clamp 0 0 0"); - - r = Angle.Clamp(0, 1, -1); - Assert.AreEqual(r, 1, "Clamp 0 1 -1"); - - r = Angle.Clamp(1, 0, float.PositiveInfinity); - Assert.AreEqual(r, 1, "Clamp 1 0 INFINITY"); - - r = Angle.Clamp(1, float.NegativeInfinity, 1); - Assert.AreEqual(r, 1, "Clamp 1 -INFINITY 1"); - } - - [Test] - public void Difference() - { - float r = 0; - - r = Angle.Difference(0, 90); - Assert.AreEqual(r, 90, "Difference 0 90"); - - r = Angle.Difference(0, -90); - Assert.AreEqual(r, -90, "Difference 0 -90"); - - r = Angle.Difference(0, 270); - Assert.AreEqual(r, -90, "Difference 0 270"); - - r = Angle.Difference(0, -270); - Assert.AreEqual(r, 90, "Difference 0 -270"); - - r = Angle.Difference(90, 0); - Assert.AreEqual(r, -90, "Difference 90 0"); - - r = Angle.Difference(-90, 0); - Assert.AreEqual(r, 90, "Difference -90 0"); - - r = Angle.Difference(0, 0); - Assert.AreEqual(r, 0, "Difference 0 0"); - - r = Angle.Difference(90, 90); - Assert.AreEqual(r, 0, "Difference 90 90"); - - r = Angle.Difference(0, float.PositiveInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "Difference 0 INFINITY"); - - r = Angle.Difference(0, float.NegativeInfinity); - Assert.AreEqual(r, float.NegativeInfinity, "Difference 0 -INFINITY"); - - r = Angle.Difference(float.NegativeInfinity, float.PositiveInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "Difference -INFINITY INFINITY"); - } - - [Test] - public void MoveTowards() - { - float r = 0; - - r = Angle.MoveTowards(0, 90, 30); - Assert.AreEqual(r, 30, "MoveTowards 0 90 30"); - - r = Angle.MoveTowards(0, 90, 90); - Assert.AreEqual(r, 90, "MoveTowards 0 90 90"); - - r = Angle.MoveTowards(0, 90, 180); - Assert.AreEqual(r, 90, "MoveTowards 0 90 180"); - - r = Angle.MoveTowards(0, 90, 270); - Assert.AreEqual(r, 90, "MoveTowrads 0 90 270"); - - r = Angle.MoveTowards(0, 90, -30); - Assert.AreEqual(r, -30, "MoveTowards 0 90 -30"); - - r = Angle.MoveTowards(0, -90, -30); - Assert.AreEqual(r, 30, "MoveTowards 0 -90 -30"); - - r = Angle.MoveTowards(0, -90, -90); - Assert.AreEqual(r, 90, "MoveTowards 0 -90 -90"); - - r = Angle.MoveTowards(0, -90, -180); - Assert.AreEqual(r, 180, "MoveTowards 0 -90 -180"); - - r = Angle.MoveTowards(0, -90, -270); - Assert.AreEqual(r, 270, "MoveTowrads 0 -90 -270"); - - r = Angle.MoveTowards(0, 90, 0); - Assert.AreEqual(r, 0, "MoveTowards 0 90 0"); - - r = Angle.MoveTowards(0, 0, 0); - Assert.AreEqual(r, 0, "MoveTowards 0 0 0"); - - r = Angle.MoveTowards(0, 0, 30); - Assert.AreEqual(r, 0, "MoveTowrads 0 0 30"); - - r = Angle.MoveTowards(0, 90, float.PositiveInfinity); - Assert.AreEqual(r, 90, "MoveTowards 0 90 INFINITY"); - - r = Angle.MoveTowards(0, float.PositiveInfinity, 30); - Assert.AreEqual(r, 30, "MoveTowrads 0 INFINITY 30"); - - r = Angle.MoveTowards(0, -90, float.NegativeInfinity); - Assert.AreEqual(r, float.PositiveInfinity, "MoveTowards 0 -90 -INFINITY"); - - r = Angle.MoveTowards(0, float.NegativeInfinity, -30); - Assert.AreEqual(r, 30, "MoveTowrads 0 -INFINITY -30"); - - } - } -} -#endif \ No newline at end of file diff --git a/LinearAlgebra/test/LinearAlgebra_Test.csproj b/LinearAlgebra/test/LinearAlgebra_Test.csproj deleted file mode 100644 index 3ee2230..0000000 --- a/LinearAlgebra/test/LinearAlgebra_Test.csproj +++ /dev/null @@ -1,19 +0,0 @@ - - - - net5.0 - false - true - - - - - - - - - - - - - diff --git a/LinearAlgebra/test/SphericalTest.cs b/LinearAlgebra/test/SphericalTest.cs deleted file mode 100644 index 3ede4f4..0000000 --- a/LinearAlgebra/test/SphericalTest.cs +++ /dev/null @@ -1,30 +0,0 @@ -#if !UNITY_5_6_OR_NEWER -using NUnit.Framework; - -namespace LinearAlgebra.Test { - public class SphericalTest { - [SetUp] - public void Setup() { - } - - [Test] - public void FromVector3() { - Vector3 v = new(0, 0, 1); - Spherical s = Spherical.FromVector3(v); - Assert.AreEqual(1.0f, s.distance, "s.distance 0 0 1"); - Assert.AreEqual(0.0f, s.direction.horizontal, "s.hor 0 0 1"); - Assert.AreEqual(0.0f, s.direction.vertical, "s.vert 0 0 1"); - } - - [Test] - public void Addition() { - Spherical v1 = Spherical.Degrees(1, 45, 0); - Spherical v2 = Spherical.zero; - Spherical r = Spherical.zero; - - r = v1 + v2; - Assert.AreEqual(v1.distance, r.distance, "Addition(0,0,0)"); - } - } -} -#endif \ No newline at end of file From 7f3698ecaa97a7d8e0f5af233ad4a445513466d9 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 28 May 2025 12:18:22 +0200 Subject: [PATCH 11/20] Fix compatibility with lastest LinearAlgebra --- src/Things/DifferentialDrive.cs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/Things/DifferentialDrive.cs b/src/Things/DifferentialDrive.cs index 271ea0d..039d26c 100644 --- a/src/Things/DifferentialDrive.cs +++ b/src/Things/DifferentialDrive.cs @@ -1,3 +1,4 @@ +using System; using LinearAlgebra; namespace RoboidControl { @@ -50,7 +51,7 @@ namespace RoboidControl { /// @sa SetLinearVelocity SetAngularVelocity public void SetDriveDimensions(float wheelDiameter, float wheelSeparation = 0) { this._wheelRadius = wheelDiameter > 0 ? wheelDiameter / 2 : -wheelDiameter / 2; - this.rpsToMs = wheelDiameter * Angle.pi; + this.rpsToMs = wheelDiameter * (float)Math.PI; if (wheelSeparation > 0) { wheelSeparation = wheelSeparation > 0 ? wheelSeparation : -wheelSeparation; From 63b5a75618bdb79357f46f2a10897140cf84cf65 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 28 May 2025 12:35:19 +0200 Subject: [PATCH 12/20] Compatibility --- LinearAlgebra/test/DirectionTest.cs | 4 +++- Unity/RelativeEncoder.cs | 2 +- Unity/Thing.cs | 24 ++++++++++++------------ Unity/Wheel.cs | 5 +++-- src/Participant.cs | 1 - src/Thing.cs | 1 - src/Things/RelativeEncoder.cs | 2 -- 7 files changed, 19 insertions(+), 20 deletions(-) diff --git a/LinearAlgebra/test/DirectionTest.cs b/LinearAlgebra/test/DirectionTest.cs index e31af4c..3cebe1a 100644 --- a/LinearAlgebra/test/DirectionTest.cs +++ b/LinearAlgebra/test/DirectionTest.cs @@ -1,3 +1,4 @@ +#if !UNITY_5_6_OR_NEWER using NUnit.Framework; namespace LinearAlgebra.Test { @@ -14,4 +15,5 @@ namespace LinearAlgebra.Test { Assert.True(r); } }; -} \ No newline at end of file +} +#endif \ No newline at end of file diff --git a/Unity/RelativeEncoder.cs b/Unity/RelativeEncoder.cs index 521878c..475a85c 100644 --- a/Unity/RelativeEncoder.cs +++ b/Unity/RelativeEncoder.cs @@ -56,7 +56,7 @@ namespace RoboidControl.Unity { // Normalize angles to range [-180..180) // Note: it is not possible to track rotation speeds higher than 180 degrees per frame because of representation limitations of Quaternions - float deltaAngle = Angle.Normalize(deltaRotation.eulerAngles.x); + float deltaAngle = Angle.Degrees(deltaRotation.eulerAngles.x).inDegrees; this.rotationSpeed = deltaAngle / Time.deltaTime; } diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 43476eb..8947915 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -190,19 +190,19 @@ namespace RoboidControl.Unity { SkinnedMeshRenderer[] meshRenderers = parentTransform.GetComponentsInChildren(); #if pHUMANOID4 - if (parentThing.objectType == 7) { - HumanoidControl hc = parentThing.gameObject.GetComponent(); - if (hc == null) - hc = parentThing.gameObject.AddComponent(); + // if (parentThing.objectType == 7) { + // HumanoidControl hc = parentThing.gameObject.GetComponent(); + // if (hc == null) + // hc = parentThing.gameObject.AddComponent(); - foreach (SkinnedMeshRenderer meshRenderer in meshRenderers) { - if (meshRenderer.rootBone != null) { - Debug.Log("Found a skinned mesh with bones"); - hc.RetrieveBonesFrom(meshRenderer.rootBone); - break; - } - } - } + // foreach (SkinnedMeshRenderer meshRenderer in meshRenderers) { + // if (meshRenderer.rootBone != null) { + // Debug.Log("Found a skinned mesh with bones"); + // hc.RetrieveBonesFrom(meshRenderer.rootBone); + // break; + // } + // } + // } #endif parentTransform.localScale = Vector3.one; if (meshRenderers.Length > 0) { diff --git a/Unity/Wheel.cs b/Unity/Wheel.cs index 53b7820..41d427f 100644 --- a/Unity/Wheel.cs +++ b/Unity/Wheel.cs @@ -38,7 +38,7 @@ namespace RoboidControl.Unity { GameObject gameObj = Instantiate(prefab); Wheel component = gameObj.GetComponent(); if (component != null) - component.core = new RoboidControl.Thing(RoboidControl.Thing.Type.UncontrolledMotor, false); + component.core = new RoboidControl.Thing(RoboidControl.Thing.Type.UncontrolledMotor); return component; } else { @@ -49,7 +49,8 @@ namespace RoboidControl.Unity { SiteServer participant = FindAnyObjectByType(); RoboidControl.Thing core = participant.coreParticipant.Get(thingId); if (core == null) - core = new(participant.coreParticipant, RoboidControl.Thing.Type.UncontrolledMotor, thingId, false); + //core = new(participant.coreParticipant, RoboidControl.Thing.Type.UncontrolledMotor, thingId, false); + core = RoboidControl.Thing.CreateRemote(participant.coreParticipant, RoboidControl.Thing.Type.UncontrolledMotor, thingId); else { ; } diff --git a/src/Participant.cs b/src/Participant.cs index a936b87..74c3773 100644 --- a/src/Participant.cs +++ b/src/Participant.cs @@ -3,7 +3,6 @@ using System.Collections.Generic; using System.Collections.Concurrent; using System.Net; using System.Net.Sockets; -using System.Reflection.Metadata; namespace RoboidControl { diff --git a/src/Thing.cs b/src/Thing.cs index b9f9b29..72a54b1 100644 --- a/src/Thing.cs +++ b/src/Thing.cs @@ -2,7 +2,6 @@ using System; using System.Collections.Generic; using System.Collections.Concurrent; using LinearAlgebra; -using Microsoft.VisualStudio.TestPlatform.ObjectModel.DataCollection; namespace RoboidControl { diff --git a/src/Things/RelativeEncoder.cs b/src/Things/RelativeEncoder.cs index d3bc8b7..aee9d7c 100644 --- a/src/Things/RelativeEncoder.cs +++ b/src/Things/RelativeEncoder.cs @@ -1,6 +1,4 @@ -using NUnit.Framework; - namespace RoboidControl { /// @brief An Incremental Encoder measures the rotations of an axle using a rotary From e646326aa31b7cc73ecfca89b3b1f2b85f024e42 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 28 May 2025 12:49:52 +0200 Subject: [PATCH 13/20] Add missing update event --- src/Thing.cs | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/Thing.cs b/src/Thing.cs index 72a54b1..a231304 100644 --- a/src/Thing.cs +++ b/src/Thing.cs @@ -117,8 +117,14 @@ namespace RoboidControl { else this.parent = parent; - this.owner = parent.owner; + this.owner = this.parent.owner; this.owner.Add(this, true); + + Participant.UpdateEvent e = new() { + messageId = ThingMsg.id, + thing = this + }; + this.owner.updateQueue.Enqueue(e); } public static Thing CreateRemote(Participant owner, byte thingType, byte thingId) { From 0400c646fadf940370edc9385dbbdc1fc08f9f7a Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 28 May 2025 16:06:35 +0200 Subject: [PATCH 14/20] Implemented ReplaceLocalParticipant --- src/Participant.cs | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/Participant.cs b/src/Participant.cs index 74c3773..936fd3c 100644 --- a/src/Participant.cs +++ b/src/Participant.cs @@ -33,7 +33,10 @@ namespace RoboidControl { this.udpClient = localParticipant.udpClient; } - public static readonly Participant localParticipant = new(); + public static Participant localParticipant = new(); + public static void ReplaceLocalParticipant(Participant participant) { + Participant.localParticipant = participant; + } public Thing root = null; From e48ab979b077d355ca5ac7fc91973a40b839d695 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 28 May 2025 16:46:47 +0200 Subject: [PATCH 15/20] Using replaceLocalParticipant --- src/Participants/ParticipantUDP.cs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/Participants/ParticipantUDP.cs b/src/Participants/ParticipantUDP.cs index 0fde7f2..26563e1 100644 --- a/src/Participants/ParticipantUDP.cs +++ b/src/Participants/ParticipantUDP.cs @@ -35,6 +35,7 @@ namespace RoboidControl { if (this.port == 0) this.isIsolated = true; Participant.AddParticipant(this); + Participant.ReplaceLocalParticipant(this); } /// /// Create a participant which will try to connect to a site. @@ -79,6 +80,7 @@ namespace RoboidControl { this.remoteSite = new Participant(ipAddress, port, this); Participant.AddParticipant(this); + Participant.ReplaceLocalParticipant(this); } private static ParticipantUDP isolatedParticipant = null; From 829c4cdd25552adb01ae405e5566273c5c6f8549 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 28 May 2025 16:53:33 +0200 Subject: [PATCH 16/20] ensure root is defined --- src/Participant.cs | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/Participant.cs b/src/Participant.cs index 936fd3c..3276855 100644 --- a/src/Participant.cs +++ b/src/Participant.cs @@ -29,6 +29,10 @@ namespace RoboidControl { public Participant(string ipAddress, int port, Participant localParticipant = null) { this.ipAddress = ipAddress; this.port = port; + + this.root = Thing.CreateRoot(this); + this.root.name = "Root"; + if (localParticipant != null) this.udpClient = localParticipant.udpClient; } @@ -39,7 +43,6 @@ namespace RoboidControl { } public Thing root = null; - /// /// The Ip Address of a participant. When the participant is local, this contains 0.0.0.0 /// From dfec91cb62978284e0db4353a3447e9e4de3a051 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 28 May 2025 17:12:27 +0200 Subject: [PATCH 17/20] Ensure root thing is parented to remote participant --- Unity/SiteServer.cs | 1 + Unity/Thing.cs | 11 ++++++++--- src/Participant.cs | 8 ++++++++ 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs index 3226edc..c067844 100644 --- a/Unity/SiteServer.cs +++ b/Unity/SiteServer.cs @@ -41,6 +41,7 @@ namespace RoboidControl.Unity { GameObject remoteParticipant = new GameObject("RemoteParticipant"); Participant participant = remoteParticipant.AddComponent(); participant.coreParticipant = e.participant; + participant.coreParticipant.component = participant; break; case ThingMsg.id: HandleThingEvent(e); diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 8947915..03ac532 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -46,13 +46,18 @@ namespace RoboidControl.Unity { protected void Init(RoboidControl.Thing core) { this.core = core; this.core.component = this; - this.owner = FindAnyObjectByType(); - core.owner = this.owner.coreParticipant; + // This is wrong, it should get the owner, which is not the siteserver + // this.owner = FindAnyObjectByType(); + // core.owner = this.owner.coreParticipant; + this.owner = core.owner.component; if (core.parent != null && core.parent.component != null) { this.transform.SetParent(core.parent.component.transform, false); this.transform.localPosition = Vector3.zero; } + else { + this.transform.SetParent(core.owner.component.transform, false); + } if (core.position != null) this.transform.localPosition = core.position.ToVector3(); @@ -108,7 +113,7 @@ namespace RoboidControl.Unity { case ThingMsg.id: Debug.Log($"{this.core.id} Handle Thing"); if (core.parent == null) - this.transform.SetParent(null, true); + this.transform.SetParent(core.owner.component.transform, true); else if (core.parent.component != null) this.transform.SetParent(core.parent.component.transform, true); break; diff --git a/src/Participant.cs b/src/Participant.cs index 3276855..d1588e9 100644 --- a/src/Participant.cs +++ b/src/Participant.cs @@ -52,6 +52,14 @@ namespace RoboidControl { /// public int port = 0; +#if UNITY_5_3_OR_NEWER + /// + /// A reference to the representation of the thing in Unity + /// + [NonSerialized] + public Unity.Participant component = null; +#endif + public UdpClient udpClient = null; /// From 3cf49f649ddd93b75eb3b5e661271de8406a6a5a Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 28 May 2025 17:16:37 +0200 Subject: [PATCH 18/20] Fix hierarchyChanged not resetting --- src/Thing.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Thing.cs b/src/Thing.cs index a231304..03b11f5 100644 --- a/src/Thing.cs +++ b/src/Thing.cs @@ -421,7 +421,7 @@ namespace RoboidControl { this.orientationUpdated = false; this.linearVelocityUpdated = false; this.angularVelocityUpdated = false; - //this.hierarchyChanged = false; + this.hierarchyChanged = false; // should recurse over children... if (recurse) { From 5ef609fd122df52b8dc8ac611a85bd2792d5ea97 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 28 May 2025 17:34:57 +0200 Subject: [PATCH 19/20] Support sending poses from Unity changes --- LinearAlgebra/src/SwingTwist.cs | 9 ++++++++- Unity/Thing.cs | 1 + src/Thing.cs | 1 + 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/LinearAlgebra/src/SwingTwist.cs b/LinearAlgebra/src/SwingTwist.cs index 22eb0bb..58c1a1a 100644 --- a/LinearAlgebra/src/SwingTwist.cs +++ b/LinearAlgebra/src/SwingTwist.cs @@ -29,11 +29,18 @@ namespace LinearAlgebra { } #if UNITY_5_3_OR_NEWER + public static SwingTwist FromQuaternion(Quaternion q) { + // q.ToAngles(out float right, out float up, out float forward); + UnityEngine.Vector3 angles = q.eulerAngles; + SwingTwist r = new SwingTwist(angles.y, angles.x, angles.z); + return r; + } + public Quaternion ToQuaternion() { Quaternion q = Quaternion.Euler(-this.swing.vertical, this.swing.horizontal, this.twist); - return q; + return q; } #endif } diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 03ac532..326aa7c 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -88,6 +88,7 @@ namespace RoboidControl.Unity { /// protected virtual void FixedUpdate() { UpdateThing(); + core.orientation = LinearAlgebra.SwingTwist.FromQuaternion(this.transform.localRotation); } /// diff --git a/src/Thing.cs b/src/Thing.cs index 03b11f5..e642388 100644 --- a/src/Thing.cs +++ b/src/Thing.cs @@ -340,6 +340,7 @@ namespace RoboidControl { if (_orientation != value) { _orientation = value; orientationUpdated = true; + updateQueue.Enqueue(new CoreEvent(PoseMsg.Id)); //OnOrientationChanged?.Invoke(); } } From a9d7c6d1453bba12d18e8bc895687e5b70ff0494 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Wed, 28 May 2025 17:42:41 +0200 Subject: [PATCH 20/20] Unity thing orientation is updated, but not correct yet --- Unity/SiteServer.cs | 2 ++ Unity/Thing.cs | 1 + 2 files changed, 3 insertions(+) diff --git a/Unity/SiteServer.cs b/Unity/SiteServer.cs index c067844..24205c7 100644 --- a/Unity/SiteServer.cs +++ b/Unity/SiteServer.cs @@ -42,6 +42,8 @@ namespace RoboidControl.Unity { Participant participant = remoteParticipant.AddComponent(); participant.coreParticipant = e.participant; participant.coreParticipant.component = participant; + participant.ipAddress = e.participant.ipAddress; + participant.port = e.participant.port; break; case ThingMsg.id: HandleThingEvent(e); diff --git a/Unity/Thing.cs b/Unity/Thing.cs index 326aa7c..575b72a 100644 --- a/Unity/Thing.cs +++ b/Unity/Thing.cs @@ -260,6 +260,7 @@ namespace RoboidControl.Unity { /// This can update the position and/or orientation when the velocity of the thing is zero. /// If a velocity is not zero, the position and/or orientation update will be ignored protected virtual void HandlePose() { + this.transform.localRotation = core.orientation.ToQuaternion(); if (core.linearVelocity.distance == 0) this.transform.localPosition = core.position.ToVector3(); if (core.angularVelocity.distance == 0)