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