diff --git a/Nerd_STF/Helpers/MatrixHelper.cs b/Nerd_STF/Helpers/MatrixHelper.cs new file mode 100644 index 0000000..114eb1d --- /dev/null +++ b/Nerd_STF/Helpers/MatrixHelper.cs @@ -0,0 +1,72 @@ +using Nerd_STF.Mathematics; +using Nerd_STF.Mathematics.Algebra; +using System.Collections.Generic; + +namespace Nerd_STF.Helpers +{ + internal static class MatrixHelper + { + public static void SetMatrixValues(TMat matrix, IEnumerable> vals, bool byRows) + where TMat : IMatrix + { + Int2 size = matrix.Size; + int x = 0; + foreach (IEnumerable part in vals) + { + int y = 0; + foreach (double v in part) + { + matrix[byRows ? (x, y) : (y, x)] = v; + y++; + if (byRows ? y >= size.x : y >= size.y) break; + } + x++; + if (byRows ? x >= size.y : x >= size.x) break; + } + } + public static void SetMatrixValues(TMat matrix, IEnumerable> vals, bool byRows) + where TMat : IMatrix + { + // Literally the same code. Sucks that casting doesn't work here. + Int2 size = matrix.Size; + int x = 0; + foreach (IEnumerable part in vals) + { + int y = 0; + foreach (double v in part) + { + matrix[byRows ? (x, y) : (y, x)] = v; + y++; + if (byRows ? y >= size.x : y >= size.y) break; + } + x++; + if (byRows ? x >= size.y : x >= size.x) break; + } + } + + public static void SetRow(TMat matrix, int row, IEnumerable vals) + where TMat: IMatrix + { + int col = 0; + int max = matrix.Size.y; + foreach (double v in vals) + { + matrix[row, col] = v; + col++; + if (col >= max) return; + } + } + public static void SetColumn(TMat matrix, int col, IEnumerable vals) + where TMat : IMatrix + { + int row = 0; + int max = matrix.Size.x; + foreach (double v in vals) + { + matrix[row, col] = v; + row++; + if (row >= max) return; + } + } + } +} diff --git a/Nerd_STF/Mathematics/Algebra/Matrix2x2.cs b/Nerd_STF/Mathematics/Algebra/Matrix2x2.cs index 9816692..8d5ac28 100644 --- a/Nerd_STF/Mathematics/Algebra/Matrix2x2.cs +++ b/Nerd_STF/Mathematics/Algebra/Matrix2x2.cs @@ -9,141 +9,117 @@ namespace Nerd_STF.Mathematics.Algebra public class Matrix2x2 : IMatrix, ISquareMatrix #if CS11_OR_GREATER - ,ISplittable, + ,ISplittable, IStaticMatrix #endif { - public static Matrix2x2 Identity => new Matrix2x2(1, 0, 0, 1); - public static Matrix2x2 SignField => new Matrix2x2(1, -1, -1, 1); + public static Matrix2x2 Identity => + new Matrix2x2(1, 0, + 0, 1); + public static Matrix2x2 SignField => + new Matrix2x2(+1, -1, + -1, +1); public static Matrix2x2 One => new Matrix2x2(1, 1, 1, 1); public static Matrix2x2 Zero => new Matrix2x2(0, 0, 0, 0); public Int2 Size => (2, 2); - public double r1c1, r1c2, - r2c1, r2c2; + public double r0c0, r0c1, + r1c0, r1c1; public Matrix2x2() { - r1c1 = 0; - r1c2 = 0; - r2c1 = 0; - r2c2 = 0; + r0c0 = 0; r0c1 = 0; + r1c0 = 0; r1c1 = 0; } public Matrix2x2(Matrix2x2 copy) { - r1c1 = copy.r1c1; - r1c2 = copy.r1c2; - r2c1 = copy.r2c1; - r2c2 = copy.r2c2; + r0c0 = copy.r0c0; r0c1 = copy.r0c1; + r1c0 = copy.r1c0; r1c1 = copy.r1c1; } - public Matrix2x2(double r1c1, double r1c2, double r2c1, double r2c2) + public Matrix2x2(double r0c0, double r0c1, double r1c0, double r1c1) { + this.r0c0 = r0c0; + this.r0c1 = r0c1; + this.r1c0 = r1c0; this.r1c1 = r1c1; - this.r1c2 = r1c2; - this.r2c1 = r2c1; - this.r2c2 = r2c2; } /// if the array is of the form [c, r], if the array is of the form [r, c]. public Matrix2x2(double[,] vals, bool byRows = true) { if (byRows) // Collection of rows ([c, r]) { - r1c1 = vals[0, 0]; r1c2 = vals[0, 1]; - r2c1 = vals[1, 0]; r2c2 = vals[1, 1]; + r0c0 = vals[0, 0]; r0c1 = vals[0, 1]; + r1c0 = vals[1, 0]; r1c1 = vals[1, 1]; } else // Collection of columns ([r, c]) { - r1c1 = vals[0, 0]; r1c2 = vals[1, 0]; - r2c1 = vals[0, 1]; r2c2 = vals[1, 1]; + r0c0 = vals[0, 0]; r0c1 = vals[1, 0]; + r1c0 = vals[0, 1]; r1c1 = vals[1, 1]; } } /// if the enumerable is a collection of rows (form [c, r]), if the enumerable is a collection of columns (form [r, c]). public Matrix2x2(IEnumerable> vals, bool byRows = true) { - int x = 0; - foreach (IEnumerable part in vals) - { - int y = 0; - foreach (double v in part) - { - this[byRows ? (x, y) : (y, x)] = v; - y++; - if (y >= 2) break; - } - x++; - if (x >= 2) break; - } + MatrixHelper.SetMatrixValues(this, vals, byRows); } /// if the enumerable is a collection of rows (form [c, r]), if the enumerable is a collection of columns (form [r, c]). public Matrix2x2(IEnumerable> vals, bool byRows = true) { - int x = 0; - foreach (IEnumerable part in vals) - { - int y = 0; - foreach (double v in part) - { - this[byRows ? (x, y) : (y, x)] = v; - y++; - if (y >= 2) break; - } - x++; - if (x >= 2) break; - } + MatrixHelper.SetMatrixValues(this, vals, byRows); } public double this[int r, int c] - { - get => this[(r, c)]; - set => this[(r, c)] = value; - } - public double this[Int2 index] { get { - switch (index.x) // (r, c) + switch (r) { case 0: - switch (index.y) + switch (c) { - case 0: return r1c1; - case 1: return r1c2; - default: throw new ArgumentOutOfRangeException(nameof(index)); + case 0: return r0c0; + case 1: return r0c1; + default: throw new ArgumentOutOfRangeException(nameof(c)); } case 1: - switch (index.y) + switch (c) { - case 0: return r2c1; - case 1: return r2c2; - default: throw new ArgumentOutOfRangeException(nameof(index)); + case 0: return r1c0; + case 1: return r1c1; + default: throw new ArgumentOutOfRangeException(nameof(c)); } - default: throw new ArgumentOutOfRangeException(nameof(index)); + default: throw new ArgumentOutOfRangeException(nameof(r)); } } set { - switch (index.x) // (r, c) + switch (r) { case 0: - switch (index.y) + switch (c) { - case 0: r1c1 = value; return; - case 1: r1c2 = value; return; - default: throw new ArgumentOutOfRangeException(nameof(index)); + case 0: r0c0 = value; return; + case 1: r0c1 = value; return; + default: throw new ArgumentOutOfRangeException(nameof(c)); } case 1: - switch (index.y) + switch (c) { - case 0: r2c1 = value; return; - case 1: r2c2 = value; return; - default: throw new ArgumentOutOfRangeException(nameof(index)); + case 0: r1c0 = value; return; + case 1: r1c1 = value; return; + default: throw new ArgumentOutOfRangeException(nameof(c)); } - default: throw new ArgumentOutOfRangeException(nameof(index)); + default: throw new ArgumentOutOfRangeException(nameof(r)); } } } + public double this[Int2 index] + { + get => this[index.x, index.y]; + set => this[index.x, index.y] = value; + } public ListTuple this[int index, RowColumn direction] { get @@ -178,8 +154,8 @@ namespace Nerd_STF.Mathematics.Algebra return sum / count; } public static Matrix2x2 Lerp(Matrix2x2 a, Matrix2x2 b, double t, bool clamp = true) => - new Matrix2x2(MathE.Lerp(a.r1c1, b.r1c1, t, clamp), MathE.Lerp(a.r1c2, b.r1c2, t, clamp), - MathE.Lerp(a.r2c1, b.r2c1, t, clamp), MathE.Lerp(a.r2c2, b.r2c2, t, clamp)); + new Matrix2x2(MathE.Lerp(a.r0c0, b.r0c0, t, clamp), MathE.Lerp(a.r0c1, b.r0c1, t, clamp), + MathE.Lerp(a.r1c0, b.r1c0, t, clamp), MathE.Lerp(a.r1c1, b.r1c1, t, clamp)); public static Matrix2x2 Product(IEnumerable vals) { bool any = false; @@ -198,20 +174,19 @@ namespace Nerd_STF.Mathematics.Algebra return result; } - public static (double[] r1c1, double[] r1c2, double[] r2c1, double[] r2c2) SplitArray(IEnumerable vals) + public static (double[] r0c0, double[] r0c1, double[] r1c0, double[] r1c1) SplitArray(IEnumerable vals) { int count = vals.Count(); - double[] r1c1 = new double[count], r1c2 = new double[count], - r2c1 = new double[count], r2c2 = new double[count]; + double[] r0c0 = new double[count], r0c1 = new double[count], + r1c0 = new double[count], r1c1 = new double[count]; int index = 0; foreach (Matrix2x2 m in vals) { - r1c1[index] = m.r1c1; - r1c2[index] = m.r1c2; - r2c1[index] = m.r2c1; - r2c2[index] = m.r2c2; + r0c0[index] = m.r0c0; r0c1[index] = m.r0c1; + r1c0[index] = m.r1c0; r1c1[index] = m.r1c1; } - return (r1c1, r1c2, r2c1, r2c2); + return (r0c0, r0c1, + r1c0, r1c1); } public ListTuple GetRow(int row) @@ -219,8 +194,8 @@ namespace Nerd_STF.Mathematics.Algebra double[] vals; switch (row) { - case 0: vals = new double[] { r1c1, r1c2 }; break; - case 1: vals = new double[] { r2c1, r2c2 }; break; + case 0: vals = new double[] { r0c0, r0c1 }; break; + case 1: vals = new double[] { r1c0, r1c1 }; break; default: throw new ArgumentOutOfRangeException(nameof(row)); } return new ListTuple(vals); @@ -230,60 +205,56 @@ namespace Nerd_STF.Mathematics.Algebra double[] vals; switch (column) { - case 0: vals = new double[] { r1c1, r2c1 }; break; - case 1: vals = new double[] { r1c2, r2c2 }; break; + case 0: vals = new double[] { r0c0, r1c0 }; break; + case 1: vals = new double[] { r0c1, r1c1 }; break; default: throw new ArgumentOutOfRangeException(nameof(column)); } return new ListTuple(vals); } - public void SetRow(int row, IEnumerable vals) + public void SetRow(int row, IEnumerable vals) => MatrixHelper.SetRow(this, row, vals); + public void SetColumn(int column, IEnumerable vals) => MatrixHelper.SetColumn(this, column, vals); + public void SetRow(int row, ListTuple vals) { - int col = 0; - foreach (double v in vals) + switch (row) { - this[(row, col)] = v; - col++; - if (col >= 2) return; + case 0: r0c0 = vals[0]; r0c1 = vals[1]; break; + case 1: r1c0 = vals[0]; r1c1 = vals[1]; break; + default: throw new ArgumentOutOfRangeException(nameof(row)); } } - public void SetColumn(int column, IEnumerable vals) + public void SetColumn(int column, ListTuple vals) { - int row = 0; - foreach (double v in vals) + switch (column) { - this[(row, column)] = v; - row++; - if (row >= 2) return; + case 0: r0c0 = vals[0]; r1c0 = vals[1]; break; + case 1: r0c1 = vals[0]; r1c1 = vals[1]; break; + default: throw new ArgumentOutOfRangeException(nameof(column)); } } - public void SetRow(int row, ListTuple vals) => SetRow(row, (IEnumerable)vals); - public void SetColumn(int row, ListTuple vals) => SetColumn(row, (IEnumerable)vals); - public double Determinant() => r1c1 * r2c2 - r1c2 * r2c1; + public double Determinant() => r0c0 * r1c1 - r0c1 * r1c0; public Matrix2x2 Adjoint() => - new Matrix2x2( r2c2, -r1c2, - -r2c1, r1c1); + new Matrix2x2( r1c1, -r0c1, + -r1c0, r0c0); public Matrix2x2 Cofactor() => - new Matrix2x2( r2c2, -r2c1, - -r1c2, r1c1); + new Matrix2x2( r1c1, -r1c0, + -r0c1, r0c0); public Matrix2x2 Inverse() { double invDet = 1 / Determinant(); - return new Matrix2x2( r2c2 * invDet, -r1c2 * invDet, - -r2c1 * invDet, r1c1 * invDet); + return new Matrix2x2( r1c1 * invDet, -r0c1 * invDet, + -r1c0 * invDet, r0c0 * invDet); } public Matrix2x2 Transpose() => - new Matrix2x2(r1c1, r2c1, - r1c2, r2c2); - public double Trace() => r1c1 + r2c2; + new Matrix2x2(r0c0, r1c0, + r0c1, r1c1); + public double Trace() => r0c0 + r1c1; public IEnumerator GetEnumerator() { - yield return r1c1; - yield return r1c2; - yield return r2c1; - yield return r2c2; + yield return r0c0; yield return r0c1; + yield return r1c0; yield return r1c1; } IEnumerator IEnumerable.GetEnumerator() => GetEnumerator(); @@ -293,8 +264,8 @@ namespace Nerd_STF.Mathematics.Algebra public bool Equals(Matrix2x2 other) => #endif !(other is null) && - r1c1 == other.r1c1 && r1c2 == other.r1c2 && - r2c1 == other.r2c1 && r2c2 == other.r2c2; + r0c0 == other.r0c0 && r0c1 == other.r0c1 && + r1c0 == other.r1c0 && r1c1 == other.r1c1; #if CS8_OR_GREATER public override bool Equals(object? other) #else @@ -306,46 +277,52 @@ namespace Nerd_STF.Mathematics.Algebra else return false; } public override int GetHashCode() => - (int)((uint)r1c1.GetHashCode() & 0xFF000000 | - (uint)r1c2.GetHashCode() & 0x00FF0000 | - (uint)r2c1.GetHashCode() & 0x0000FF00 | - (uint)r2c2.GetHashCode() & 0x000000FF); + (int)((uint)r0c0.GetHashCode() & 0xFF000000 | + (uint)r0c1.GetHashCode() & 0x00FF0000 | + (uint)r1c0.GetHashCode() & 0x0000FF00 | + (uint)r1c1.GetHashCode() & 0x000000FF); public override string ToString() => ToStringHelper.MatrixToString(this, null); #if CS8_OR_GREATER public string ToString(string? format) => ToStringHelper.MatrixToString(this, format); -#else - public string ToString(string format) => ToStringHelper.MatrixToString(this, format); -#endif -#if CS8_OR_GREATER public string ToString(string? format, IFormatProvider? provider) => ToStringHelper.MatrixToString(this, format); #else + public string ToString(string format) => ToStringHelper.MatrixToString(this, format); public string ToString(string format, IFormatProvider provider) => ToStringHelper.MatrixToString(this, format); #endif public static Matrix2x2 operator +(Matrix2x2 a) => - new Matrix2x2(a.r1c1, a.r1c2, - a.r2c1, a.r2c2); + new Matrix2x2(a.r0c0, a.r0c1, + a.r1c0, a.r1c1); public static Matrix2x2 operator +(Matrix2x2 a, Matrix2x2 b) => - new Matrix2x2(a.r1c1 + b.r1c1, a.r1c2 + b.r1c2, - a.r2c1 + b.r2c1, a.r2c2 + b.r2c2); + new Matrix2x2(a.r0c0 + b.r0c0, a.r0c1 + b.r0c1, + a.r1c0 + b.r1c0, a.r1c1 + b.r1c1); public static Matrix2x2 operator -(Matrix2x2 a) => - new Matrix2x2(-a.r1c1, -a.r1c2, - -a.r2c1, -a.r2c2); + new Matrix2x2(-a.r0c0, -a.r0c1, + -a.r1c0, -a.r1c1); public static Matrix2x2 operator -(Matrix2x2 a, Matrix2x2 b) => - new Matrix2x2(a.r1c1 - b.r1c1, a.r1c2 - b.r1c2, - a.r2c1 - b.r2c1, a.r2c2 - b.r2c2); - public static Matrix2x2 operator *(Matrix2x2 a, Matrix2x2 b) => - new Matrix2x2(a.r1c1 * b.r1c1 + a.r1c2 * b.r2c1, a.r1c1 * b.r1c2 + a.r1c2 * b.r2c2, - a.r2c1 * b.r1c1 + a.r2c2 * b.r2c1, a.r2c1 * b.r1c2 + a.r2c2 * b.r2c2); + new Matrix2x2(a.r0c0 - b.r0c0, a.r0c1 - b.r0c1, + a.r1c0 - b.r1c0, a.r1c1 - b.r1c1); public static Matrix2x2 operator *(Matrix2x2 a, double b) => - new Matrix2x2(a.r1c1 * b, a.r1c2 * b, - a.r2c1 * b, a.r2c2 * b); + new Matrix2x2(a.r0c0 * b, a.r0c1 * b, + a.r1c0 * b, a.r1c1 * b); public static Float2 operator *(Matrix2x2 a, Float2 b) => - new Float2(a.r1c1 * b.x + a.r1c2 * b.y, - a.r2c1 * b.x + a.r2c2 * b.y); + new Float2(a.r0c0 * b.x + a.r0c1 * b.y, + a.r1c0 * b.x + a.r1c1 * b.y); + public static Matrix2x2 operator *(Matrix2x2 a, Matrix2x2 b) => + new Matrix2x2(a.r0c0 * b.r0c0 + a.r0c1 * b.r1c0, a.r0c0 * b.r0c1 + a.r0c1 * b.r1c1, + a.r1c0 * b.r0c0 + a.r1c1 * b.r1c0, a.r1c0 * b.r0c1 + a.r1c1 * b.r1c1); public static Matrix2x2 operator /(Matrix2x2 a, double b) => - new Matrix2x2(a.r1c1 / b, a.r1c2 / b, - a.r2c1 / b, a.r2c2 / b); + new Matrix2x2(a.r0c0 / b, a.r0c1 / b, + a.r1c0 / b, a.r1c1 / b); public static Matrix2x2 operator ~(Matrix2x2 a) => a.Inverse(); + public static bool operator ==(Matrix2x2 a, Matrix2x2 b) => a.Equals(b); + public static bool operator !=(Matrix2x2 a, Matrix2x2 b) => !a.Equals(b); + + public static explicit operator Matrix2x2(Matrix3x3 mat) => + new Matrix2x2(mat.r0c0, mat.r0c1, + mat.r1c0, mat.r1c1); + public static explicit operator Matrix2x2(Matrix4x4 mat) => + new Matrix2x2(mat.r0c0, mat.r0c1, + mat.r1c0, mat.r1c1); } } diff --git a/Nerd_STF/Mathematics/Algebra/Matrix3x3.cs b/Nerd_STF/Mathematics/Algebra/Matrix3x3.cs new file mode 100644 index 0000000..b409710 --- /dev/null +++ b/Nerd_STF/Mathematics/Algebra/Matrix3x3.cs @@ -0,0 +1,418 @@ +using Nerd_STF.Helpers; +using System; +using System.Collections; +using System.Collections.Generic; +using System.Linq; + +namespace Nerd_STF.Mathematics.Algebra +{ + public class Matrix3x3 : IMatrix, + ISquareMatrix, + ISubmatrixOperations +#if CS11_OR_GREATER + ,ISplittable, + IStaticMatrix +#endif + { + public static Matrix3x3 Identity => + new Matrix3x3(1, 0, 0, + 0, 1, 0, + 0, 0, 1); + public static Matrix3x3 SignField => + new Matrix3x3(+1, -1, +1, + -1, +1, -1, + +1, -1, +1); + + public static Matrix3x3 One => new Matrix3x3(1, 1, 1, 1, 1, 1, 1, 1, 1); + public static Matrix3x3 Zero => new Matrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0); + + public Int2 Size => (3, 3); + + public double r0c0, r0c1, r0c2, + r1c0, r1c1, r1c2, + r2c0, r2c1, r2c2; + + public Matrix3x3() + { + r0c0 = 0; r0c1 = 0; r0c2 = 0; + r1c0 = 0; r1c1 = 0; r1c2 = 0; + r2c0 = 0; r2c1 = 0; r2c2 = 0; + } + public Matrix3x3(Matrix3x3 copy) + { + r0c0 = copy.r0c0; r0c1 = copy.r0c1; r0c2 = copy.r0c2; + r1c0 = copy.r1c0; r1c1 = copy.r1c1; r1c2 = copy.r1c2; + r2c0 = copy.r2c0; r2c1 = copy.r2c1; r2c2 = copy.r2c2; + } + public Matrix3x3(double r0c0, double r0c1, double r0c2, double r1c0, double r1c1, double r1c2, double r2c0, double r2c1, double r2c2) + { + this.r0c0 = r0c0; this.r0c1 = r0c1; this.r0c2 = r0c2; + this.r1c0 = r1c0; this.r1c1 = r1c1; this.r1c2 = r1c2; + this.r2c0 = r2c0; this.r2c1 = r2c1; this.r2c2 = r2c2; + } + /// if the array is of the form [c, r], if the array is of the form [r, c]. + public Matrix3x3(double[,] vals, bool byRows = true) + { + if (byRows) // Collection of rows ([c, r]) + { + r0c0 = vals[0, 0]; r0c1 = vals[0, 1]; r0c2 = vals[0, 2]; + r1c0 = vals[1, 0]; r1c1 = vals[1, 1]; r1c2 = vals[1, 2]; + r2c0 = vals[2, 0]; r2c1 = vals[2, 1]; r2c2 = vals[2, 2]; + } + else // Collection of columns ([r, c]) + { + r0c0 = vals[0, 0]; r0c1 = vals[1, 0]; r0c2 = vals[2, 0]; + r1c0 = vals[0, 1]; r1c1 = vals[1, 1]; r1c2 = vals[2, 1]; + r2c0 = vals[0, 2]; r2c1 = vals[1, 2]; r2c2 = vals[2, 2]; + } + } + /// if the enumerable is a collection of rows (form [c, r]), if the enumerable is a collection of columns (form [r, c]). + public Matrix3x3(IEnumerable> vals, bool byRows = true) + { + MatrixHelper.SetMatrixValues(this, vals, byRows); + } + /// if the enumerable is a collection of rows (form [c, r]), if the enumerable is a collection of columns (form [r, c]). + public Matrix3x3(IEnumerable> vals, bool byRows = true) + { + MatrixHelper.SetMatrixValues(this, vals, byRows); + } + + public double this[int r, int c] + { + get + { + switch (r) + { + case 0: + switch (c) + { + case 0: return r0c0; + case 1: return r0c1; + case 2: return r0c2; + default: throw new ArgumentOutOfRangeException(nameof(c)); + } + case 1: + switch (c) + { + case 0: return r1c0; + case 1: return r1c1; + case 2: return r1c2; + default: throw new ArgumentOutOfRangeException(nameof(c)); + } + case 2: + switch (c) + { + case 0: return r2c0; + case 1: return r2c1; + case 2: return r2c2; + default: throw new ArgumentOutOfRangeException(nameof(c)); + } + default: throw new ArgumentOutOfRangeException(nameof(r)); + } + } + set + { + switch (r) + { + case 0: + switch (c) + { + case 0: r0c0 = value; return; + case 1: r0c1 = value; return; + case 2: r0c2 = value; return; + default: throw new ArgumentOutOfRangeException(nameof(c)); + } + case 1: + switch (c) + { + case 0: r1c0 = value; return; + case 1: r1c1 = value; return; + case 2: r1c2 = value; return; + default: throw new ArgumentOutOfRangeException(nameof(c)); + } + case 2: + switch (c) + { + case 0: r2c0 = value; return; + case 1: r2c1 = value; return; + case 2: r2c2 = value; return; + default: throw new ArgumentOutOfRangeException(nameof(c)); + } + default: throw new ArgumentOutOfRangeException(nameof(r)); + } + } + } + public double this[Int2 index] + { + get => this[index.x, index.y]; + set => this[index.x, index.y] = value; + } + public ListTuple this[int index, RowColumn direction] + { + get + { + switch (direction) + { + case RowColumn.Row: return GetRow(index); + case RowColumn.Column: return GetColumn(index); + default: throw new ArgumentException($"Invalid direction {direction}."); + } + } + set + { + switch (direction) + { + case RowColumn.Row: SetRow(index, value); break; + case RowColumn.Column: SetColumn(index, value); break; + default: throw new ArgumentException($"Invalid direction {direction}."); + } + } + } + + public static Matrix3x3 Average(IEnumerable vals) + { + Matrix3x3 result = Zero; + int count = 0; + foreach (Matrix3x3 m in vals) + { + result += m; + count++; + } + return result / count; + } + public static Matrix3x3 Lerp(Matrix3x3 a, Matrix3x3 b, double t, bool clamp = true) => + new Matrix3x3(MathE.Lerp(a.r0c0, b.r0c0, t, clamp), MathE.Lerp(a.r0c1, b.r0c1, t, clamp), MathE.Lerp(a.r0c2, b.r0c2, t, clamp), + MathE.Lerp(a.r1c0, b.r1c0, t, clamp), MathE.Lerp(a.r1c1, b.r1c1, t, clamp), MathE.Lerp(a.r1c2, b.r1c2, t, clamp), + MathE.Lerp(a.r2c0, b.r2c0, t, clamp), MathE.Lerp(a.r2c1, b.r2c1, t, clamp), MathE.Lerp(a.r2c2, b.r2c2, t, clamp)); + public static Matrix3x3 Product(IEnumerable vals) + { + Matrix3x3 result = One; + bool any = false; + foreach (Matrix3x3 m in vals) + { + any = true; + result *= m; + } + return any ? result : Zero; + } + public static Matrix3x3 Sum(IEnumerable vals) + { + Matrix3x3 result = Zero; + foreach (Matrix3x3 m in vals) result += m; + return result; + } + + public static (double[] r0c0, double[] r0c1, double[] r0c2, double[] r1c0, double[] r1c1, double[] r1c2, double[] r2c0, double[] r2c1, double[] r2c2) SplitArray(IEnumerable vals) + { + int count = vals.Count(); + double[] r0c0 = new double[count], r0c1 = new double[count], r0c2 = new double[count], + r1c0 = new double[count], r1c1 = new double[count], r1c2 = new double[count], + r2c0 = new double[count], r2c1 = new double[count], r2c2 = new double[count]; + int index = 0; + foreach (Matrix3x3 m in vals) + { + r0c0[index] = m.r0c0; r0c1[index] = m.r0c1; r0c2[index] = m.r0c2; + r1c0[index] = m.r1c0; r1c1[index] = m.r1c1; r1c2[index] = m.r1c2; + r2c0[index] = m.r2c0; r2c1[index] = m.r2c1; r2c2[index] = m.r2c2; + } + return (r0c0, r0c1, r0c2, + r1c0, r1c1, r1c2, + r2c0, r2c1, r2c2); + } + + public ListTuple GetRow(int row) + { + double[] vals; + switch (row) + { + case 0: vals = new double[] { r0c0, r0c1, r0c2 }; break; + case 1: vals = new double[] { r1c0, r1c1, r1c2 }; break; + case 2: vals = new double[] { r2c0, r2c1, r2c2 }; break; + default: throw new ArgumentOutOfRangeException(nameof(row)); + } + return new ListTuple(vals); + } + public ListTuple GetColumn(int column) + { + double[] vals; + switch (column) + { + case 0: vals = new double[] { r0c0, r1c0, r2c0 }; break; + case 1: vals = new double[] { r0c1, r1c1, r2c1 }; break; + case 2: vals = new double[] { r0c2, r1c2, r2c2 }; break; + default: throw new ArgumentOutOfRangeException(nameof(column)); + } + return new ListTuple(vals); + } + public void SetRow(int row, IEnumerable vals) => MatrixHelper.SetRow(this, row, vals); + public void SetColumn(int column, IEnumerable vals) => MatrixHelper.SetColumn(this, column, vals); + public void SetRow(int row, ListTuple vals) + { + switch (row) + { + case 0: r0c0 = vals[0]; r0c1 = vals[1]; r0c2 = vals[2]; break; + case 1: r1c0 = vals[0]; r1c1 = vals[1]; r1c2 = vals[2]; break; + case 2: r2c0 = vals[0]; r2c1 = vals[1]; r2c2 = vals[2]; break; + default: throw new ArgumentOutOfRangeException(nameof(row)); + } + } + public void SetColumn(int column, ListTuple vals) + { + switch (column) + { + case 0: r0c0 = vals[0]; r1c0 = vals[1]; r2c0 = vals[2]; break; + case 1: r0c1 = vals[0]; r1c1 = vals[1]; r2c1 = vals[2]; break; + case 2: r0c2 = vals[0]; r1c2 = vals[1]; r2c2 = vals[2]; break; + default: throw new ArgumentOutOfRangeException(nameof(column)); + } + } + + public double Determinant() => // Alternating sum of the determinants of the first row of submatrices. + r0c0 * (r1c1 * r2c2 - r1c2 * r2c1) - + r0c1 * (r1c0 * r2c2 - r1c2 * r2c0) + + r0c2 * (r1c0 * r2c1 - r1c1 * r2c0); + + public Matrix3x3 Adjoint() => // Transpose(Cofactor) + new Matrix3x3(r1c1 * r2c2 - r1c2 * r2c1, r0c2 * r2c1 - r0c1 * r2c2, r0c1 * r1c2 - r0c2 * r1c1, + r1c2 * r2c0 - r1c0 * r2c2, r0c0 * r2c2 - r0c2 * r2c0, r0c2 * r1c0 - r0c0 * r1c2, + r1c0 * r2c1 - r1c1 * r2c0, r0c1 * r2c0 - r0c0 * r2c1, r0c0 * r1c1 - r0c1 * r1c0); + public Matrix3x3 Cofactor() => // [r, c] = Determinant(Submatrix(r, c)) + new Matrix3x3(r1c1 * r2c2 - r1c2 * r2c1, r1c2 * r2c0 - r1c0 * r2c2, r1c0 * r2c1 - r1c1 * r2c0, + r0c2 * r2c1 - r0c1 * r2c2, r0c0 * r2c2 - r0c2 * r2c0, r0c1 * r2c0 - r0c0 * r2c1, + r0c1 * r1c2 - r0c2 * r1c1, r0c2 * r1c0 - r0c0 * r1c2, r0c0 * r1c1 - r0c1 * r1c0); + public Matrix3x3 Inverse() // Adjoint / Determinant + { + double invDet = 1 / Determinant(); + return new Matrix3x3(invDet * (r1c1 * r2c2 - r1c2 * r2c1), invDet * (r0c2 * r2c1 - r0c1 * r2c2), invDet * (r0c1 * r1c2 - r0c2 * r1c1), + invDet * (r1c2 * r2c0 - r1c0 * r2c2), invDet * (r0c0 * r2c2 - r0c2 * r2c0), invDet * (r0c2 * r1c0 - r0c0 * r1c2), + invDet * (r1c0 * r2c1 - r1c1 * r2c0), invDet * (r0c1 * r2c0 - r0c0 * r2c1), invDet * (r0c0 * r1c1 - r0c1 * r1c0)); + } + public Matrix3x3 Transpose() => + new Matrix3x3(r0c0, r1c0, r2c0, + r0c1, r1c1, r2c1, + r0c2, r1c2, r2c2); + public Matrix2x2 Submatrix(int row, int column) + { + switch (row) + { + case 0: + switch (column) + { + case 0: return new Matrix2x2(r1c1, r1c2, r2c1, r2c2); + case 1: return new Matrix2x2(r1c0, r1c2, r2c0, r2c2); + case 2: return new Matrix2x2(r1c0, r1c1, r2c0, r2c1); + default: throw new ArgumentOutOfRangeException(nameof(column)); + } + case 1: + switch (column) + { + case 0: return new Matrix2x2(r0c1, r0c2, r2c1, r2c2); + case 1: return new Matrix2x2(r0c0, r0c2, r2c0, r2c2); + case 2: return new Matrix2x2(r0c0, r0c1, r2c0, r2c1); + default: throw new ArgumentOutOfRangeException(nameof(column)); + } + case 2: + switch (column) + { + case 0: return new Matrix2x2(r0c1, r0c2, r1c1, r1c2); + case 1: return new Matrix2x2(r0c0, r0c2, r1c0, r1c2); + case 2: return new Matrix2x2(r0c0, r0c1, r1c0, r1c1); + default: throw new ArgumentOutOfRangeException(nameof(column)); + } + default: throw new ArgumentOutOfRangeException(nameof(column)); + } + } + public double Trace() => r0c0 + r1c1 + r2c2; + + public IEnumerator GetEnumerator() + { + yield return r0c0; yield return r0c1; yield return r0c2; + yield return r1c0; yield return r1c1; yield return r1c2; + yield return r2c0; yield return r2c1; yield return r2c2; + } + IEnumerator IEnumerable.GetEnumerator() => GetEnumerator(); + +#if CS8_OR_GREATER + public bool Equals(Matrix3x3? other) => +#else + public bool Equals(Matrix3x3 other) => +#endif + !(other is null) && + r0c0 == other.r0c0 && r0c1 == other.r0c1 && r0c2 == other.r0c2 && + r1c0 == other.r1c0 && r1c1 == other.r1c1 && r1c2 == other.r1c2 && + r2c0 == other.r2c0 && r2c1 == other.r2c1 && r2c2 == other.r2c2; +#if CS8_OR_GREATER + public override bool Equals(object? other) +#else + public override bool Equals(object other) +#endif + { + if (other is null) return false; + else if (other is Matrix3x3 otherMat) return Equals(otherMat); + else return false; + } + public override int GetHashCode() => + (int)((uint)r0c0.GetHashCode() & 0xE0000000 | + (uint)r0c1.GetHashCode() & 0x1E000000 | + (uint)r0c2.GetHashCode() & 0x01C00000 | + (uint)r1c0.GetHashCode() & 0x003C0000 | + (uint)r1c1.GetHashCode() & 0x00038000 | + (uint)r1c2.GetHashCode() & 0x00007800 | + (uint)r2c0.GetHashCode() & 0x00000700 | + (uint)r2c1.GetHashCode() & 0x000000F0 | + (uint)r2c2.GetHashCode() & 0x0000000F); + public override string ToString() => ToStringHelper.MatrixToString(this, null); +#if CS8_OR_GREATER + public string ToString(string? format) => ToStringHelper.MatrixToString(this, format); + public string ToString(string? format, IFormatProvider? provider) => ToStringHelper.MatrixToString(this, format); +#else + public string ToString(string format) => ToStringHelper.MatrixToString(this, format); + public string ToString(string format, IFormatProvider provider) => ToStringHelper.MatrixToString(this, format); +#endif + + public static Matrix3x3 operator +(Matrix3x3 a) => + new Matrix3x3(a.r0c0, a.r0c1, a.r0c2, + a.r1c0, a.r1c1, a.r1c2, + a.r2c0, a.r2c1, a.r2c2); + public static Matrix3x3 operator +(Matrix3x3 a, Matrix3x3 b) => + new Matrix3x3(a.r0c0 + b.r0c0, a.r0c1 + b.r0c1, a.r0c2 + b.r0c2, + a.r1c0 + b.r1c0, a.r1c1 + b.r1c1, a.r1c2 + b.r1c2, + a.r2c0 + b.r2c0, a.r2c1 + b.r2c1, a.r2c2 + b.r2c2); + public static Matrix3x3 operator -(Matrix3x3 a) => + new Matrix3x3(-a.r0c0, -a.r0c1, -a.r0c2, + -a.r1c0, -a.r1c1, -a.r1c2, + -a.r2c0, -a.r2c1, -a.r2c2); + public static Matrix3x3 operator -(Matrix3x3 a, Matrix3x3 b) => + new Matrix3x3(a.r0c0 - b.r0c0, a.r0c1 - b.r0c1, a.r0c2 - b.r0c2, + a.r1c0 - b.r1c0, a.r1c1 - b.r1c1, a.r1c2 - b.r1c2, + a.r2c0 - b.r2c0, a.r2c1 - b.r2c1, a.r2c2 - b.r2c2); + public static Matrix3x3 operator *(Matrix3x3 a, double b) => + new Matrix3x3(a.r0c0 * b, a.r0c1 * b, a.r0c2 * b, + a.r1c0 * b, a.r1c1 * b, a.r1c2 * b, + a.r2c0 * b, a.r2c1 * b, a.r2c2 * b); + public static Float3 operator *(Matrix3x3 a, Float3 b) => + new Float3(a.r0c0 * b.x + a.r0c1 * b.y + a.r0c2 * b.z, + a.r1c0 * b.x + a.r1c1 * b.y + a.r1c2 * b.z, + a.r2c0 * b.x + a.r2c1 * b.y + a.r2c2 * b.z); + public static Matrix3x3 operator *(Matrix3x3 a, Matrix3x3 b) => + new Matrix3x3(a.r0c0 * b.r0c0 + a.r0c1 * b.r1c0 + a.r0c2 * b.r2c0, a.r0c0 * b.r0c1 + a.r0c1 * b.r1c1 + a.r0c2 * b.r2c1, a.r0c0 * b.r0c2 + a.r0c1 * b.r1c2 + a.r0c2 * b.r2c2, + a.r1c0 * b.r0c0 + a.r1c1 * b.r1c0 + a.r1c2 * b.r2c0, a.r1c0 * b.r0c1 + a.r1c1 * b.r1c1 + a.r1c2 * b.r2c1, a.r1c0 * b.r0c2 + a.r1c1 * b.r1c2 + a.r1c2 * b.r2c2, + a.r2c0 * b.r0c0 + a.r2c1 * b.r1c0 + a.r2c2 * b.r2c0, a.r2c0 * b.r0c1 + a.r2c1 * b.r1c1 + a.r2c2 * b.r2c1, a.r2c0 * b.r0c2 + a.r2c1 * b.r1c2 + a.r2c2 * b.r2c2); + public static Matrix3x3 operator /(Matrix3x3 a, double b) => + new Matrix3x3(a.r0c0 / b, a.r0c1 / b, a.r0c2 / b, + a.r1c0 / b, a.r1c1 / b, a.r1c2 / b, + a.r2c0 / b, a.r2c1 / b, a.r2c2 / b); + public static Matrix3x3 operator ~(Matrix3x3 a) => a.Inverse(); + public static bool operator ==(Matrix3x3 a, Matrix3x3 b) => a.Equals(b); + public static bool operator !=(Matrix3x3 a, Matrix3x3 b) => !a.Equals(b); + + public static implicit operator Matrix3x3(Matrix2x2 mat) => + new Matrix3x3(mat.r0c0, mat.r0c1, 0, + mat.r1c0, mat.r1c1, 0, + 0 , 0 , 1); + public static explicit operator Matrix3x3(Matrix4x4 mat) => + new Matrix3x3(mat.r0c0, mat.r0c1, mat.r0c2, + mat.r1c0, mat.r1c1, mat.r1c2, + mat.r2c0, mat.r2c1, mat.r2c2); + } +} diff --git a/Nerd_STF/Mathematics/Algebra/Matrix4x4.cs b/Nerd_STF/Mathematics/Algebra/Matrix4x4.cs new file mode 100644 index 0000000..cfcd758 --- /dev/null +++ b/Nerd_STF/Mathematics/Algebra/Matrix4x4.cs @@ -0,0 +1,562 @@ +using Nerd_STF.Helpers; +using System; +using System.Collections; +using System.Collections.Generic; +using System.Linq; + +namespace Nerd_STF.Mathematics.Algebra +{ + public class Matrix4x4 : IMatrix, + ISquareMatrix, + ISubmatrixOperations +#if CS11_OR_GREATER + ,ISplittable, + IStaticMatrix +#endif + { + public static Matrix4x4 Identity => + new Matrix4x4(1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1); + public static Matrix4x4 SignField => + new Matrix4x4(+1, -1, +1, -1, + -1, +1, -1, +1, + +1, -1, +1, -1, + -1, +1, -1, +1); + + public static Matrix4x4 One => new Matrix4x4(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1); + public static Matrix4x4 Zero => new Matrix4x4(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + + public Int2 Size => (4, 4); + + public double r0c0, r0c1, r0c2, r0c3, + r1c0, r1c1, r1c2, r1c3, + r2c0, r2c1, r2c2, r2c3, + r3c0, r3c1, r3c2, r3c3; + + public Matrix4x4() + { + r0c0 = 0; r0c1 = 0; r0c2 = 0; r0c3 = 0; + r1c0 = 0; r1c1 = 0; r1c2 = 0; r1c3 = 0; + r2c0 = 0; r2c1 = 0; r2c2 = 0; r2c3 = 0; + r3c0 = 0; r3c1 = 0; r3c2 = 0; r3c3 = 0; + } + public Matrix4x4(Matrix4x4 copy) + { + r0c0 = copy.r0c0; r0c1 = copy.r0c1; r0c2 = copy.r0c2; r0c3 = copy.r0c3; + r1c0 = copy.r1c0; r1c1 = copy.r1c1; r1c2 = copy.r1c2; r1c3 = copy.r1c3; + r2c0 = copy.r2c0; r2c1 = copy.r2c1; r2c2 = copy.r2c2; r2c3 = copy.r2c3; + r3c0 = copy.r3c0; r3c1 = copy.r3c1; r3c2 = copy.r3c2; r3c3 = copy.r3c3; + } + public Matrix4x4(double r0c0, double r0c1, double r0c2, double r0c3, double r1c0, double r1c1, double r1c2, double r1c3, double r2c0, double r2c1, double r2c2, double r2c3, double r3c0, double r3c1, double r3c2, double r3c3) + { + this.r0c0 = r0c0; this.r0c1 = r0c1; this.r0c2 = r0c2; this.r0c3 = r0c3; + this.r1c0 = r1c0; this.r1c1 = r1c1; this.r1c2 = r1c2; this.r1c3 = r1c3; + this.r2c0 = r2c0; this.r2c1 = r2c1; this.r2c2 = r2c2; this.r2c3 = r2c3; + this.r3c0 = r3c0; this.r3c1 = r3c1; this.r3c2 = r3c2; this.r3c3 = r3c3; + } + /// if the array is of the form [c, r], if the array is of the form [r, c]. + public Matrix4x4(double[,] vals, bool byRows = true) + { + if (byRows) // Collection of rows ([c, r]) + { + r0c0 = vals[0, 0]; r0c1 = vals[0, 1]; r0c2 = vals[0, 2]; r0c3 = vals[0, 3]; + r1c0 = vals[1, 0]; r1c1 = vals[1, 1]; r1c2 = vals[1, 2]; r1c3 = vals[1, 3]; + r2c0 = vals[2, 0]; r2c1 = vals[2, 1]; r2c2 = vals[2, 2]; r2c3 = vals[2, 3]; + r3c0 = vals[3, 0]; r3c1 = vals[3, 1]; r3c2 = vals[3, 2]; r3c3 = vals[3, 3]; + } + else + { + r0c0 = vals[0, 0]; r0c1 = vals[1, 0]; r0c2 = vals[2, 0]; r0c3 = vals[3, 0]; + r1c0 = vals[0, 1]; r1c1 = vals[1, 1]; r1c2 = vals[2, 1]; r1c3 = vals[3, 1]; + r2c0 = vals[0, 2]; r2c1 = vals[1, 2]; r2c2 = vals[2, 2]; r2c3 = vals[3, 2]; + r3c0 = vals[0, 3]; r3c1 = vals[1, 3]; r3c2 = vals[2, 3]; r3c3 = vals[3, 3]; + } + } + /// if the enumerable is a collection of rows (form [c, r]), if the enumerable is a collection of columns (form [r, c]). + public Matrix4x4(IEnumerable> vals, bool byRows = true) + { + MatrixHelper.SetMatrixValues(this, vals, byRows); + } + /// if the enumerable is a collection of rows (form [c, r]), if the enumerable is a collection of columns (form [r, c]). + public Matrix4x4(IEnumerable> vals, bool byRows = true) + { + MatrixHelper.SetMatrixValues(this, vals, byRows); + } + + public double this[int r, int c] + { + get + { + switch (r) + { + case 0: + switch (c) + { + case 0: return r0c0; + case 1: return r0c1; + case 2: return r0c2; + case 3: return r0c3; + default: throw new ArgumentOutOfRangeException(nameof(c)); + } + case 1: + switch (c) + { + case 0: return r1c0; + case 1: return r1c1; + case 2: return r1c2; + case 3: return r1c3; + default: throw new ArgumentOutOfRangeException(nameof(c)); + } + case 2: + switch (c) + { + case 0: return r2c0; + case 1: return r2c1; + case 2: return r2c2; + case 3: return r2c3; + default: throw new ArgumentOutOfRangeException(nameof(c)); + } + case 3: + switch (c) + { + case 0: return r3c0; + case 1: return r3c1; + case 2: return r3c2; + case 3: return r3c3; + default: throw new ArgumentOutOfRangeException(nameof(c)); + } + default: throw new ArgumentOutOfRangeException(nameof(r)); + } + } + set + { + switch (r) + { + case 0: + switch (c) + { + case 0: r0c0 = value; return; + case 1: r0c1 = value; return; + case 2: r0c2 = value; return; + case 3: r0c3 = value; return; + default: throw new ArgumentOutOfRangeException(nameof(c)); + } + case 1: + switch (c) + { + case 0: r1c0 = value; return; + case 1: r1c1 = value; return; + case 2: r1c2 = value; return; + case 3: r1c3 = value; return; + default: throw new ArgumentOutOfRangeException(nameof(c)); + } + case 2: + switch (c) + { + case 0: r2c0 = value; return; + case 1: r2c1 = value; return; + case 2: r2c2 = value; return; + case 3: r2c3 = value; return; + default: throw new ArgumentOutOfRangeException(nameof(c)); + } + case 3: + switch (c) + { + case 0: r3c0 = value; return; + case 1: r3c1 = value; return; + case 2: r3c2 = value; return; + case 3: r3c3 = value; return; + default: throw new ArgumentOutOfRangeException(nameof(c)); + } + default: throw new ArgumentOutOfRangeException(nameof(r)); + } + } + } + public double this[Int2 index] + { + get => this[index.x, index.y]; + set => this[index.x, index.y] = value; + } + public ListTuple this[int index, RowColumn direction] + { + get + { + switch (direction) + { + case RowColumn.Row: return GetRow(index); + case RowColumn.Column: return GetColumn(index); + default: throw new ArgumentException($"Invalid direction {direction}."); + } + } + set + { + switch (direction) + { + case RowColumn.Row: SetRow(index, value); break; + case RowColumn.Column: SetColumn(index, value); break; + default: throw new ArgumentException($"Invalid direction {direction}."); + } + } + } + + public static Matrix4x4 Average(IEnumerable vals) + { + Matrix4x4 result = Zero; + int count = 0; + foreach (Matrix4x4 m in vals) + { + result += m; + count++; + } + return result / count; + } + public static Matrix4x4 Lerp(Matrix4x4 a, Matrix4x4 b, double t, bool clamp = true) => + new Matrix4x4(MathE.Lerp(a.r0c0, b.r0c0, t, clamp), MathE.Lerp(a.r0c1, b.r0c1, t, clamp), MathE.Lerp(a.r0c2, b.r0c2, t, clamp), MathE.Lerp(a.r0c3, b.r0c3, t, clamp), + MathE.Lerp(a.r1c0, b.r1c0, t, clamp), MathE.Lerp(a.r1c1, b.r1c1, t, clamp), MathE.Lerp(a.r1c2, b.r1c2, t, clamp), MathE.Lerp(a.r1c3, b.r1c3, t, clamp), + MathE.Lerp(a.r2c0, b.r2c0, t, clamp), MathE.Lerp(a.r2c1, b.r2c1, t, clamp), MathE.Lerp(a.r2c2, b.r2c2, t, clamp), MathE.Lerp(a.r2c3, b.r2c3, t, clamp), + MathE.Lerp(a.r3c0, b.r3c0, t, clamp), MathE.Lerp(a.r3c1, b.r3c1, t, clamp), MathE.Lerp(a.r3c2, b.r3c2, t, clamp), MathE.Lerp(a.r3c3, b.r3c3, t, clamp)); + public static Matrix4x4 Product(IEnumerable vals) + { + Matrix4x4 result = One; + bool any = false; + foreach (Matrix4x4 m in vals) + { + any = true; + result *= m; + } + return any ? result : Zero; + } + public static Matrix4x4 Sum(IEnumerable vals) + { + Matrix4x4 result = Zero; + foreach (Matrix4x4 m in vals) result += m; + return result; + } + + public static (double[] r0c0, double[] r0c1, double[] r0c2, double[] r0c3, double[] r1c0, double[] r1c1, double[] r1c2, double[] r1c3, double[] r2c0, double[] r2c1, double[] r2c2, double[] r2c3, double[] r3c0, double[] r3c1, double[] r3c2, double[] r3c3) SplitArray(IEnumerable vals) + { + int count = vals.Count(); + double[] r0c0 = new double[count], r0c1 = new double[count], r0c2 = new double[count], r0c3 = new double[count], + r1c0 = new double[count], r1c1 = new double[count], r1c2 = new double[count], r1c3 = new double[count], + r2c0 = new double[count], r2c1 = new double[count], r2c2 = new double[count], r2c3 = new double[count], + r3c0 = new double[count], r3c1 = new double[count], r3c2 = new double[count], r3c3 = new double[count]; + int index = 0; + foreach (Matrix4x4 m in vals) + { + r0c0[index] = m.r0c0; r0c1[index] = m.r0c1; r0c2[index] = m.r0c2; r0c3[index] = m.r0c3; + r1c0[index] = m.r1c0; r1c1[index] = m.r1c1; r1c2[index] = m.r1c2; r1c3[index] = m.r1c3; + r2c0[index] = m.r2c0; r2c1[index] = m.r2c1; r2c2[index] = m.r2c2; r2c3[index] = m.r2c3; + r3c0[index] = m.r3c0; r3c1[index] = m.r3c1; r3c2[index] = m.r3c2; r3c3[index] = m.r3c3; + } + return (r0c0, r0c1, r0c2, r0c3, + r1c0, r1c1, r1c2, r1c3, + r2c0, r2c1, r2c2, r2c3, + r3c0, r3c1, r3c2, r3c3); + } + + public ListTuple GetRow(int row) + { + double[] vals; + switch (row) + { + case 0: vals = new double[] { r0c0, r0c1, r0c2, r0c3 }; break; + case 1: vals = new double[] { r1c0, r1c1, r1c2, r1c3 }; break; + case 2: vals = new double[] { r2c0, r2c1, r2c2, r2c3 }; break; + case 3: vals = new double[] { r3c0, r3c1, r3c2, r3c3 }; break; + default: throw new ArgumentOutOfRangeException(nameof(row)); + } + return new ListTuple(vals); + } + public ListTuple GetColumn(int column) + { + double[] vals; + switch (column) + { + case 0: vals = new double[] { r0c0, r1c0, r2c0, r3c0 }; break; + case 1: vals = new double[] { r0c1, r1c1, r2c1, r3c1 }; break; + case 2: vals = new double[] { r0c2, r1c2, r2c2, r3c2 }; break; + case 3: vals = new double[] { r0c3, r1c3, r2c3, r3c3 }; break; + default: throw new ArgumentOutOfRangeException(nameof(column)); + } + return new ListTuple(vals); + } + public void SetRow(int row, IEnumerable vals) => MatrixHelper.SetRow(this, row, vals); + public void SetColumn(int column, IEnumerable vals) => MatrixHelper.SetColumn(this, column, vals); + public void SetRow(int row, ListTuple vals) + { + switch (row) + { + case 0: r0c0 = vals[0]; r0c1 = vals[1]; r0c2 = vals[2]; r0c3 = vals[3]; break; + case 1: r1c0 = vals[0]; r1c1 = vals[1]; r1c2 = vals[2]; r1c3 = vals[3]; break; + case 2: r2c0 = vals[0]; r2c1 = vals[1]; r2c2 = vals[2]; r2c3 = vals[3]; break; + case 3: r3c0 = vals[0]; r3c1 = vals[1]; r3c2 = vals[2]; r3c3 = vals[3]; break; + default: throw new ArgumentOutOfRangeException(nameof(row)); + } + } + public void SetColumn(int column, ListTuple vals) + { + switch (column) + { + case 0: r0c0 = vals[0]; r1c0 = vals[1]; r2c0 = vals[2]; r3c0 = vals[3]; break; + case 1: r0c1 = vals[0]; r1c1 = vals[1]; r2c1 = vals[2]; r3c1 = vals[3]; break; + case 2: r0c2 = vals[0]; r1c2 = vals[1]; r2c2 = vals[2]; r3c2 = vals[3]; break; + case 3: r0c3 = vals[0]; r1c3 = vals[1]; r2c3 = vals[2]; r3c3 = vals[3]; break; + default: throw new ArgumentOutOfRangeException(nameof(column)); + } + } + + // Sorry some of these are huge. I just want to inline + // it as much as physically possible. + public double Determinant() + { + double A = r2c2 * r3c3 - r2c3 * r3c2, + B = r2c1 * r3c3 - r2c3 * r3c1, + C = r2c1 * r3c2 - r2c2 * r3c1, + D = r2c0 * r3c3 - r2c3 * r3c0, + E = r2c0 * r3c2 - r2c2 * r3c0, + F = r2c0 * r3c1 - r2c1 * r3c0; + return r0c0 * (r1c1 * A - r1c2 * B + r1c3 * C) - + r0c1 * (r1c0 * A - r1c2 * D + r1c3 * E) + + r0c2 * (r1c0 * B - r1c1 * D + r1c3 * F) - + r0c3 * (r1c0 * C - r1c1 * E + r1c2 * F); + } + + public Matrix4x4 Adjoint() // Transpose(Cofactor) + { + double A = r2c2 * r3c3 - r2c3 * r3c2, + B = r2c1 * r3c3 - r2c3 * r3c1, + C = r2c1 * r3c2 - r2c2 * r3c1, + D = r2c0 * r3c3 - r2c3 * r3c0, + E = r2c0 * r3c2 - r2c2 * r3c0, + F = r2c0 * r3c1 - r2c1 * r3c0, + G = r1c2 * r3c3 - r1c3 * r3c2, + H = r1c1 * r3c3 - r1c3 * r3c1, + I = r1c1 * r3c2 - r1c2 * r3c1, + J = r1c0 * r3c3 - r1c3 * r3c0, + K = r1c0 * r3c2 - r1c2 * r3c0, + L = r1c0 * r3c1 - r1c1 * r3c0, + M = r1c2 * r2c3 - r1c3 * r2c2, + N = r1c1 * r2c3 - r1c3 * r2c1, + O = r1c1 * r2c2 - r1c2 * r2c1, + P = r1c0 * r2c3 - r1c3 * r2c0, + Q = r1c0 * r2c2 - r1c2 * r2c0, + R = r1c0 * r2c1 - r1c1 * r2c0; + return new Matrix4x4(r1c1 * A - r1c2 * B + r1c3 * C, r0c2 * B - r0c3 * C - r0c1 * A, r0c1 * G - r0c2 * H + r0c3 * I, r0c2 * N - r0c3 * O - r0c1 * M, + r1c2 * D - r1c3 * E - r1c0 * A, r0c0 * A - r0c2 * D + r0c3 * E, r0c2 * J - r0c3 * K - r0c0 * G, r0c0 * M - r0c2 * P + r0c3 * Q, + r1c0 * B - r1c1 * D + r1c3 * F, r0c1 * D - r0c3 * F - r0c0 * B, r0c0 * H - r0c1 * J + r0c3 * L, r0c1 * P - r0c3 * R - r0c0 * N, + r1c1 * E - r1c2 * F - r1c0 * C, r0c0 * C - r0c1 * E + r0c2 * F, r0c1 * K - r0c2 * L - r0c0 * I, r0c0 * O - r0c1 * Q + r0c2 * R); + } + public Matrix4x4 Cofactor() // [r, c] = Determinant(Submatrix(r, c)) + { + double A = r2c2 * r3c3 - r2c3 * r3c2, + B = r2c1 * r3c3 - r2c3 * r3c1, + C = r2c1 * r3c2 - r2c2 * r3c1, + D = r2c0 * r3c3 - r2c3 * r3c0, + E = r2c0 * r3c2 - r2c2 * r3c0, + F = r2c0 * r3c1 - r2c1 * r3c0, + G = r1c2 * r3c3 - r1c3 * r3c2, + H = r1c1 * r3c3 - r1c3 * r3c1, + I = r1c1 * r3c2 - r1c2 * r3c1, + J = r1c0 * r3c3 - r1c3 * r3c0, + K = r1c0 * r3c2 - r1c2 * r3c0, + L = r1c0 * r3c1 - r1c1 * r3c0, + M = r1c2 * r2c3 - r1c3 * r2c2, + N = r1c1 * r2c3 - r1c3 * r2c1, + O = r1c1 * r2c2 - r1c2 * r2c1, + P = r1c0 * r2c3 - r1c3 * r2c0, + Q = r1c0 * r2c2 - r1c2 * r2c0, + R = r1c0 * r2c1 - r1c1 * r2c0; + return new Matrix4x4(r1c1 * A - r1c2 * B + r1c3 * C, r1c2 * D - r1c3 * E - r1c0 * A, r1c0 * B - r1c1 * D + r1c3 * F, r1c1 * E - r1c2 * F - r1c0 * C, + r0c2 * B - r0c3 * C - r0c1 * A, r0c0 * A - r0c2 * D + r0c3 * E, r0c1 * D - r0c3 * F - r0c0 * B, r0c0 * C - r0c1 * E + r0c2 * F, + r0c1 * G - r0c2 * H + r0c3 * I, r0c2 * J - r0c3 * K - r0c0 * G, r0c0 * H - r0c1 * J + r0c3 * L, r0c1 * K - r0c2 * L - r0c0 * I, + r0c2 * N - r0c3 * O - r0c1 * M, r0c0 * M - r0c2 * P + r0c3 * Q, r0c1 * P - r0c3 * R - r0c0 * N, r0c0 * O - r0c1 * Q + r0c2 * R); + } + public Matrix4x4 Inverse() // Adjoint / Determinant() + { + double invDet = 1 / Determinant(), + A = r2c2 * r3c3 - r2c3 * r3c2, + B = r2c1 * r3c3 - r2c3 * r3c1, + C = r2c1 * r3c2 - r2c2 * r3c1, + D = r2c0 * r3c3 - r2c3 * r3c0, + E = r2c0 * r3c2 - r2c2 * r3c0, + F = r2c0 * r3c1 - r2c1 * r3c0, + G = r1c2 * r3c3 - r1c3 * r3c2, + H = r1c1 * r3c3 - r1c3 * r3c1, + I = r1c1 * r3c2 - r1c2 * r3c1, + J = r1c0 * r3c3 - r1c3 * r3c0, + K = r1c0 * r3c2 - r1c2 * r3c0, + L = r1c0 * r3c1 - r1c1 * r3c0, + M = r1c2 * r2c3 - r1c3 * r2c2, + N = r1c1 * r2c3 - r1c3 * r2c1, + O = r1c1 * r2c2 - r1c2 * r2c1, + P = r1c0 * r2c3 - r1c3 * r2c0, + Q = r1c0 * r2c2 - r1c2 * r2c0, + R = r1c0 * r2c1 - r1c1 * r2c0; + return new Matrix4x4(invDet * (r1c1 * A - r1c2 * B + r1c3 * C), invDet * (r0c2 * B - r0c3 * C - r0c1 * A), invDet * (r0c1 * G - r0c2 * H + r0c3 * I), invDet * (r0c2 * N - r0c3 * O - r0c1 * M), + invDet * (r1c2 * D - r1c3 * E - r1c0 * A), invDet * (r0c0 * A - r0c2 * D + r0c3 * E), invDet * (r0c2 * J - r0c3 * K - r0c0 * G), invDet * (r0c0 * M - r0c2 * P + r0c3 * Q), + invDet * (r1c0 * B - r1c1 * D + r1c3 * F), invDet * (r0c1 * D - r0c3 * F - r0c0 * B), invDet * (r0c0 * H - r0c1 * J + r0c3 * L), invDet * (r0c1 * P - r0c3 * R - r0c0 * N), + invDet * (r1c1 * E - r1c2 * F - r1c0 * C), invDet * (r0c0 * C - r0c1 * E + r0c2 * F), invDet * (r0c1 * K - r0c2 * L - r0c0 * I), invDet * (r0c0 * O - r0c1 * Q + r0c2 * R)); + } + public Matrix4x4 Transpose() => + new Matrix4x4(r0c0, r1c0, r2c0, r3c0, + r0c1, r1c1, r2c1, r3c1, + r0c2, r1c2, r2c2, r3c2, + r0c3, r1c3, r2c3, r3c3); + public Matrix3x3 Submatrix(int row, int column) + { + switch (row) + { + case 0: + switch (column) + { + case 0: return new Matrix3x3(r1c1, r1c2, r1c3, r2c1, r2c2, r2c3, r3c1, r3c2, r3c3); + case 1: return new Matrix3x3(r1c0, r1c2, r1c3, r2c0, r2c2, r2c3, r3c0, r3c2, r3c3); + case 2: return new Matrix3x3(r1c0, r1c1, r1c3, r2c0, r2c1, r2c3, r3c0, r3c1, r3c3); + case 3: return new Matrix3x3(r1c0, r1c1, r1c2, r2c0, r2c1, r2c2, r3c0, r3c1, r3c2); + default: throw new ArgumentOutOfRangeException(nameof(column)); + } + case 1: + switch (column) + { + case 0: return new Matrix3x3(r0c1, r0c2, r0c3, r2c1, r2c2, r2c3, r3c1, r3c2, r3c3); + case 1: return new Matrix3x3(r0c0, r0c2, r0c3, r2c0, r2c2, r2c3, r3c0, r3c2, r3c3); + case 2: return new Matrix3x3(r0c0, r0c1, r0c3, r2c0, r2c1, r2c3, r3c0, r3c1, r3c3); + case 3: return new Matrix3x3(r0c0, r0c1, r0c2, r2c0, r2c1, r2c2, r3c0, r3c1, r3c2); + default: throw new ArgumentOutOfRangeException(nameof(column)); + } + case 2: + switch (column) + { + case 0: return new Matrix3x3(r0c1, r0c2, r0c3, r1c1, r1c2, r1c3, r3c1, r3c2, r3c3); + case 1: return new Matrix3x3(r0c0, r0c2, r0c3, r1c0, r1c2, r1c3, r3c0, r3c2, r3c3); + case 2: return new Matrix3x3(r0c0, r0c1, r0c3, r1c0, r1c1, r1c3, r3c0, r3c1, r3c3); + case 3: return new Matrix3x3(r0c0, r0c1, r0c2, r1c0, r1c1, r1c2, r3c0, r3c1, r3c2); + default: throw new ArgumentOutOfRangeException(nameof(column)); + } + case 3: + switch (column) + { + case 0: return new Matrix3x3(r0c1, r0c2, r0c3, r1c1, r1c2, r1c3, r2c1, r2c2, r2c3); + case 1: return new Matrix3x3(r0c0, r0c2, r0c3, r1c0, r1c2, r1c3, r2c0, r2c2, r2c3); + case 2: return new Matrix3x3(r0c0, r0c1, r0c3, r1c0, r1c1, r1c3, r2c0, r2c1, r2c3); + case 3: return new Matrix3x3(r0c0, r0c1, r0c2, r1c0, r1c1, r1c2, r2c0, r2c1, r2c2); + default: throw new ArgumentOutOfRangeException(nameof(column)); + } + default: throw new ArgumentOutOfRangeException(nameof(row)); + } + } + public double Trace() => r0c0 + r1c1 + r2c2 + r3c3; + + public IEnumerator GetEnumerator() + { + yield return r0c0; yield return r0c1; yield return r0c2; yield return r0c3; + yield return r1c0; yield return r1c1; yield return r1c2; yield return r1c3; + yield return r2c0; yield return r2c1; yield return r2c2; yield return r2c3; + yield return r3c0; yield return r3c1; yield return r3c2; yield return r3c3; + } + IEnumerator IEnumerable.GetEnumerator() => GetEnumerator(); + +#if CS8_OR_GREATER + public bool Equals(Matrix4x4? other) => +#else + public bool Equals(Matrix4x4 other) => +#endif + !(other is null) && + r0c0 == other.r0c0 && r0c1 == other.r0c1 && r0c2 == other.r0c2 && r0c3 == other.r0c3 && + r1c0 == other.r1c0 && r1c1 == other.r1c1 && r1c2 == other.r1c2 && r1c3 == other.r1c3 && + r2c0 == other.r2c0 && r2c1 == other.r2c1 && r2c2 == other.r2c2 && r2c3 == other.r2c3 && + r3c0 == other.r3c0 && r3c1 == other.r3c1 && r3c2 == other.r3c2 && r3c3 == other.r3c3; +#if CS8_OR_GREATER + public override bool Equals(object? other) +#else + public override bool Equals(object other) +#endif + { + if (other is null) return false; + else if (other is Matrix4x4 otherMat) return Equals(otherMat); + else return false; + } + public override int GetHashCode() => + (int)((uint)r0c0.GetHashCode() & 0xC0000000 | + (uint)r0c1.GetHashCode() & 0x30000000 | + (uint)r0c2.GetHashCode() & 0x0C000000 | + (uint)r0c3.GetHashCode() & 0x03000000 | + (uint)r1c0.GetHashCode() & 0x00C00000 | + (uint)r1c1.GetHashCode() & 0x00300000 | + (uint)r1c2.GetHashCode() & 0x000C0000 | + (uint)r1c3.GetHashCode() & 0x00030000 | + (uint)r2c0.GetHashCode() & 0x0000C000 | + (uint)r2c1.GetHashCode() & 0x00003000 | + (uint)r2c2.GetHashCode() & 0x00000C00 | + (uint)r2c3.GetHashCode() & 0x00000300 | + (uint)r3c0.GetHashCode() & 0x000000C0 | + (uint)r3c1.GetHashCode() & 0x00000030 | + (uint)r3c2.GetHashCode() & 0x0000000C | + (uint)r3c3.GetHashCode() & 0x00000003); + public override string ToString() => ToStringHelper.MatrixToString(this, null); +#if CS8_OR_GREATER + public string ToString(string? format) => ToStringHelper.MatrixToString(this, format); + public string ToString(string? format, IFormatProvider? provider) => ToStringHelper.MatrixToString(this, format); +#else + public string ToString(string format) => ToStringHelper.MatrixToString(this, format); + public string ToString(string format, IFormatProvider provider) => ToStringHelper.MatrixToString(this, format); +#endif + + public static Matrix4x4 operator +(Matrix4x4 a) => + new Matrix4x4(a.r0c0, a.r0c1, a.r0c2, a.r0c3, + a.r1c0, a.r1c1, a.r1c2, a.r1c3, + a.r2c0, a.r2c1, a.r2c2, a.r2c3, + a.r3c0, a.r3c1, a.r3c2, a.r3c3); + public static Matrix4x4 operator +(Matrix4x4 a, Matrix4x4 b) => + new Matrix4x4(a.r0c0 + b.r0c0, a.r0c1 + b.r0c1, a.r0c2 + b.r0c2, a.r0c3 + b.r0c3, + a.r1c0 + b.r1c0, a.r1c1 + b.r1c1, a.r1c2 + b.r1c2, a.r1c3 + b.r1c3, + a.r2c0 + b.r2c0, a.r2c1 + b.r2c1, a.r2c2 + b.r2c2, a.r2c3 + b.r2c3, + a.r3c0 + b.r3c0, a.r3c1 + b.r3c1, a.r3c2 + b.r3c2, a.r3c3 + b.r3c3); + public static Matrix4x4 operator -(Matrix4x4 a) => + new Matrix4x4(-a.r0c0, -a.r0c1, -a.r0c2, -a.r0c3, + -a.r1c0, -a.r1c1, -a.r1c2, -a.r1c3, + -a.r2c0, -a.r2c1, -a.r2c2, -a.r2c3, + -a.r3c0, -a.r3c1, -a.r3c2, -a.r3c3); + public static Matrix4x4 operator -(Matrix4x4 a, Matrix4x4 b) => + new Matrix4x4(a.r0c0 - b.r0c0, a.r0c1 - b.r0c1, a.r0c2 - b.r0c2, a.r0c3 - b.r0c3, + a.r1c0 - b.r1c0, a.r1c1 - b.r1c1, a.r1c2 - b.r1c2, a.r1c3 - b.r1c3, + a.r2c0 - b.r2c0, a.r2c1 - b.r2c1, a.r2c2 - b.r2c2, a.r2c3 - b.r2c3, + a.r3c0 - b.r3c0, a.r3c1 - b.r3c1, a.r3c2 - b.r3c2, a.r3c3 - b.r3c3); + public static Matrix4x4 operator *(Matrix4x4 a, double b) => + new Matrix4x4(a.r0c0 * b, a.r0c1 * b, a.r0c2 * b, a.r0c3 * b, + a.r1c0 * b, a.r1c1 * b, a.r1c2 * b, a.r1c3 * b, + a.r2c0 * b, a.r2c1 * b, a.r2c2 * b, a.r2c3 * b, + a.r3c0 * b, a.r3c1 * b, a.r3c2 * b, a.r3c3 * b); + public static Float4 operator *(Matrix4x4 a, Float4 b) => + new Float4(a.r0c0 * b.w + a.r0c1 * b.x + a.r0c2 * b.y + a.r0c3 * b.z, + a.r1c0 * b.w + a.r1c1 * b.x + a.r1c2 * b.y + a.r1c3 * b.z, + a.r2c0 * b.w + a.r2c1 * b.x + a.r2c2 * b.y + a.r2c3 * b.z, + a.r3c0 * b.w + a.r3c1 * b.x + a.r3c2 * b.y + a.r3c3 * b.z); + public static Matrix4x4 operator *(Matrix4x4 a, Matrix4x4 b) => + new Matrix4x4(a.r0c0 * b.r0c0 + a.r0c1 * b.r1c0 + a.r0c2 * b.r2c0 + a.r0c3 * b.r3c0, a.r0c0 * b.r0c1 + a.r0c1 * b.r1c1 + a.r0c2 * b.r2c1 + a.r0c3 * b.r3c1, a.r0c0 * b.r0c2 + a.r0c1 * b.r1c2 + a.r0c2 * b.r2c2 + a.r0c3 * b.r3c2, a.r0c0 * b.r0c3 + a.r0c1 * b.r1c3 + a.r0c2 * b.r2c3 + a.r0c3 * b.r3c3, + a.r1c0 * b.r0c0 + a.r1c1 * b.r1c0 + a.r1c2 * b.r2c0 + a.r1c3 * b.r3c0, a.r1c0 * b.r0c1 + a.r1c1 * b.r1c1 + a.r1c2 * b.r2c1 + a.r1c3 * b.r3c1, a.r1c0 * b.r0c2 + a.r1c1 * b.r1c2 + a.r1c2 * b.r2c2 + a.r1c3 * b.r3c2, a.r1c0 * b.r0c3 + a.r1c1 * b.r1c3 + a.r1c2 * b.r2c3 + a.r1c3 * b.r3c3, + a.r2c0 * b.r0c0 + a.r2c1 * b.r1c0 + a.r2c2 * b.r2c0 + a.r2c3 * b.r3c0, a.r2c0 * b.r0c1 + a.r2c1 * b.r1c1 + a.r2c2 * b.r2c1 + a.r2c3 * b.r3c1, a.r2c0 * b.r0c2 + a.r2c1 * b.r1c2 + a.r2c2 * b.r2c2 + a.r2c3 * b.r3c2, a.r2c0 * b.r0c3 + a.r2c1 * b.r1c3 + a.r2c2 * b.r2c3 + a.r2c3 * b.r3c3, + a.r3c0 * b.r0c0 + a.r3c1 * b.r1c0 + a.r3c2 * b.r2c0 + a.r3c3 * b.r3c0, a.r3c0 * b.r0c1 + a.r3c1 * b.r1c1 + a.r3c2 * b.r2c1 + a.r3c3 * b.r3c1, a.r3c0 * b.r0c2 + a.r3c1 * b.r1c2 + a.r3c2 * b.r2c2 + a.r3c3 * b.r3c2, a.r3c0 * b.r0c3 + a.r3c1 * b.r1c3 + a.r3c2 * b.r2c3 + a.r3c3 * b.r3c3); + public static Matrix4x4 operator /(Matrix4x4 a, double b) => + new Matrix4x4(a.r0c0 / b, a.r0c1 / b, a.r0c2 / b, a.r0c3 / b, + a.r1c0 / b, a.r1c1 / b, a.r1c2 / b, a.r1c3 / b, + a.r2c0 / b, a.r2c1 / b, a.r2c2 / b, a.r2c3 / b, + a.r3c0 / b, a.r3c1 / b, a.r3c2 / b, a.r3c3 / b); + public static Matrix4x4 operator ~(Matrix4x4 a) => a.Inverse(); + public static bool operator ==(Matrix4x4 a, Matrix4x4 b) => a.Equals(b); + public static bool operator !=(Matrix4x4 a, Matrix4x4 b) => !a.Equals(b); + + public static implicit operator Matrix4x4(Matrix2x2 mat) => + new Matrix4x4(1, 0 , 0 , 0, + 0, mat.r0c0, mat.r0c1, 0, + 0, mat.r1c0, mat.r1c1, 0, + 0, 0 , 0 , 1); + public static implicit operator Matrix4x4(Matrix3x3 mat) => + new Matrix4x4(1, 0 , 0 , 0 , + 0, mat.r0c0, mat.r0c1, mat.r0c2, + 0, mat.r1c0, mat.r1c1, mat.r1c2, + 0, mat.r2c0, mat.r2c1, mat.r2c2); + } +} diff --git a/Nerd_STF/Mathematics/MathE.cs b/Nerd_STF/Mathematics/MathE.cs index f05401d..e557d47 100644 --- a/Nerd_STF/Mathematics/MathE.cs +++ b/Nerd_STF/Mathematics/MathE.cs @@ -342,22 +342,8 @@ namespace Nerd_STF.Mathematics return -1; // Will only get here if there are negative numbers in the collection. } - public static unsafe double InverseSqrt(double num) - { - return InverseSqrt((float)num); // temp while I fix this. - - // My variation of the method below for doubles. - // Not much has changed, just the funny constant. - - long raw = *(long*)# - double half = num * 0.5; - raw = 0x5FE6EB3BDFFFFF36L - (raw >> 1); - num = *(double*)&raw; - - num *= 1.5 * (half * num * num); // Newton's method. - return num; - } - public static unsafe float InverseSqrt(float num) + public static double InverseSqrt(double num) => 1 / Sqrt(num); + public static unsafe float InverseSqrtFast(float num) { // I think we all know this function. Code structure // has changed (ported), but the idea is exactly the @@ -685,7 +671,7 @@ namespace Nerd_STF.Mathematics public static IEquation Cot(IEquation inputRad, int terms = 8) => new Equation((double x) => Cot(inputRad[x], terms)); - public static double Sqrt(double num) => 1 / InverseSqrt(num); // !!TODO!!: Bring back Newton's + public static double Sqrt(double num) => 1 / InverseSqrtFast((float)num); // !!TODO!!: Bring back Newton's public static IEquation Sqrt(IEquation equ) => new Equation((double x) => Sqrt(equ.Get(x))); diff --git a/Nerd_STF/Nerd_STF.csproj b/Nerd_STF/Nerd_STF.csproj index 279ea3e..0a1e1cf 100644 --- a/Nerd_STF/Nerd_STF.csproj +++ b/Nerd_STF/Nerd_STF.csproj @@ -3,7 +3,7 @@ - netstandard1.1;netstandard1.3;netstandard2.1;net471;netcoreapp3.0;net5.0;net7.0 + netstandard1.1;netstandard1.3;netstandard2.1;netcoreapp3.0;net5.0;net7.0 true True portable @@ -142,19 +142,19 @@ Anyway, that's most of the big changes! I don't know if I'll do the full changel - - - - - + + + - + + +