Added matrix inversion

readme.md

@@ -5211,12 +5211,18 @@ Maths utilities for manipulating vectors and matrices.
 | Function | Description |
 |:--- |:--- |
 | ```angle_between(v1, v2)``` | Return the angle between two vectors |
+| ```augment(m)``` | Augment a matrix by adding an identity matrix to the right |
 | ```euler(R)``` | Convert a rotation matrix to a Euler rotation vector. |
 | ```identity(n, x = 1)``` | Construct an arbitrary size identity matrix |
+| ```invert(m)``` | Invert a matrix |
+| ```nearly_zero(x)``` | True if x is close to zero |
 | ```reverse(v)``` | Reverse a vector |
 | ```rot3_z(a)``` | Generate a 3x3 matrix to rotate around z |
 | ```rotate(a, v)``` | Generate a 4x4 rotation matrix, ```a``` can be a vector of three angles or a single angle around ```z```, or around axis ```v``` |
+| ```rowswap(m, i, j)``` | Swap two rows of a matrix |
 | ```scale(v)``` |  Generate a 4x4 matrix that scales by ```v```, which can be a vector of xyz factors or a scalar to scale all axes equally |
+| ```solve(m, i = 0, j = 0)``` | Solve each row ensuring diagonal is not zero |
+| ```solve_row(m, i)``` | Make diagonal one by dividing the row by it and subtract from other rows to make column zero |
 | ```transform(v, m)``` | Apply 4x4 transform to a 3 vector by extending it and cropping it again |
 | ```transform_points(path, m)``` | Apply transform to a path |
 | ```translate(v)``` | Generate a 4x4 translation matrix, ```v``` can be ```[x, y]```, ```[x, y, z]``` or ```z``` |

utils/maths.scad

@@ -90,3 +90,34 @@ function euler(R) = let(ay = asin(-R[2][0]), cy = cos(ay)) //! Convert a rotatio
     cy ? [ atan2(R[2][1] / cy, R[2][2] / cy), ay, atan2(R[1][0] / cy, R[0][0] / cy) ]
        : R[2][0] < 0 ? [atan2( R[0][1],  R[0][2]),  180, 0]
                      : [atan2(-R[0][1], -R[0][2]), -180, 0];
+// Matrix inversion: https://www.mathsisfun.com/algebra/matrix-inverse-row-operations-gauss-jordan.html
+
+function augment(m) = let(l = len(m), n = identity(l)) [ //! Augment a matrix by adding an identity matrix to the right
+    for(i = [0 : l - 1])
+        concat(m[i], n[i])
+];
+
+function rowswap(m, i, j) = [ //! Swap two rows of a matrix
+    for(k = [0 : len(m) - 1])
+        k == i ? m[j] : k == j ? m[i] : m[k]
+];
+
+function solve_row(m, i) = let(diag = m[i][i]) [ //! Make diagonal one by dividing the row by it and subtract from other rows to make column zero
+    for(j = [0 : len(m) - 1])
+        i == j ? m[j] / diag : m[j] - m[i] * m[j][i] / diag
+];
+
+function nearly_zero(x) = abs(x) < 1e-5; //! True if x is close to zero
+
+function solve(m, i = 0, j = 0) = //! Solve each row ensuring diagonal is not zero
+    i < len(m) ?
+        assert(i + j < len(m), "matrix is singular")
+         solve(!nearly_zero(m[i + j][i]) ? solve_row(j ? rowswap(m, i, i + j) : m, i) : solve(m, i, j + 1), i + 1)
+        : m;
+
+function invert(m) = let(n =len(m), m = solve(augment(m))) [ //! Invert a matrix
+    for(i = [0 : n - 1]) [
+        for(j = [n : 2 * n - 1])
+             each m[i][j]
+    ]
+];