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/src/Angle.cs b/src/Angle.cs index ca03a50..7bb8dc3 100644 --- a/src/Angle.cs +++ b/src/Angle.cs @@ -1,13 +1,11 @@ using System; -namespace 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; @@ -23,8 +21,7 @@ namespace 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); } @@ -36,8 +33,7 @@ namespace 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; } @@ -48,8 +44,7 @@ namespace 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; @@ -66,8 +61,7 @@ namespace 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); @@ -83,8 +77,7 @@ namespace 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 258e27e..f231029 100644 --- a/src/Direction.cs +++ b/src/Direction.cs @@ -1,34 +1,56 @@ -namespace LinearAlgebra -{ +namespace LinearAlgebra { - public class Direction - { + /// + /// 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() - { + public Direction() { horizontal = 0; vertical = 0; } - public Direction(float horizontal, float vertical) - { - this.horizontal = horizontal; - this.vertical = vertical; + // 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 = 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 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) - { + 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 70873f0..2217b84 100644 --- a/src/Float.cs +++ b/src/Float.cs @@ -1,11 +1,9 @@ -namespace LinearAlgebra -{ +namespace LinearAlgebra { /// /// Float number utilities /// - public class Float - { + public class Float { /// /// The precision of float numbers /// @@ -22,8 +20,7 @@ namespace 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) @@ -36,8 +33,7 @@ namespace 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 1f0542e..34fd2cc 100644 --- a/src/Matrix.cs +++ b/src/Matrix.cs @@ -5,54 +5,44 @@ using Vector2Float = UnityEngine.Vector2; using Quaternion = UnityEngine.Quaternion; #endif -namespace LinearAlgebra -{ +namespace LinearAlgebra { - public readonly struct Slice - { + public readonly struct Slice { public uint start { get; } public uint stop { get; } - public Slice(uint start, uint stop) - { + public Slice(uint start, uint stop) { this.start = start; this.stop = stop; } } - public class Matrix2 - { + 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) - { + public Matrix2(uint nRows, uint nCols) { this.data = new float[nRows, nCols]; } - public Matrix2(float[,] data) - { + public Matrix2(float[,] data) { this.data = data; } - public Matrix2 Clone() - { + public Matrix2 Clone() { float[,] data = new float[this.nRows, nCols]; - for (int rowIx = 0; rowIx < this.nRows; rowIx++) - { + 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) - { + public static Matrix2 Zero(uint nRows, uint nCols) { return new Matrix2(nRows, nCols); } - public static Matrix2 FromVector3(Vector3Float v) - { + public static Matrix2 FromVector3(Vector3Float v) { float[,] result = new float[3, 1]; result[0, 0] = v.x; result[1, 0] = v.y; @@ -60,46 +50,39 @@ namespace LinearAlgebra return new Matrix2(result); } - public static Matrix2 Identity(uint size) - { + public static Matrix2 Identity(uint size) { return Diagonal(1, size); } - public static Matrix2 Identity(uint nRows, uint nCols) - { + public static Matrix2 Identity(uint nRows, uint nCols) { Matrix2 m = Zero(nRows, nCols); m.FillDiagonal(1); return m; } - public static Matrix2 Diagonal(Matrix1 v) - { + 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) - { + 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) - { + 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) - { + 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) - { + public static Matrix2 SkewMatrix(Vector3Float v) { float[,] result = new float[3, 3] { {0, -v.z, v.y}, {v.z, 0, -v.x}, @@ -119,34 +102,27 @@ namespace LinearAlgebra return row; } #endif - public void SetRow(int rowIx, Matrix1 v) - { + 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) - { + 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) - { + public Matrix1 GetColumn(int colIx) { float[] column = new float[this.nRows]; - for (int i = 0; i < this.nRows; i++) - { + 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++) - { + 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; @@ -155,75 +131,63 @@ namespace LinearAlgebra return true; } - public Matrix2 Transpose() - { + public Matrix2 Transpose() { float[,] resultData = new float[this.nCols, this.nRows]; - for (uint rowIx = 0; rowIx < this.nRows; rowIx++) - { + 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 - { + public Matrix2 transposed { get => Transpose(); } - public static Matrix2 operator -(Matrix2 m) - { + public static Matrix2 operator -(Matrix2 m) { float[,] result = new float[m.nRows, m.nCols]; - for (int i = 0; i < m.nRows; i++) - { + 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) - { + 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 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) - { + 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 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) - { + 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++) - { + 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]; @@ -236,14 +200,11 @@ namespace LinearAlgebra // double checked code } - public static Matrix1 operator *(Matrix2 A, Matrix1 v) - { + 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++) - { + 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]; } } @@ -251,8 +212,7 @@ namespace LinearAlgebra return new Matrix1(result); } - public static Vector3Float operator *(Matrix2 A, Vector3Float v) - { + 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, @@ -260,41 +220,34 @@ namespace LinearAlgebra ); } - public static Matrix2 operator *(Matrix2 A, float 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 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) - { + public static Matrix2 operator *(float s, Matrix2 A) { return A * s; } - public static Matrix2 operator /(Matrix2 A, float 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 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) - { + 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 i = 0; i < A.nRows; i++) { for (int j = 0; j < A.nCols; j++) result[i, j] = s / A.data[i, j]; } @@ -302,21 +255,17 @@ namespace LinearAlgebra return new Matrix2(result); } - public Matrix2 Slice(Slice slice) - { + public Matrix2 Slice(Slice slice) { return Slice(slice.start, slice.stop); } - public Matrix2 Slice(uint from, uint to) - { + 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++) - { + for (uint rowIx = from; rowIx < to; rowIx++) { + for (int colIx = 0; colIx < this.nCols; colIx++) { result[resultRowIx, colIx] = this.data[rowIx, colIx]; } resultRowIx++; @@ -324,67 +273,55 @@ namespace LinearAlgebra return new Matrix2(result); } - public Matrix2 Slice(Slice rowRange, Slice colRange) - { + 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) - { + 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 i = rowRange.start; i < rowRange.stop; i++) { for (uint j = colRange.start; j < colRange.stop; j++) result[resultRowIx, resultColIx] = this.data[i, j]; } return new Matrix2(result); } - public void UpdateSlice(Slice slice, Matrix2 m) - { + public void UpdateSlice(Slice slice, Matrix2 m) { int mRowIx = 0; - for (uint rowIx = slice.start; rowIx < slice.stop; rowIx++, mRowIx++) - { + 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) - { + 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++) - { + 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() - { + 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++) - { + 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++) - { + 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 @@ -395,8 +332,7 @@ namespace LinearAlgebra augmentedMatrix[i, j] /= pivot; // Eliminate the column below the pivot - for (int j = i + 1; j < n; j++) - { + 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]; @@ -404,11 +340,9 @@ namespace LinearAlgebra } // Back substitution - for (uint i = n - 1; i >= 0; i--) - { + for (uint i = n - 1; i >= 0; i--) { // Eliminate the column above the pivot - for (uint j = i - 1; j >= 0; j--) - { + 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]; @@ -417,8 +351,7 @@ namespace LinearAlgebra // Extract the inverse matrix from the augmented matrix float[,] inverse = new float[n, n]; - for (int i = 0; i < n; i++) - { + for (int i = 0; i < n; i++) { for (int j = 0; j < n; j++) inverse[i, j] = augmentedMatrix[i, j + n]; } @@ -426,8 +359,7 @@ namespace LinearAlgebra return new Matrix2(inverse); } - public float Determinant() - { + public float Determinant() { uint n = this.nRows; if (n != this.nCols) throw new System.ArgumentException("Matrix must be square."); @@ -446,19 +378,16 @@ namespace LinearAlgebra } // Helper function to compute the minor of a matrix - private Matrix2 Minor(int rowToRemove, int colToRemove) - { + 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++) - { + for (int i = 0; i < n; i++) { if (i == rowToRemove) continue; c = 0; - for (int j = 0; j < n; j++) - { + for (int j = 0; j < n; j++) { if (j == colToRemove) continue; minor[r, c] = this.data[i, j]; @@ -471,37 +400,31 @@ namespace LinearAlgebra } } - public class Matrix1 - { + public class Matrix1 { public float[] data { get; } public uint size => (uint)data.GetLength(0); - public Matrix1(uint size) - { + public Matrix1(uint size) { this.data = new float[size]; } - public Matrix1(float[] data) - { + public Matrix1(float[] data) { this.data = data; } - public static Matrix1 Zero(uint size) - { + public static Matrix1 Zero(uint size) { return new Matrix1(size); } - public static Matrix1 FromVector2(Vector2Float v) - { + 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) - { + public static Matrix1 FromVector3(Vector3Float v) { float[] result = new float[3]; result[0] = v.x; result[1] = v.y; @@ -520,19 +443,15 @@ namespace LinearAlgebra } #endif - public Vector2Float vector2 - { - get - { + 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 - { + 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]); @@ -549,8 +468,7 @@ namespace LinearAlgebra } #endif - public Matrix1 Clone() - { + public Matrix1 Clone() { float[] data = new float[this.size]; for (int rowIx = 0; rowIx < this.size; rowIx++) data[rowIx] = this.data[rowIx]; @@ -558,32 +476,27 @@ namespace LinearAlgebra } - public float magnitude - { - get - { + 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) - { + 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++) - { + for (int i = 0; i < A.size; i++) { result[i] = A.data[i] + B.data[i]; } return new Matrix1(result); } - public Matrix2 Transpose() - { + public Matrix2 Transpose() { float[,] r = new float[1, this.size]; for (uint colIx = 0; colIx < this.size; colIx++) r[1, colIx] = this.data[colIx]; @@ -591,21 +504,18 @@ namespace LinearAlgebra return new Matrix2(r); } - public static float Dot(Matrix1 a, Matrix1 b) - { + 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++) - { + 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) - { + public static Matrix1 operator *(Matrix1 A, float f) { float[] result = new float[A.size]; for (int i = 0; i < A.size; i++) @@ -613,17 +523,14 @@ namespace LinearAlgebra return new Matrix1(result); } - public static Matrix1 operator *(float f, Matrix1 A) - { + public static Matrix1 operator *(float f, Matrix1 A) { return A * f; } - public Matrix1 Slice(Slice range) - { + public Matrix1 Slice(Slice range) { return Slice(range.start, range.stop); } - public Matrix1 Slice(uint from, uint to) - { + public Matrix1 Slice(uint from, uint to) { if (from < 0 || to >= this.size) throw new System.ArgumentException("Slice index out of range."); @@ -634,8 +541,7 @@ namespace LinearAlgebra return new Matrix1(result); } - public void UpdateSlice(Slice slice, Matrix1 v) - { + 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]; diff --git a/src/Quat32.cs b/src/Quat32.cs index 3026f8e..f13266d 100644 --- a/src/Quat32.cs +++ b/src/Quat32.cs @@ -1,38 +1,32 @@ using System; -namespace LinearAlgebra -{ - public class Quat32 - { +namespace LinearAlgebra { + public class Quat32 { public float x; public float y; public float z; public float w; - public Quat32() - { + public Quat32() { this.x = 0; this.y = 0; this.z = 0; this.w = 1; } - public Quat32(float x, float y, float z, float w) - { + 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) - { + 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) - { + 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); @@ -42,8 +36,7 @@ namespace LinearAlgebra float yawOver2 = yaw * 0.5f; float sinYawOver2 = (float)Math.Sin((float)yawOver2); float cosYawOver2 = (float)Math.Cos((float)yawOver2); - Quat32 result = new Quat32() - { + Quat32 result = new Quat32() { w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2, x = sinYawOver2 * cosPitchOver2 * cosRollOver2 + @@ -56,27 +49,23 @@ namespace LinearAlgebra return result; } - public void ToAngles(out float right, out float up, out float forward) - { + 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 + 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 + 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 - { + else { float sqx = this.x * this.x; float sqy = this.y * this.y; float sqz = this.z * this.z; diff --git a/src/Quaternion.cs b/src/Quaternion.cs index f481ef4..30f86c1 100644 --- a/src/Quaternion.cs +++ b/src/Quaternion.cs @@ -3,18 +3,15 @@ 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; diff --git a/src/Spherical.cs b/src/Spherical.cs index 497a508..ad6d9b8 100644 --- a/src/Spherical.cs +++ b/src/Spherical.cs @@ -3,42 +3,92 @@ using System; using Vector3Float = UnityEngine.Vector3; #endif -namespace LinearAlgebra -{ - public class Spherical - { - public float distance; - public Direction direction; +namespace LinearAlgebra { + /// + /// A spherical vector + /// + public class Spherical { - 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); + /// + /// Create a default vector with zero distance + /// + public Spherical() { + this.distance = 0; + this.direction = new Direction(); } - public Spherical(float distance, Direction 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; } - public static Spherical FromVector3(Vector3Float v) - { + /// + /// 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 new Spherical(distance, 0, 0); - else - { + 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 new Spherical(distance, horizontalAngle, verticalAngle); + return Spherical.Degrees(distance, horizontalAngle, verticalAngle); } } - public Vector3Float ToVector3() - { + 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); @@ -50,8 +100,34 @@ namespace LinearAlgebra float y = this.distance * cosVertical; float z = this.distance * sinVertical * cosHorizontal; - Vector3Float v = new Vector3Float(x, y, z); + 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 index a7a73da..8db0b1c 100644 --- a/src/SwingTwist.cs +++ b/src/SwingTwist.cs @@ -1,26 +1,21 @@ -namespace LinearAlgebra -{ +namespace LinearAlgebra { - public class SwingTwist - { + 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) - { + 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); + 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) - { + 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); diff --git a/src/Vector2.cs b/src/Vector2.cs index 603b0de..1840a7a 100644 --- a/src/Vector2.cs +++ b/src/Vector2.cs @@ -1,63 +1,50 @@ using System; using System.Numerics; -namespace 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; } } @@ -65,8 +52,7 @@ namespace LinearAlgebra /// /// 2-dimensional vectors /// - public struct Vector2 : IEquatable - { + public struct Vector2 : IEquatable { /// /// The right axis of the vector @@ -83,8 +69,7 @@ namespace 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; } @@ -129,10 +114,8 @@ namespace 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; } @@ -142,10 +125,8 @@ namespace 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; } @@ -155,10 +136,8 @@ namespace 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) @@ -173,8 +152,7 @@ namespace 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; } @@ -185,8 +163,7 @@ namespace 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; } @@ -197,8 +174,7 @@ namespace 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; } @@ -210,8 +186,7 @@ namespace 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; } @@ -223,8 +198,7 @@ namespace 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; } @@ -236,8 +210,7 @@ namespace 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; } @@ -254,8 +227,7 @@ namespace 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; @@ -271,8 +243,7 @@ namespace 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); } @@ -285,8 +256,7 @@ namespace 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); } @@ -294,8 +264,7 @@ namespace LinearAlgebra /// Get an hash code for the vector /// /// The hash code - public override int GetHashCode() - { + public override int GetHashCode() { return (x, y).GetHashCode(); } @@ -305,8 +274,7 @@ namespace 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); @@ -319,8 +287,7 @@ namespace 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; } @@ -334,8 +301,7 @@ namespace 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; } @@ -347,8 +313,7 @@ namespace 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; @@ -371,15 +336,13 @@ namespace 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) }; @@ -393,8 +356,7 @@ namespace 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 616b82e..7994dcb 100644 --- a/src/Vector3.cs +++ b/src/Vector3.cs @@ -1,16 +1,13 @@ #if !UNITY_5_3_OR_NEWER using System; -namespace 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; @@ -21,16 +18,13 @@ namespace 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); } } @@ -39,8 +33,7 @@ namespace 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 @@ -61,8 +54,7 @@ namespace 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; @@ -101,19 +93,15 @@ namespace LinearAlgebra /// public static readonly Vector3 forward = new Vector3(0, 1, 0); - public float magnitude - { - get - { - float d = (float)Math.Sqrt(x * x + y * y); + public readonly float magnitude { + get { + float d = (float)Math.Sqrt(x * x + y * y + z * z); return d; } } - public Vector3 normalized - { - get - { + public Vector3 normalized { + get { float l = magnitude; Vector3 v = zero; if (l > Float.epsilon) @@ -122,79 +110,66 @@ namespace 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 5d94014..4b58cdd 100644 --- a/src/float16.cs +++ b/src/float16.cs @@ -1,10 +1,8 @@ using System; -namespace LinearAlgebra -{ +namespace LinearAlgebra { - public class float16 - { + public class float16 { // // FILE: float16.cpp // AUTHOR: Rob Tillaart @@ -16,14 +14,12 @@ namespace 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); } @@ -157,8 +153,7 @@ namespace LinearAlgebra // // CORE CONVERSION // - float f16tof32(ushort _value) - { + float f16tof32(ushort _value) { //ushort sgn; ushort man; int exp; @@ -173,13 +168,11 @@ namespace 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 @@ -193,50 +186,43 @@ namespace 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 @@ -245,13 +231,11 @@ namespace 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 @@ -276,8 +260,7 @@ namespace 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); @@ -287,18 +270,15 @@ namespace 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 @@ -307,13 +287,11 @@ namespace 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/SphericalTest.cs b/test/SphericalTest.cs new file mode 100644 index 0000000..d9cca1d --- /dev/null +++ b/test/SphericalTest.cs @@ -0,0 +1,28 @@ +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)"); + } + } +} \ No newline at end of file