commit 7ecc78ee557798b58b68d1ed8bc3e3033e252ae3 Author: Pascal Serrarens Date: Wed Mar 12 14:52:14 2025 +0100 Migrated to RoboidControl diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..f0b2f47 --- /dev/null +++ b/.gitignore @@ -0,0 +1,6 @@ +DoxyGen/DoxyWarnLogfile.txt +.vscode/settings.json +**bin +**obj +**.meta +*.sln diff --git a/src/Angle.cs b/src/Angle.cs new file mode 100644 index 0000000..c3dd711 --- /dev/null +++ b/src/Angle.cs @@ -0,0 +1,117 @@ +using System; + +namespace LinearAlgebra +{ + + /// + /// %Angle utilities + /// + public static class Angle + { + public const float pi = 3.1415927410125732421875F; + // public static float Rad2Deg = 360.0f / ((float)Math.PI * 2); + // public static float Deg2Rad = ((float)Math.PI * 2) / 360.0f; + + public const float Rad2Deg = 360.0f / ((float)Math.PI * 2); //0.0174532924F; + public const float Deg2Rad = ((float)Math.PI * 2) / 360.0f; //57.29578F; + + /// + /// Clamp the angle between the given min and max values + /// + /// The angle to clamp + /// The minimum angle + /// The maximum angle + /// The clamped angle + /// Angles are normalized + public static float Clamp(float angle, float min, float max) + { + float normalizedAngle = Normalize(angle); + return Float.Clamp(normalizedAngle, min, max); + } + + /// + /// Determine the angle difference, result is a normalized angle + /// + /// First first angle + /// The second angle + /// the angle between the two angles + /// Angle values should be degrees + public static float Difference(float a, float b) + { + float r = Normalize(b - a); + return r; + } + + /// + /// Normalize an angle to the range -180 < angle <= 180 + /// + /// The angle to normalize + /// The normalized angle in interval (-180..180] + /// Angle values should be in degrees + public static float Normalize(float angle) + { + if (float.IsInfinity(angle)) + return angle; + + while (angle <= -180) angle += 360; + while (angle > 180) angle -= 360; + return angle; + } + + /// + /// Rotate from one angle to the other with a maximum degrees + /// + /// Starting angle + /// Target angle + /// Maximum angle to rotate + /// The resulting angle + /// This function is compatible with radian and degrees angles + public static float MoveTowards(float fromAngle, float toAngle, float maxAngle) + { + float d = toAngle - fromAngle; + d = Normalize(d); + d = Math.Sign(d) * Float.Clamp(Math.Abs(d), 0, maxAngle); + return fromAngle + d; + } + + /// + /// Map interval of angles between vectors [0..Pi] to interval [0..1] + /// + /// The first vector + /// The second vector + /// The resulting factor in interval [0..1] + /// Vectors a and b must be normalized + /// \deprecated Please use Vector2.ToFactor instead. + [Obsolete("Please use Vector2.ToFactor instead.")] + public static float ToFactor(Vector2 v1, Vector2 v2) + { + return (1 - Vector2.Dot(v1, v2)) / 2; + } + + // Normalize all vector angles to the range -180 < angle < 180 + //public static Vector3 Normalize(Vector3 angles) { + // float x = Normalize(angles.x); + // float y = Normalize(angles.y); + // float z = Normalize(angles.z); + // return new Vector3(x, y, z); + //} + + // Returns the signed angle in degrees between from and to. + //public static float SignedAngle(Vector3 from, Vector3 to) { + // float angle = Vector3.Angle(from, to); + // Vector3 cross = Vector3.Cross(from, to); + // if (cross.y < 0) angle = -angle; + // return angle; + //} + + // Returns the signed angle in degrees between from and to. + //public static float SignedAngle(Vector2 from, Vector2 to) { + // float sign = Math.Sign(from.y * to.x - from.x * to.y); + // return Vector2.Angle(from, to) * sign; + //} + + //public static Quaternion ToQuaternion(Rotation orientation) { + // return new Quaternion(orientation.x, orientation.y, orientation.z, orientation.w); + //} + } +} \ No newline at end of file diff --git a/src/Direction.cs b/src/Direction.cs new file mode 100644 index 0000000..d16ac72 --- /dev/null +++ b/src/Direction.cs @@ -0,0 +1,60 @@ +using System; +#if UNITY_5_3_OR_NEWER +using Vector3Float = UnityEngine.Vector3; +#endif + +namespace LinearAlgebra +{ + + public class Direction + { + public float horizontal; + public float vertical; + + public Direction() + { + horizontal = 0; + vertical = 0; + } + public Direction(float horizontal, float vertical) + { + this.horizontal = horizontal; + this.vertical = vertical; + //Normalize(); + } + + public readonly static Direction forward = new Direction(0, 0); + public readonly static Direction backward = new Direction(-180, 0); + public readonly static Direction up = new Direction(0, 90); + public readonly static Direction down = new Direction(0, -90); + public readonly static Direction left = new Direction(-90, 0); + public readonly static Direction right = new Direction(90, 0); + + public void Normalize() + { + if (this.vertical > 90 || this.vertical < -90) + { + this.horizontal += 180; + this.vertical = 180 - this.vertical; + } + } + + public Vector3Float ToVector3() + { + float verticalRad = (Angle.pi / 2) - this.vertical * Angle.Deg2Rad; + float horizontalRad = this.horizontal * Angle.Deg2Rad; + float cosVertical = (float)Math.Cos(verticalRad); + float sinVertical = (float)Math.Sin(verticalRad); + float cosHorizontal = (float)Math.Cos(horizontalRad); + float sinHorizontal = (float)Math.Sin(horizontalRad); + + float x = sinVertical * sinHorizontal; + float y = cosVertical; + float z = sinVertical * cosHorizontal; + + Vector3Float v = new(x, y, z); + return v; + } + } + +} \ No newline at end of file diff --git a/src/Float.cs b/src/Float.cs new file mode 100644 index 0000000..70873f0 --- /dev/null +++ b/src/Float.cs @@ -0,0 +1,45 @@ +namespace LinearAlgebra +{ + + /// + /// Float number utilities + /// + public class Float + { + /// + /// The precision of float numbers + /// + public const float epsilon = 1E-05f; + /// + /// The square of the float number precision + /// + public const float sqrEpsilon = 1e-10f; + + /// + /// Clamp the value between the given minimum and maximum values + /// + /// The value to clamp + /// The minimum value + /// The maximum value + /// The clamped value + public static float Clamp(float f, float min, float max) + { + if (f < min) + return min; + if (f > max) + return max; + return f; + } + + /// + /// Clamp the value between to the interval [0..1] + /// + /// The value to clamp + /// The clamped value + public static float Clamp01(float f) + { + return Clamp(f, 0, 1); + } + } + +} \ No newline at end of file diff --git a/src/LinearAlgebra.csproj b/src/LinearAlgebra.csproj new file mode 100644 index 0000000..14d3947 --- /dev/null +++ b/src/LinearAlgebra.csproj @@ -0,0 +1,14 @@ + + + + false + false + net5.0 + + + + + + + + diff --git a/src/Matrix.cs b/src/Matrix.cs new file mode 100644 index 0000000..1f0542e --- /dev/null +++ b/src/Matrix.cs @@ -0,0 +1,645 @@ +using System; +#if UNITY_5_3_OR_NEWER +using Vector3Float = UnityEngine.Vector3; +using Vector2Float = UnityEngine.Vector2; +using Quaternion = UnityEngine.Quaternion; +#endif + +namespace LinearAlgebra +{ + + public readonly struct Slice + { + public uint start { get; } + public uint stop { get; } + public Slice(uint start, uint stop) + { + this.start = start; + this.stop = stop; + } + } + + public class Matrix2 + { + public float[,] data { get; } + + public uint nRows => (uint)data.GetLength(0); + public uint nCols => (uint)data.GetLength(1); + + public Matrix2(uint nRows, uint nCols) + { + this.data = new float[nRows, nCols]; + } + public Matrix2(float[,] data) + { + this.data = data; + } + + public Matrix2 Clone() + { + float[,] data = new float[this.nRows, nCols]; + for (int rowIx = 0; rowIx < this.nRows; rowIx++) + { + for (int colIx = 0; colIx < this.nCols; colIx++) + data[rowIx, colIx] = this.data[rowIx, colIx]; + } + return new Matrix2(data); + } + + public static Matrix2 Zero(uint nRows, uint nCols) + { + return new Matrix2(nRows, nCols); + } + + public static Matrix2 FromVector3(Vector3Float v) + { + float[,] result = new float[3, 1]; + result[0, 0] = v.x; + result[1, 0] = v.y; + result[2, 0] = v.z; + return new Matrix2(result); + } + + public static Matrix2 Identity(uint size) + { + return Diagonal(1, size); + } + public static Matrix2 Identity(uint nRows, uint nCols) + { + Matrix2 m = Zero(nRows, nCols); + m.FillDiagonal(1); + return m; + } + + public static Matrix2 Diagonal(Matrix1 v) + { + float[,] resultData = new float[v.size, v.size]; + for (int ix = 0; ix < v.size; ix++) + resultData[ix, ix] = v.data[ix]; + return new Matrix2(resultData); + } + public static Matrix2 Diagonal(float f, uint size) + { + float[,] resultData = new float[size, size]; + for (int ix = 0; ix < size; ix++) + resultData[ix, ix] = f; + return new Matrix2(resultData); + } + public void FillDiagonal(Matrix1 v) + { + uint n = Math.Min(Math.Min(this.nRows, this.nCols), v.size); + for (int ix = 0; ix < n; ix++) + this.data[ix, ix] = v.data[ix]; + } + public void FillDiagonal(float f) + { + uint n = Math.Min(this.nRows, this.nCols); + for (int ix = 0; ix < n; ix++) + this.data[ix, ix] = f; + } + + public static Matrix2 SkewMatrix(Vector3Float v) + { + float[,] result = new float[3, 3] { + {0, -v.z, v.y}, + {v.z, 0, -v.x}, + {-v.y, v.x, 0} + }; + return new Matrix2(result); + } + +#if UNITY_5_3_OR_NEWER + public Vector3Float GetRow3(int rowIx) { + uint cols = this.nCols; + Vector3Float row = new() { + x = this.data[rowIx, 0], + y = this.data[rowIx, 1], + z = this.data[rowIx, 2] + }; + return row; + } +#endif + public void SetRow(int rowIx, Matrix1 v) + { + for (uint ix = 0; ix < v.size; ix++) + this.data[rowIx, ix] = v.data[ix]; + } + public void SetRow3(int rowIx, Vector3Float v) + { + this.data[rowIx, 0] = v.x; + this.data[rowIx, 1] = v.y; + this.data[rowIx, 2] = v.z; + } + + public Matrix1 GetColumn(int colIx) + { + float[] column = new float[this.nRows]; + for (int i = 0; i < this.nRows; i++) + { + column[i] = this.data[i, colIx]; + } + return new Matrix1(column); + } + + public static bool AllClose(Matrix2 A, Matrix2 B, float atol = 1e-08f) + { + for (int i = 0; i < A.nRows; i++) + { + for (int j = 0; j < A.nCols; j++) + { + float d = MathF.Abs(A.data[i, j] - B.data[i, j]); + if (d > atol) + return false; + } + } + return true; + } + + public Matrix2 Transpose() + { + float[,] resultData = new float[this.nCols, this.nRows]; + for (uint rowIx = 0; rowIx < this.nRows; rowIx++) + { + for (uint colIx = 0; colIx < this.nCols; colIx++) + resultData[colIx, rowIx] = this.data[rowIx, colIx]; + } + return new Matrix2(resultData); + // double checked code + } + public Matrix2 transposed + { + get => Transpose(); + } + + public static Matrix2 operator -(Matrix2 m) + { + float[,] result = new float[m.nRows, m.nCols]; + + for (int i = 0; i < m.nRows; i++) + { + for (int j = 0; j < m.nCols; j++) + result[i, j] = -m.data[i, j]; + } + return new Matrix2(result); + } + + public static Matrix2 operator -(Matrix2 A, Matrix2 B) + { + if (A.nRows != B.nRows || A.nCols != B.nCols) + throw new System.ArgumentException("Size of A must match size of B."); + + float[,] result = new float[A.nRows, B.nCols]; + + for (int i = 0; i < A.nRows; i++) + { + for (int j = 0; j < A.nCols; j++) + result[i, j] = A.data[i, j] - B.data[i, j]; + } + return new Matrix2(result); + } + + public static Matrix2 operator +(Matrix2 A, Matrix2 B) + { + if (A.nRows != B.nRows || A.nCols != B.nCols) + throw new System.ArgumentException("Size of A must match size of B."); + + float[,] result = new float[A.nRows, B.nCols]; + + for (int i = 0; i < A.nRows; i++) + { + for (int j = 0; j < A.nCols; j++) + result[i, j] = A.data[i, j] + B.data[i, j]; + } + return new Matrix2(result); + } + + public static Matrix2 operator *(Matrix2 A, Matrix2 B) + { + if (A.nCols != B.nRows) + throw new System.ArgumentException("Number of columns in A must match number of rows in B."); + + float[,] result = new float[A.nRows, B.nCols]; + + for (int i = 0; i < A.nRows; i++) + { + for (int j = 0; j < B.nCols; j++) + { + float sum = 0.0f; + for (int k = 0; k < A.nCols; k++) + sum += A.data[i, k] * B.data[k, j]; + + result[i, j] = sum; + } + } + + return new Matrix2(result); + // double checked code + } + + public static Matrix1 operator *(Matrix2 A, Matrix1 v) + { + float[] result = new float[A.nRows]; + + for (int i = 0; i < A.nRows; i++) + { + for (int j = 0; j < A.nCols; j++) + { + result[i] += A.data[i, j] * v.data[j]; + } + } + + return new Matrix1(result); + } + + public static Vector3Float operator *(Matrix2 A, Vector3Float v) + { + return new Vector3Float( + A.data[0, 0] * v.x + A.data[0, 1] * v.y + A.data[0, 2] * v.z, + A.data[1, 0] * v.x + A.data[1, 1] * v.y + A.data[1, 2] * v.z, + A.data[2, 0] * v.x + A.data[2, 1] * v.y + A.data[2, 2] * v.z + ); + } + + public static Matrix2 operator *(Matrix2 A, float s) + { + float[,] result = new float[A.nRows, A.nCols]; + + for (int i = 0; i < A.nRows; i++) + { + for (int j = 0; j < A.nCols; j++) + result[i, j] = A.data[i, j] * s; + } + + return new Matrix2(result); + } + public static Matrix2 operator *(float s, Matrix2 A) + { + return A * s; + } + + public static Matrix2 operator /(Matrix2 A, float s) + { + float[,] result = new float[A.nRows, A.nCols]; + + for (int i = 0; i < A.nRows; i++) + { + for (int j = 0; j < A.nCols; j++) + result[i, j] = A.data[i, j] / s; + } + + return new Matrix2(result); + } + public static Matrix2 operator /(float s, Matrix2 A) + { + float[,] result = new float[A.nRows, A.nCols]; + + for (int i = 0; i < A.nRows; i++) + { + for (int j = 0; j < A.nCols; j++) + result[i, j] = s / A.data[i, j]; + } + + return new Matrix2(result); + } + + public Matrix2 Slice(Slice slice) + { + return Slice(slice.start, slice.stop); + } + public Matrix2 Slice(uint from, uint to) + { + if (from < 0 || to >= this.nRows) + throw new System.ArgumentException("Slice index out of range."); + + float[,] result = new float[to - from, this.nCols]; + int resultRowIx = 0; + for (uint rowIx = from; rowIx < to; rowIx++) + { + for (int colIx = 0; colIx < this.nCols; colIx++) + { + result[resultRowIx, colIx] = this.data[rowIx, colIx]; + } + resultRowIx++; + } + + return new Matrix2(result); + } + public Matrix2 Slice(Slice rowRange, Slice colRange) + { + return Slice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop)); + } + + public Matrix2 Slice((uint start, uint stop) rowRange, (uint start, uint stop) colRange) + { + float[,] result = new float[rowRange.stop - rowRange.start, colRange.stop - colRange.start]; + + uint resultRowIx = 0; + uint resultColIx = 0; + for (uint i = rowRange.start; i < rowRange.stop; i++) + { + for (uint j = colRange.start; j < colRange.stop; j++) + result[resultRowIx, resultColIx] = this.data[i, j]; + } + return new Matrix2(result); + } + + public void UpdateSlice(Slice slice, Matrix2 m) + { + int mRowIx = 0; + for (uint rowIx = slice.start; rowIx < slice.stop; rowIx++, mRowIx++) + { + for (int colIx = 0; colIx < this.nCols; colIx++) + this.data[rowIx, colIx] = m.data[mRowIx, colIx]; + } + } + public void UpdateSlice(Slice rowRange, Slice colRange, Matrix2 m) + { + UpdateSlice((rowRange.start, rowRange.stop), (colRange.start, colRange.stop), m); + } + public void UpdateSlice((uint start, uint stop) rowRange, (uint start, uint stop) colRange, Matrix2 m) + { + for (uint i = rowRange.start; i < rowRange.stop; i++) + { + for (uint j = colRange.start; j < colRange.stop; j++) + this.data[i, j] = m.data[i - rowRange.start, j - colRange.start]; + } + } + + public Matrix2 Inverse() + { + Matrix2 A = this; + // unchecked + uint n = A.nRows; + + // Create an identity matrix of the same size as the original matrix + float[,] augmentedMatrix = new float[n, 2 * n]; + for (int i = 0; i < n; i++) + { + for (int j = 0; j < n; j++) + { + augmentedMatrix[i, j] = A.data[i, j]; + augmentedMatrix[i, j + n] = (i == j) ? 1 : 0; // Identity matrix + } + } + + // Perform Gaussian elimination + for (int i = 0; i < n; i++) + { + // Find the pivot row + float pivot = augmentedMatrix[i, i]; + if (Math.Abs(pivot) < 1e-10) // Check for singular matrix + throw new InvalidOperationException("Matrix is singular and cannot be inverted."); + + // Normalize the pivot row + for (int j = 0; j < 2 * n; j++) + augmentedMatrix[i, j] /= pivot; + + // Eliminate the column below the pivot + for (int j = i + 1; j < n; j++) + { + float factor = augmentedMatrix[j, i]; + for (int k = 0; k < 2 * n; k++) + augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k]; + } + } + + // Back substitution + for (uint i = n - 1; i >= 0; i--) + { + // Eliminate the column above the pivot + for (uint j = i - 1; j >= 0; j--) + { + float factor = augmentedMatrix[j, i]; + for (int k = 0; k < 2 * n; k++) + augmentedMatrix[j, k] -= factor * augmentedMatrix[i, k]; + } + } + + // Extract the inverse matrix from the augmented matrix + float[,] inverse = new float[n, n]; + for (int i = 0; i < n; i++) + { + for (int j = 0; j < n; j++) + inverse[i, j] = augmentedMatrix[i, j + n]; + } + + return new Matrix2(inverse); + } + + public float Determinant() + { + uint n = this.nRows; + if (n != this.nCols) + throw new System.ArgumentException("Matrix must be square."); + + if (n == 1) + return this.data[0, 0]; // Base case for 1x1 matrix + + if (n == 2) // Base case for 2x2 matrix + return this.data[0, 0] * this.data[1, 1] - this.data[0, 1] * this.data[1, 0]; + + float det = 0; + for (int col = 0; col < n; col++) + det += (col % 2 == 0 ? 1 : -1) * this.data[0, col] * this.Minor(0, col).Determinant(); + + return det; + } + + // Helper function to compute the minor of a matrix + private Matrix2 Minor(int rowToRemove, int colToRemove) + { + uint n = this.nRows; + float[,] minor = new float[n - 1, n - 1]; + + int r = 0, c = 0; + for (int i = 0; i < n; i++) + { + if (i == rowToRemove) continue; + + c = 0; + for (int j = 0; j < n; j++) + { + if (j == colToRemove) continue; + + minor[r, c] = this.data[i, j]; + c++; + } + r++; + } + + return new Matrix2(minor); + } + } + + public class Matrix1 + { + public float[] data { get; } + + public uint size => (uint)data.GetLength(0); + + public Matrix1(uint size) + { + this.data = new float[size]; + } + + public Matrix1(float[] data) + { + this.data = data; + } + + public static Matrix1 Zero(uint size) + { + return new Matrix1(size); + } + + public static Matrix1 FromVector2(Vector2Float v) + { + float[] result = new float[2]; + result[0] = v.x; + result[1] = v.y; + return new Matrix1(result); + } + + public static Matrix1 FromVector3(Vector3Float v) + { + float[] result = new float[3]; + result[0] = v.x; + result[1] = v.y; + result[2] = v.z; + return new Matrix1(result); + } + +#if UNITY_5_3_OR_NEWER + public static Matrix1 FromQuaternion(Quaternion q) { + float[] result = new float[4]; + result[0] = q.x; + result[1] = q.y; + result[2] = q.z; + result[3] = q.w; + return new Matrix1(result); + } +#endif + + public Vector2Float vector2 + { + get + { + if (this.size != 2) + throw new System.ArgumentException("Matrix1 must be of size 2"); + return new Vector2Float(this.data[0], this.data[1]); + } + } + public Vector3Float vector3 + { + get + { + if (this.size != 3) + throw new System.ArgumentException("Matrix1 must be of size 3"); + return new Vector3Float(this.data[0], this.data[1], this.data[2]); + } + } + +#if UNITY_5_3_OR_NEWER + public Quaternion quaternion { + get { + if (this.size != 4) + throw new System.ArgumentException("Matrix1 must be of size 4"); + return new Quaternion(this.data[0], this.data[1], this.data[2], this.data[3]); + } + } +#endif + + public Matrix1 Clone() + { + float[] data = new float[this.size]; + for (int rowIx = 0; rowIx < this.size; rowIx++) + data[rowIx] = this.data[rowIx]; + return new Matrix1(data); + } + + + public float magnitude + { + get + { + float sum = 0; + foreach (var elm in data) + sum += elm; + return sum / data.Length; + } + } + public static Matrix1 operator +(Matrix1 A, Matrix1 B) + { + if (A.size != B.size) + throw new System.ArgumentException("Size of A must match size of B."); + + float[] result = new float[A.size]; + + for (int i = 0; i < A.size; i++) + { + result[i] = A.data[i] + B.data[i]; + } + return new Matrix1(result); + } + + public Matrix2 Transpose() + { + float[,] r = new float[1, this.size]; + for (uint colIx = 0; colIx < this.size; colIx++) + r[1, colIx] = this.data[colIx]; + + return new Matrix2(r); + } + + public static float Dot(Matrix1 a, Matrix1 b) + { + if (a.size != b.size) + throw new System.ArgumentException("Vectors must be of the same length."); + + float result = 0.0f; + for (int i = 0; i < a.size; i++) + { + result += a.data[i] * b.data[i]; + } + return result; + } + + public static Matrix1 operator *(Matrix1 A, float f) + { + float[] result = new float[A.size]; + + for (int i = 0; i < A.size; i++) + result[i] += A.data[i] * f; + + return new Matrix1(result); + } + public static Matrix1 operator *(float f, Matrix1 A) + { + return A * f; + } + + public Matrix1 Slice(Slice range) + { + return Slice(range.start, range.stop); + } + public Matrix1 Slice(uint from, uint to) + { + if (from < 0 || to >= this.size) + throw new System.ArgumentException("Slice index out of range."); + + float[] result = new float[to - from]; + int resultIx = 0; + for (uint ix = from; ix < to; ix++) + result[resultIx++] = this.data[ix]; + + return new Matrix1(result); + } + public void UpdateSlice(Slice slice, Matrix1 v) + { + int vIx = 0; + for (uint ix = slice.start; ix < slice.stop; ix++, vIx++) + this.data[ix] = v.data[vIx]; + } + } + +} \ No newline at end of file diff --git a/src/Quat32.cs b/src/Quat32.cs new file mode 100644 index 0000000..3026f8e --- /dev/null +++ b/src/Quat32.cs @@ -0,0 +1,98 @@ +using System; + +namespace LinearAlgebra +{ + public class Quat32 + { + public float x; + public float y; + public float z; + public float w; + + public Quat32() + { + this.x = 0; + this.y = 0; + this.z = 0; + this.w = 1; + } + + public Quat32(float x, float y, float z, float w) + { + this.x = x; + this.y = y; + this.z = z; + this.w = w; + } + + public static Quat32 FromSwingTwist(SwingTwist s) + { + Quat32 q32 = Quat32.Euler(-s.swing.vertical, s.swing.horizontal, s.twist); + return q32; + } + + public static Quat32 Euler(float yaw, float pitch, float roll) + { + float rollOver2 = roll * Angle.Deg2Rad * 0.5f; + float sinRollOver2 = (float)Math.Sin((float)rollOver2); + float cosRollOver2 = (float)Math.Cos((float)rollOver2); + float pitchOver2 = pitch * 0.5f; + float sinPitchOver2 = (float)Math.Sin((float)pitchOver2); + float cosPitchOver2 = (float)Math.Cos((float)pitchOver2); + float yawOver2 = yaw * 0.5f; + float sinYawOver2 = (float)Math.Sin((float)yawOver2); + float cosYawOver2 = (float)Math.Cos((float)yawOver2); + Quat32 result = new Quat32() + { + w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + + sinYawOver2 * sinPitchOver2 * sinRollOver2, + x = sinYawOver2 * cosPitchOver2 * cosRollOver2 + + cosYawOver2 * sinPitchOver2 * sinRollOver2, + y = cosYawOver2 * sinPitchOver2 * cosRollOver2 - + sinYawOver2 * cosPitchOver2 * sinRollOver2, + z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - + sinYawOver2 * sinPitchOver2 * cosRollOver2 + }; + return result; + } + + public void ToAngles(out float right, out float up, out float forward) + { + float test = this.x * this.y + this.z * this.w; + if (test > 0.499f) + { // singularity at north pole + right = 0; + up = 2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg; + forward = 90; + return; + //return Vector3(0, 2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, 90); + } + else if (test < -0.499f) + { // singularity at south pole + right = 0; + up = -2 * (float)Math.Atan2(this.x, this.w) * Angle.Rad2Deg; + forward = -90; + return; + //return Vector3(0, -2 * (float)atan2(this.x, this.w) * Angle.Rad2Deg, -90); + } + else + { + float sqx = this.x * this.x; + float sqy = this.y * this.y; + float sqz = this.z * this.z; + + right = (float)Math.Atan2(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * Angle.Rad2Deg; + up = (float)Math.Atan2(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * Angle.Rad2Deg; + forward = (float)Math.Asin(2 * test) * Angle.Rad2Deg; + return; + // return Vector3( + // atan2f(2 * this.x * this.w - 2 * this.y * this.z, 1 - 2 * sqx - 2 * sqz) * + // Rad2Deg, + // atan2f(2 * this.y * this.w - 2 * this.x * this.z, 1 - 2 * sqy - 2 * sqz) * + // Rad2Deg, + // asinf(2 * test) * Angle.Rad2Deg); + } + } + + } +} \ No newline at end of file diff --git a/src/Quaternion.cs b/src/Quaternion.cs new file mode 100644 index 0000000..f481ef4 --- /dev/null +++ b/src/Quaternion.cs @@ -0,0 +1,80 @@ +using System; +#if UNITY_5_3_OR_NEWER +using Quaternion = UnityEngine.Quaternion; +#endif + +namespace LinearAlgebra +{ + + public class QuaternionOf + { + public T x; + public T y; + public T z; + public T w; + + public QuaternionOf(T x, T y, T z, T w) + { + this.x = x; + this.y = y; + this.z = z; + this.w = w; + } + +#if UNITY_5_3_OR_NEWER + public static Matrix2 ToRotationMatrix(Quaternion q) { + float w = q.x, x = q.y, y = q.z, z = q.w; + + float[,] result = new float[,] + { + { 1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y) }, + { 2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x) }, + { 2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y) } + }; + return new Matrix2(result); + } + + public static Quaternion FromRotationMatrix(Matrix2 m) { + float trace = m.data[0, 0] + m.data[1, 1] + m.data[2, 2]; + float w, x, y, z; + + if (trace > 0) { + float s = 0.5f / (float)Math.Sqrt(trace + 1.0f); + w = 0.25f / s; + x = (m.data[2, 1] - m.data[1, 2]) * s; + y = (m.data[0, 2] - m.data[2, 0]) * s; + z = (m.data[1, 0] - m.data[0, 1]) * s; + } + else { + if (m.data[0, 0] > m.data[1, 1] && m.data[0, 0] > m.data[2, 2]) { + float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[0, 0] - m.data[1, 1] - m.data[2, 2]); + w = (m.data[2, 1] - m.data[1, 2]) / s; + x = 0.25f * s; + y = (m.data[0, 1] + m.data[1, 0]) / s; + z = (m.data[0, 2] + m.data[2, 0]) / s; + } + else if (m.data[1, 1] > m.data[2, 2]) { + float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[1, 1] - m.data[0, 0] - m.data[2, 2]); + w = (m.data[0, 2] - m.data[2, 0]) / s; + x = (m.data[0, 1] + m.data[1, 0]) / s; + y = 0.25f * s; + z = (m.data[1, 2] + m.data[2, 1]) / s; + } + else { + float s = 2.0f * (float)Math.Sqrt(1.0f + m.data[2, 2] - m.data[0, 0] - m.data[1, 1]); + w = (m.data[1, 0] - m.data[0, 1]) / s; + x = (m.data[0, 2] + m.data[2, 0]) / s; + y = (m.data[1, 2] + m.data[2, 1]) / s; + z = 0.25f * s; + } + } + + return new Quaternion(x, y, z, w); + } +#endif + } + + // public class Quaternion : QuaternionOf { + // public Quaternion(float x, float y, float z, float w) : base(x, y, z, w) { } + // } +} \ No newline at end of file diff --git a/src/Spherical.cs b/src/Spherical.cs new file mode 100644 index 0000000..497a508 --- /dev/null +++ b/src/Spherical.cs @@ -0,0 +1,57 @@ +using System; +#if UNITY_5_3_OR_NEWER +using Vector3Float = UnityEngine.Vector3; +#endif + +namespace LinearAlgebra +{ + public class Spherical + { + public float distance; + public Direction direction; + + public static Spherical zero = new Spherical(0, 0, 0); + public static Spherical forward = new Spherical(1, 0, 0); + + public Spherical(float distance, float horizontal, float vertical) + { + this.distance = distance; + this.direction = new Direction(horizontal, vertical); + } + public Spherical(float distance, Direction direction) + { + this.distance = distance; + this.direction = direction; + } + + public static Spherical FromVector3(Vector3Float v) + { + float distance = v.magnitude; + if (distance == 0.0f) + return new Spherical(distance, 0, 0); + else + { + float verticalAngle = (float)((Angle.pi / 2 - Math.Acos(v.y / distance)) * Angle.Rad2Deg); + float horizontalAngle = (float)Math.Atan2(v.x, v.z) * Angle.Rad2Deg; + return new Spherical(distance, horizontalAngle, verticalAngle); + } + } + + public Vector3Float ToVector3() + { + float verticalRad = (Angle.pi / 2) - this.direction.vertical * Angle.Deg2Rad; + float horizontalRad = this.direction.horizontal * Angle.Deg2Rad; + float cosVertical = (float)Math.Cos(verticalRad); + float sinVertical = (float)Math.Sin(verticalRad); + float cosHorizontal = (float)Math.Cos(horizontalRad); + float sinHorizontal = (float)Math.Sin(horizontalRad); + + float x = this.distance * sinVertical * sinHorizontal; + float y = this.distance * cosVertical; + float z = this.distance * sinVertical * cosHorizontal; + + Vector3Float v = new Vector3Float(x, y, z); + return v; + } + } +} \ No newline at end of file diff --git a/src/SwingTwist.cs b/src/SwingTwist.cs new file mode 100644 index 0000000..acf8978 --- /dev/null +++ b/src/SwingTwist.cs @@ -0,0 +1,46 @@ +using System.Numerics; +#if UNITY_5_3_OR_NEWER +using Quaternion = UnityEngine.Quaternion; +#endif + +namespace LinearAlgebra +{ + + public class SwingTwist + { + public Direction swing; + public float twist; + + public static readonly SwingTwist zero = new SwingTwist(0, 0, 0); + + public SwingTwist(Direction swing, float twist) + { + this.swing = swing; + this.twist = twist; + } + public SwingTwist(float horizontalSwing, float verticalSwing, float twist) + { + this.swing = new Direction(horizontalSwing, verticalSwing); + this.swing.Normalize(); + this.twist = twist; + } + public static SwingTwist FromQuat32(Quat32 q32) + { + // UnityEngine.Quaternion q = new(q32.x, q32.y, q32.z, q32.w); + // SwingTwist r = new(q.eulerAngles.y, q.eulerAngles.x, q.eulerAngles.z); + q32.ToAngles(out float right, out float up, out float forward); + SwingTwist r = new SwingTwist(up, right, forward); + return r; + } + +#if UNITY_5_3_OR_NEWER + public Quaternion ToQuaternion() { + Quaternion q = Quaternion.Euler(-this.swing.vertical, + this.swing.horizontal, + this.twist); + return q; + } +#endif + } + +} \ No newline at end of file diff --git a/src/Vector2.cs b/src/Vector2.cs new file mode 100644 index 0000000..603b0de --- /dev/null +++ b/src/Vector2.cs @@ -0,0 +1,401 @@ +using System; +using System.Numerics; + +namespace LinearAlgebra +{ + + public class Vector2Of where T : IComparable + { + public T x; + public T y; + + public Vector2Of(T x, T y) + { + this.x = x; + this.y = y; + } + } + + public class Vector2Int : Vector2Of + { + public Vector2Int(int x, int y) : base(x, y) { } + + public static Vector2Int operator -(Vector2Int v1, Vector2Int v2) + { + return new Vector2Int(v1.x - v2.x, v1.y - v2.y); + } + + public float magnitude + { + get + { + return (float)Math.Sqrt(this.x * this.x + this.y * this.y); + } + } + + public static float Distance(Vector2Int v1, Vector2Int v2) + { + return (v1 - v2).magnitude; + } + } + + public class Vector2Float : Vector2Of + { + public Vector2Float(float x, float y) : base(x, y) { } + + public static Vector2Float operator -(Vector2Float v1, Vector2Float v2) + { + return new Vector2Float(v1.x - v2.x, v1.y - v2.y); + } + + public float magnitude + { + get + { + return (float)Math.Sqrt(this.x * this.x + this.y * this.y); + } + } + + public static float Distance(Vector2Float v1, Vector2Float v2) + { + return (v1 - v2).magnitude; + } + } + + /// + /// 2-dimensional vectors + /// + public struct Vector2 : IEquatable + { + + /// + /// The right axis of the vector + /// + public float x; // left/right + /// + /// The upward/forward axis of the vector + /// + public float y; // forward/backward + // directions are to be inline with Vector3 as much as possible... + + /// + /// Create a new 2-dimensional vector + /// + /// x axis value + /// y axis value + public Vector2(float x, float y) + { + this.x = x; + this.y = y; + } + + /// + /// A vector with zero for all axis + /// + public static readonly Vector2 zero = new Vector2(0, 0); + /// + /// A vector with values (1, 1) + /// + public static readonly Vector2 one = new Vector2(1, 1); + /// + /// A vector with values (0, 1) + /// + public static readonly Vector2 up = new Vector2(0, 1); + /// + /// A vector with values (0, -1) + /// + public static readonly Vector2 down = new Vector2(0, -1); + /// + /// A vector with values (0, 1) + /// + public static readonly Vector2 forward = new Vector2(0, 1); + /// + /// A vector with values (0, -1) + /// + public static readonly Vector2 back = new Vector2(0, -1); + /// + /// A vector3 with values (-1, 0) + /// + public static readonly Vector2 left = new Vector2(-1, 0); + /// + /// A vector with values (1, 0) + /// + public static readonly Vector2 right = new Vector2(1, 0); + + /// + /// The squared length of this vector + /// + /// The squared length + /// The squared length is computationally simpler than the real length. + /// Think of Pythagoras A^2 + B^2 = C^2. + /// This leaves out the calculation of the squared root of C. + public float sqrMagnitude + { + get + { + float d = x * x + y * y; + return d; + } + } + + /// + /// The length of this vector + /// + /// The length of this vector + public float magnitude + { + get + { + float d = (float)Math.Sqrt(x * x + y * y); + return d; + } + } + + /// + /// Convert the vector to a length of a 1 + /// + /// The vector with length 1 + public Vector2 normalized + { + get + { + float l = magnitude; + Vector2 v = zero; + if (l > Float.epsilon) + v = this / l; + return v; + } + } + + /// + /// Add two vectors + /// + /// The first vector + /// The second vector + /// The result of adding the two vectors + public static Vector2 operator +(Vector2 v1, Vector2 v2) + { + Vector2 v = new Vector2(v1.x + v2.x, v1.y + v2.y); + return v; + } + + /// + /// Subtract two vectors + /// + /// The first vector + /// The second vector + /// The result of adding the two vectors + public static Vector2 operator -(Vector2 v1, Vector2 v2) + { + Vector2 v = new Vector2(v1.x - v2.x, v1.y - v2.y); + return v; + } + + /// + /// Negate the vector + /// + /// The vector to negate + /// The negated vector + /// This will result in a vector pointing in the opposite direction + public static Vector2 operator -(Vector2 v1) + { + Vector2 v = new Vector2(-v1.x, -v1.y); + return v; + } + + /// + /// Scale a vector uniformly up + /// + /// The vector to scale + /// The scaling factor + /// The scaled vector + /// Each component of the vector will be multipled with the same factor. + public static Vector2 operator *(Vector2 v1, float f) + { + Vector2 v = new Vector2(v1.x * f, v1.y * f); + return v; + } + + /// + /// Scale a vector uniformly up + /// + /// The scaling factor + /// The vector to scale + /// The scaled vector + /// Each component of the vector will be multipled with the same factor. + public static Vector2 operator *(float f, Vector2 v1) + { + Vector2 v = new Vector2(f * v1.x, f * v1.y); + return v; + } + + /// + /// Scale a vector uniformly down + /// + /// The vector to scale + /// The scaling factor + /// The scaled vector + /// Each component of the vector will be devided by the same factor. + public static Vector2 operator /(Vector2 v1, float f) + { + Vector2 v = new Vector2(v1.x / f, v1.y / f); + return v; + } + + /// + /// Tests if the vector has equal values as the given vector + /// + /// The vector to compare to + /// true if the vector values are equal + public bool Equals(Vector2 v1) => x == v1.x && y == v1.y; + + /// + /// Tests if the vector is equal to the given object + /// + /// The object to compare to + /// false when the object is not a Vector2 or does not have equal values + public override bool Equals(object obj) + { + if (!(obj is Vector2 v)) + return false; + + return (x == v.x && y == v.y); + } + + /// + /// Tests if the two vectors have equal values + /// + /// The first vector + /// The second vector + /// truewhen the vectors have equal values + /// Note that this uses a Float equality check which cannot be not exact in all cases. + /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon + /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon + public static bool operator ==(Vector2 v1, Vector2 v2) + { + return (v1.x == v2.x && v1.y == v2.y); + } + + /// + /// Tests if two vectors have different values + /// + /// The first vector + /// The second vector + /// truewhen the vectors have different values + /// Note that this uses a Float equality check which cannot be not exact in all case. + /// In most cases it is better to check if the Vector2.Distance between the vectors is smaller than Float.epsilon. + /// Or more efficient: (v1 - v2).sqrMagnitude < Float.sqrEpsilon + public static bool operator !=(Vector2 v1, Vector2 v2) + { + return (v1.x != v2.x || v1.y != v2.y); + } + + /// + /// Get an hash code for the vector + /// + /// The hash code + public override int GetHashCode() + { + return (x, y).GetHashCode(); + } + + /// + /// Get the distance between two vectors + /// + /// The first vector + /// The second vector + /// The distance between the two vectors + public static float Distance(Vector2 v1, Vector2 v2) + { + float x = v1.x - v2.x; + float y = v1.y - v2.y; + float d = (float)Math.Sqrt(x * x + y * y); + return d; + } + + /// + /// The dot product of two vectors + /// + /// The first vector + /// The second vector + /// The dot product of the two vectors + public static float Dot(Vector2 v1, Vector2 v2) + { + return v1.x * v2.x + v1.y * v2.y; + } + + /// + /// Lerp between two vectors + /// + /// The from vector + /// The to vector + /// The interpolation distance [0..1] + /// The lerped vector + /// The factor f is unclamped. Value 0 matches the *v1* vector, Value 1 + /// matches the *v2* vector Value -1 is *v1* vector minus the difference + /// between *v1* and *v2* etc. + public static Vector2 Lerp(Vector2 v1, Vector2 v2, float f) + { + Vector2 v = v1 + (v2 - v1) * f; + return v; + } + + /// + /// Calculate the signed angle between two vectors. + /// + /// The starting vector + /// The ending vector + /// The axis to rotate around + /// The signed angle in degrees + public static float SignedAngle(Vector2 from, Vector2 to) + { + //float sign = Math.Sign(v1.y * v2.x - v1.x * v2.y); + //return Vector2.Angle(v1, v2) * sign; + + float sqrMagFrom = from.sqrMagnitude; + float sqrMagTo = to.sqrMagnitude; + + if (sqrMagFrom == 0 || sqrMagTo == 0) + return 0; + //if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo)) + // return nanf(""); + + float angleFrom = (float)Math.Atan2(from.y, from.x); + float angleTo = (float)Math.Atan2(to.y, to.x); + return (angleTo - angleFrom) * Angle.Rad2Deg; + } + + /// + /// Rotates the vector with the given angle + /// + /// The vector to rotate + /// The angle in degrees + /// + public static Vector2 Rotate(Vector2 v1, float angle) + { + float sin = (float)Math.Sin(angle * Angle.Deg2Rad); + float cos = (float)Math.Cos(angle * Angle.Deg2Rad); + + float tx = v1.x; + float ty = v1.y; + Vector2 v = new Vector2() + { + x = (cos * tx) - (sin * ty), + y = (sin * tx) + (cos * ty) + }; + return v; + } + + /// + /// Map interval of angles between vectors [0..Pi] to interval [0..1] + /// + /// The first vector + /// The second vector + /// The resulting factor in interval [0..1] + /// Vectors a and b must be normalized + public static float ToFactor(Vector2 v1, Vector2 v2) + { + return (1 - Vector2.Dot(v1, v2)) / 2; + } + } +} \ No newline at end of file diff --git a/src/Vector3.cs b/src/Vector3.cs new file mode 100644 index 0000000..616b82e --- /dev/null +++ b/src/Vector3.cs @@ -0,0 +1,204 @@ +#if !UNITY_5_3_OR_NEWER +using System; + +namespace LinearAlgebra +{ + public class Vector3Of + { + public T x; + public T y; + public T z; + + public Vector3Of(T x, T y, T z) + { + this.x = x; + this.y = y; + this.z = z; + } + + // public uint magnitude { + // get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); + // } + } + + public class Vector3Int : Vector3Of + { + public Vector3Int(int x, int y, int z) : base(x, y, z) { } + } + public class Vector3Float : Vector3Of + { + public Vector3Float(float x, float y, float z) : base(x, y, z) { } + + public float magnitude + { + get => (float)Math.Sqrt(this.x * this.x + this.y * this.y + this.z * this.z); + } + } + + /// + /// 3-dimensional vectors + /// + /// This uses the right-handed coordinate system. + public struct Vector3 : IEquatable + { + + /// + /// The right axis of the vector + /// + public float x; //> left/right + /// + /// The upward axis of the vector + /// + public float y; //> up/down + /// + /// The forward axis of the vector + /// + public float z; //> forward/backward + + /// + /// Create a new 3-dimensional vector + /// + /// x axis value + /// y axis value + /// z axis value + public Vector3(float x, float y, float z) + { + this.x = x; + this.y = y; + this.z = z; + } + + /// + /// A vector with zero for all axis + /// + public static readonly Vector3 zero = new Vector3(0, 0, 0); + /// + /// A vector with one for all axis + /// + public static readonly Vector3 one = new Vector3(1, 1, 1); + /// + /// A vector3 with values (-1, 0, 0) + /// + public static readonly Vector3 left = new Vector3(-1, 0, 0); + /// + /// A vector with values (1, 0, 0) + /// + public static readonly Vector3 right = new Vector3(1, 0, 0); + /// + /// A vector with values (0, -1, 0) + /// + public static readonly Vector3 down = new Vector3(0, -1, 0); + /// + /// A vector with values (0, 1, 0) + /// + public static readonly Vector3 up = new Vector3(0, 1, 0); + /// + /// A vector with values (0, 0, -1) + /// + public static readonly Vector3 back = new Vector3(0, -1, 0); + /// + /// A vector with values (0, 0, 1) + /// + public static readonly Vector3 forward = new Vector3(0, 1, 0); + + public float magnitude + { + get + { + float d = (float)Math.Sqrt(x * x + y * y); + return d; + } + } + + public Vector3 normalized + { + get + { + float l = magnitude; + Vector3 v = zero; + if (l > Float.epsilon) + v = this / l; + return v; + } + } + + public static Vector3 operator +(Vector3 v1, Vector3 v2) + { + Vector3 v = new Vector3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z); + return v; + } + + public static Vector3 operator -(Vector3 v1, Vector3 v2) + { + Vector3 v = new Vector3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z); + return v; + } + + public static Vector3 operator -(Vector3 v1) + { + Vector3 v = new Vector3(-v1.x, -v1.y, -v1.z); + return v; + } + + public static Vector3 operator *(Vector3 v1, float d) + { + Vector3 v = new Vector3(v1.x * d, v1.y * d, v1.z * d); + return v; + } + + public static Vector3 operator *(float d, Vector3 v1) + { + Vector3 v = new Vector3(d * v1.x, d * v1.y, d * v1.z); + return v; + } + + public static Vector3 operator /(Vector3 v1, float d) + { + Vector3 v = new Vector3(v1.x / d, v1.y / d, v1.z / d); + return v; + } + + public bool Equals(Vector3 v) => (x == v.x && y == v.y && z == v.z); + + public override bool Equals(object obj) + { + if (!(obj is Vector3 v)) + return false; + + return (x == v.x && y == v.y && z == v.z); + } + + public static bool operator ==(Vector3 v1, Vector3 v2) + { + return (v1.x == v2.x && v1.y == v2.y && v1.z == v2.z); + } + + public static bool operator !=(Vector3 v1, Vector3 v2) + { + return (v1.x != v2.x || v1.y != v2.y || v1.z != v2.z); + } + + public override int GetHashCode() + { + return (x, y, z).GetHashCode(); + } + + public static float Distance(Vector3 v1, Vector3 v2) + { + return (v2 - v1).magnitude; + } + + public static float Dot(Vector3 v1, Vector3 v2) + { + return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z; + } + + public static Vector3 Lerp(Vector3 v1, Vector3 v2, float f) + { + Vector3 v = v1 + (v2 - v1) * f; + return v; + } + + } +} +#endif \ No newline at end of file diff --git a/src/float16.cs b/src/float16.cs new file mode 100644 index 0000000..5d94014 --- /dev/null +++ b/src/float16.cs @@ -0,0 +1,344 @@ +using System; + +namespace LinearAlgebra +{ + + public class float16 + { + // + // FILE: float16.cpp + // AUTHOR: Rob Tillaart + // VERSION: 0.1.8 + // PURPOSE: library for Float16s for Arduino + // URL: http://en.wikipedia.org/wiki/Half-precision_floating-point_format + + ushort _value; + + public float16() { _value = 0; } + + public float16(float f) + { + //_value = f32tof16(f); + _value = F32ToF16__(f); + } + + public float toFloat() + { + return f16tof32(_value); + } + + public ushort GetBinary() { return _value; } + public void SetBinary(ushort value) { _value = value; } + + ////////////////////////////////////////////////////////// + // + // EQUALITIES + // + /* + bool float16::operator ==(const float16 &f) { return (_value == f._value); } + + bool float16::operator !=(const float16 &f) { return (_value != f._value); } + + bool float16::operator >(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value < f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value > f._value; + } + + bool float16::operator >=(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value <= f._value; + if (_value & 0x8000) + return false; + if (f._value & 0x8000) + return true; + return _value >= f._value; + } + + bool float16::operator <(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value > f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value < f._value; + } + + bool float16::operator <=(const float16 &f) { + if ((_value & 0x8000) && (f._value & 0x8000)) + return _value >= f._value; + if (_value & 0x8000) + return true; + if (f._value & 0x8000) + return false; + return _value <= f._value; + } + + ////////////////////////////////////////////////////////// + // + // NEGATION + // + float16 float16::operator -() { + float16 f16; + f16.setBinary(_value ^ 0x8000); + return f16; + } + + ////////////////////////////////////////////////////////// + // + // MATH + // + float16 float16::operator +(const float16 &f) { + return float16(this->toDouble() + f.toDouble()); + } + + float16 float16::operator -(const float16 &f) { + return float16(this->toDouble() - f.toDouble()); + } + + float16 float16::operator *(const float16 &f) { + return float16(this->toDouble() * f.toDouble()); + } + + float16 float16::operator /(const float16 &f) { + return float16(this->toDouble() / f.toDouble()); + } + + float16 & float16::operator+=(const float16 &f) { + *this = this->toDouble() + f.toDouble(); + return *this; + } + + float16 & float16::operator-=(const float16 &f) { + *this = this->toDouble() - f.toDouble(); + return *this; + } + + float16 & float16::operator*=(const float16 &f) { + *this = this->toDouble() * f.toDouble(); + return *this; + } + + float16 & float16::operator/=(const float16 &f) { + *this = this->toDouble() / f.toDouble(); + return *this; + } + + ////////////////////////////////////////////////////////// + // + // MATH HELPER FUNCTIONS + // + int float16::sign() { + if (_value & 0x8000) + return -1; + if (_value & 0xFFFF) + return 1; + return 0; + } + + bool float16::isZero() { return ((_value & 0x7FFF) == 0x0000); } + + bool float16::isNaN() { + if ((_value & 0x7C00) != 0x7C00) + return false; + if ((_value & 0x03FF) == 0x0000) + return false; + return true; + } + + bool float16::isInf() { return ((_value == 0x7C00) || (_value == 0xFC00)); } + */ + ////////////////////////////////////////////////////////// + // + // CORE CONVERSION + // + float f16tof32(ushort _value) + { + //ushort sgn; + ushort man; + int exp; + float f; + + //Debug.Log($"{_value}"); + + bool sgn = (_value & 0x8000) > 0; + exp = (_value & 0x7C00) >> 10; + man = (ushort)(_value & 0x03FF); + + //Debug.Log($"{sgn} {exp} {man}"); + + // ZERO + if ((_value & 0x7FFF) == 0) + { + return sgn ? -0 : 0; + } + // NAN & INF + if (exp == 0x001F) + { + if (man == 0) + return sgn ? float.NegativeInfinity : float.PositiveInfinity; //-INFINITY : INFINITY; + else + return float.NaN; // NAN; + } + + // SUBNORMAL/NORMAL + if (exp == 0) + f = 0; + else + f = 1; + + // PROCESS MANTISSE + for (int i = 9; i >= 0; i--) + { + f *= 2; + if ((man & (1 << i)) != 0) + f = f + 1; + } + //Debug.Log($"{f}"); + f = f * (float)Math.Pow(2.0f, exp - 25); + if (exp == 0) + { + f = f * (float)Math.Pow(2.0f, -13); // 5.96046447754e-8; + } + //Debug.Log($"{f}"); + return sgn ? -f : f; + } + + public static uint SingleToInt32Bits(float value) + { + byte[] bytes = BitConverter.GetBytes(value); + if (BitConverter.IsLittleEndian) + Array.Reverse(bytes); // If the system is little-endian, reverse the byte order + return BitConverter.ToUInt32(bytes, 0); + } + + public ushort F32ToF16__(float f) + { + uint t = BitConverter.ToUInt32(BitConverter.GetBytes(f), 0); + ushort man = (ushort)((t & 0x007FFFFF) >> 12); + int exp = (int)((t & 0x7F800000) >> 23); + bool sgn = (t & 0x80000000) != 0; + + // handle 0 + if ((t & 0x7FFFFFFF) == 0) + { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // denormalized float32 does not fit in float16 + if (exp == 0x00) + { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // handle infinity & NAN + if (exp == 0x00FF) + { + if (man != 0) + return 0xFE00; // NAN + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + + // normal numbers + exp = exp - 127 + 15; + // overflow does not fit => INF + if (exp > 30) + { + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + // subnormal numbers + if (exp < -38) + { + return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ? + } + if (exp <= 0) // subnormal + { + man >>= (exp + 14); + // rounding + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | man); + return man; + } + + // normal + // TODO rounding + exp <<= 10; + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | exp | man); + return (ushort)(exp | man); + } + + //This function is faulty!!!! + ushort f32tof16(float f) + { + //uint t = *(uint*)&f; + //uint t = (uint)BitConverter.SingleToInt32Bits(f); + uint t = SingleToInt32Bits(f); + // man bits = 10; but we keep 11 for rounding + ushort man = (ushort)((t & 0x007FFFFF) >> 12); + short exp = (short)((t & 0x7F800000) >> 23); + bool sgn = (t & 0x80000000) != 0; + + // handle 0 + if ((t & 0x7FFFFFFF) == 0) + { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // denormalized float32 does not fit in float16 + if (exp == 0x00) + { + return sgn ? (ushort)0x8000 : (ushort)0x0000; + } + // handle infinity & NAN + if (exp == 0x00FF) + { + if (man != 0) + return 0xFE00; // NAN + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + + // normal numbers + exp = (short)(exp - 127 + 15); + // overflow does not fit => INF + if (exp > 30) + { + return sgn ? (ushort)0xFC00 : (ushort)0x7C00; // -INF : INF + } + // subnormal numbers + if (exp < -38) + { + return sgn ? (ushort)0x8000 : (ushort)0x0000; // -0 or 0 ? just 0 ? + } + if (exp <= 0) // subnormal + { + man >>= (exp + 14); + // rounding + man++; + man >>= 1; + if (sgn) + return (ushort)(0x8000 | man); + return man; + } + + // normal + // TODO rounding + exp <<= 10; + man++; + man >>= 1; + ushort uexp = (ushort)exp; + if (sgn) + return (ushort)(0x8000 | uexp | man); + return (ushort)(uexp | man); + } + + // -- END OF FILE -- + } + +} \ No newline at end of file diff --git a/test/AngleTest.cs b/test/AngleTest.cs new file mode 100644 index 0000000..34b107a --- /dev/null +++ b/test/AngleTest.cs @@ -0,0 +1,171 @@ +#if !UNITY_5_6_OR_NEWER +using NUnit.Framework; + +namespace LinearAlgebra.Test +{ + public class Tests + { + [SetUp] + public void Setup() + { + } + + [Test] + public void Test_Normalize() + { + float r = 0; + + r = Angle.Normalize(90); + Assert.AreEqual(r, 90, "Normalize 90"); + + r = Angle.Normalize(-90); + Assert.AreEqual(r, -90, "Normalize -90"); + + r = Angle.Normalize(270); + Assert.AreEqual(r, -90, "Normalize 270"); + + r = Angle.Normalize(270 + 360); + Assert.AreEqual(r, -90, "Normalize 270+360"); + + r = Angle.Normalize(-270); + Assert.AreEqual(r, 90, "Normalize -270"); + + r = Angle.Normalize(-270 - 360); + Assert.AreEqual(r, 90, "Normalize -270-360"); + + r = Angle.Normalize(0); + Assert.AreEqual(r, 0, "Normalize 0"); + + r = Angle.Normalize(float.PositiveInfinity); + Assert.AreEqual(r, float.PositiveInfinity, "Normalize INFINITY"); + + r = Angle.Normalize(float.NegativeInfinity); + Assert.AreEqual(r, float.NegativeInfinity, "Normalize INFINITY"); + } + + [Test] + public void Clamp() + { + float r = 0; + + r = Angle.Clamp(1, 0, 2); + Assert.AreEqual(r, 1, "Clamp 1 0 2"); + + r = Angle.Clamp(-1, 0, 2); + Assert.AreEqual(r, 0, "Clamp -1 0 2"); + + r = Angle.Clamp(3, 0, 2); + Assert.AreEqual(r, 2, "Clamp 3 0 2"); + + r = Angle.Clamp(1, 0, 0); + Assert.AreEqual(r, 0, "Clamp 1 0 0"); + + r = Angle.Clamp(0, 0, 0); + Assert.AreEqual(r, 0, "Clamp 0 0 0"); + + r = Angle.Clamp(0, 1, -1); + Assert.AreEqual(r, 1, "Clamp 0 1 -1"); + + r = Angle.Clamp(1, 0, float.PositiveInfinity); + Assert.AreEqual(r, 1, "Clamp 1 0 INFINITY"); + + r = Angle.Clamp(1, float.NegativeInfinity, 1); + Assert.AreEqual(r, 1, "Clamp 1 -INFINITY 1"); + } + + [Test] + public void Difference() + { + float r = 0; + + r = Angle.Difference(0, 90); + Assert.AreEqual(r, 90, "Difference 0 90"); + + r = Angle.Difference(0, -90); + Assert.AreEqual(r, -90, "Difference 0 -90"); + + r = Angle.Difference(0, 270); + Assert.AreEqual(r, -90, "Difference 0 270"); + + r = Angle.Difference(0, -270); + Assert.AreEqual(r, 90, "Difference 0 -270"); + + r = Angle.Difference(90, 0); + Assert.AreEqual(r, -90, "Difference 90 0"); + + r = Angle.Difference(-90, 0); + Assert.AreEqual(r, 90, "Difference -90 0"); + + r = Angle.Difference(0, 0); + Assert.AreEqual(r, 0, "Difference 0 0"); + + r = Angle.Difference(90, 90); + Assert.AreEqual(r, 0, "Difference 90 90"); + + r = Angle.Difference(0, float.PositiveInfinity); + Assert.AreEqual(r, float.PositiveInfinity, "Difference 0 INFINITY"); + + r = Angle.Difference(0, float.NegativeInfinity); + Assert.AreEqual(r, float.NegativeInfinity, "Difference 0 -INFINITY"); + + r = Angle.Difference(float.NegativeInfinity, float.PositiveInfinity); + Assert.AreEqual(r, float.PositiveInfinity, "Difference -INFINITY INFINITY"); + } + + [Test] + public void MoveTowards() + { + float r = 0; + + r = Angle.MoveTowards(0, 90, 30); + Assert.AreEqual(r, 30, "MoveTowards 0 90 30"); + + r = Angle.MoveTowards(0, 90, 90); + Assert.AreEqual(r, 90, "MoveTowards 0 90 90"); + + r = Angle.MoveTowards(0, 90, 180); + Assert.AreEqual(r, 90, "MoveTowards 0 90 180"); + + r = Angle.MoveTowards(0, 90, 270); + Assert.AreEqual(r, 90, "MoveTowrads 0 90 270"); + + r = Angle.MoveTowards(0, 90, -30); + Assert.AreEqual(r, -30, "MoveTowards 0 90 -30"); + + r = Angle.MoveTowards(0, -90, -30); + Assert.AreEqual(r, 30, "MoveTowards 0 -90 -30"); + + r = Angle.MoveTowards(0, -90, -90); + Assert.AreEqual(r, 90, "MoveTowards 0 -90 -90"); + + r = Angle.MoveTowards(0, -90, -180); + Assert.AreEqual(r, 180, "MoveTowards 0 -90 -180"); + + r = Angle.MoveTowards(0, -90, -270); + Assert.AreEqual(r, 270, "MoveTowrads 0 -90 -270"); + + r = Angle.MoveTowards(0, 90, 0); + Assert.AreEqual(r, 0, "MoveTowards 0 90 0"); + + r = Angle.MoveTowards(0, 0, 0); + Assert.AreEqual(r, 0, "MoveTowards 0 0 0"); + + r = Angle.MoveTowards(0, 0, 30); + Assert.AreEqual(r, 0, "MoveTowrads 0 0 30"); + + r = Angle.MoveTowards(0, 90, float.PositiveInfinity); + Assert.AreEqual(r, 90, "MoveTowards 0 90 INFINITY"); + + r = Angle.MoveTowards(0, float.PositiveInfinity, 30); + Assert.AreEqual(r, 30, "MoveTowrads 0 INFINITY 30"); + + r = Angle.MoveTowards(0, -90, float.NegativeInfinity); + Assert.AreEqual(r, float.PositiveInfinity, "MoveTowards 0 -90 -INFINITY"); + + r = Angle.MoveTowards(0, float.NegativeInfinity, -30); + Assert.AreEqual(r, 30, "MoveTowrads 0 -INFINITY -30"); + + } + } +} +#endif \ No newline at end of file diff --git a/test/LinearAlgebra_Test.csproj b/test/LinearAlgebra_Test.csproj new file mode 100644 index 0000000..3ee2230 --- /dev/null +++ b/test/LinearAlgebra_Test.csproj @@ -0,0 +1,19 @@ + + + + net5.0 + false + true + + + + + + + + + + + + +