From 399c40f7a6fd9b0d48c02e2ac9e26d629beeb7c5 Mon Sep 17 00:00:00 2001
From: Adrian Mariano <avm4@cornell.edu>
Date: Tue, 1 Sep 2020 18:38:31 -0400
Subject: [PATCH] Added null_space and diagonal_matrix

---
 arrays.scad            | 10 ++++++++++
 math.scad              | 17 +++++++++++++++++
 tests/test_arrays.scad |  8 ++++++++
 tests/test_math.scad   | 23 +++++++++++++++++++++++
 4 files changed, 58 insertions(+)

diff --git a/arrays.scad b/arrays.scad
index 4d22be2..8f44fe8 100644
--- a/arrays.scad
+++ b/arrays.scad
@@ -1216,6 +1216,16 @@ function block_matrix(M) =
     assert(badrows==[], "Inconsistent or invalid input")
     bigM;
 
+// Function: diagonal_matrix()
+// Usage:
+//   diagonal_matrix(diag, [offdiag])
+// Description:
+//   Creates a square matrix with the items in the list `diag` on
+//   its diagonal.  The off diagonal entries are set to offdiag,
+//   which is zero by default. 
+function diagonal_matrix(diag,offdiag=0) =
+  [for(i=[0:1:len(diag)-1]) [for(j=[0:len(diag)-1]) i==j?diag[i] : offdiag]];
+
 
 // Function: submatrix_set()
 // Usage: submatrix_set(M,A,[m],[n])
diff --git a/math.scad b/math.scad
index a66e1cf..6e4be5c 100644
--- a/math.scad
+++ b/math.scad
@@ -712,6 +712,23 @@ function matrix_inverse(A) =
     assert(is_matrix(A,square=true),"Input to matrix_inverse() must be a square matrix")
     linear_solve(A,ident(len(A)));
 
+// Function: null_space()
+// Usage:
+//   null_space(A)
+// Description:
+//   Returns an orthonormal basis for the null space of A, namely the vectors {x} such that Ax=0.  If the null space
+//   is just the origin then returns an empty list. 
+function null_space(A,eps=1e-12) =
+    assert(is_matrix(A))
+    let(
+      Q_R=qr_factor(transpose(A),pivot=true),
+      R=Q_R[1],
+      zrow = [for(i=idx(R)) if (is_zero(R[i],eps)) i]
+    )
+    len(zrow)==0
+      ? []
+      : transpose(subindex(Q_R[0],zrow));
+
 
 // Function: qr_factor()
 // Usage: qr = qr_factor(A,[pivot])
diff --git a/tests/test_arrays.scad b/tests/test_arrays.scad
index eaec5b3..1dfe421 100644
--- a/tests/test_arrays.scad
+++ b/tests/test_arrays.scad
@@ -481,6 +481,14 @@ module test_block_matrix() {
 test_block_matrix();
 
 
+module test_diagonal_matrix() {
+    assert_equal(diagonal_matrix([1,2,3]), [[1,0,0],[0,2,0],[0,0,3]]);
+    assert_equal(diagonal_matrix([1,"c",2]), [[1,0,0],[0,"c",0],[0,0,2]]);
+    assert_equal(diagonal_matrix([1,"c",2],"X"), [[1,"X","X"],["X","c","X"],["X","X",2]]);
+    assert_equal(diagonal_matrix([[1,1],[2,2],[3,3]], [0,0]), [[ [1,1],[0,0],[0,0]], [[0,0],[2,2],[0,0]], [[0,0],[0,0],[3,3]]]);
+}
+test_diagonal_matrix();
+
 module test_submatrix_set() {
     test = [[1,2,3,4,5],[6,7,8,9,10],[11,12,13,14,15], [16,17,18,19,20]];
     ragged = [[1,2,3,4,5],[6,7,8,9,10],[11,12], [16,17]];
diff --git a/tests/test_math.scad b/tests/test_math.scad
index 7b53cc6..9a25cec 100644
--- a/tests/test_math.scad
+++ b/tests/test_math.scad
@@ -969,6 +969,29 @@ module test_quadratic_roots(){
 }
 test_quadratic_roots();
 
+
+module test_null_space(){
+    assert_equal(null_space([[3,2,1],[3,6,3],[3,9,-3]]),[]);
+
+    function nullcheck(A,dim) =
+      let(v=null_space(A))
+        len(v)==dim && is_zero(A*transpose(v),eps=1e-12);
+    
+   A = [[-1, 2, -5, 2],[-3,-1,3,-3],[5,0,5,0],[3,-4,11,-4]];
+   assert(nullcheck(A,1));
+
+   B = [
+        [  4,    1,    8,    6,   -2,    3],
+        [ 10,    5,   10,   10,    0,    5],
+        [  8,    1,    8,    8,   -6,    1],
+        [ -8,   -8,    6,   -1,   -8,   -1],
+        [  2,    2,    0,    1,    2,    1],
+        [  2,   -3,   10,    6,   -8,    1],
+       ];
+   assert(nullcheck(B,3));
+}
+test_null_space();
+
 module test_qr_factor() {
   // Check that R is upper triangular
   function is_ut(R) =