From 8fc3af02649f8577352ded69ee3b4ea3734e0062 Mon Sep 17 00:00:00 2001 From: Adrian Mariano <avm4@cornell.edu> Date: Tue, 17 Mar 2020 07:11:25 -0400 Subject: [PATCH] modified linear solvers to handle matrix RHS, added error checking to lerp and affine_frame_map, adde same_shape(), added square option to is_matrix. --- affine.scad | 14 ++++++++---- common.scad | 11 ++++++++++ math.scad | 61 ++++++++++++++++++++++++++++++++++++++--------------- 3 files changed, 65 insertions(+), 21 deletions(-) diff --git a/affine.scad b/affine.scad index 5f0b248..4764de5 100644 --- a/affine.scad +++ b/affine.scad @@ -245,13 +245,14 @@ function affine3d_rot_from_to(from, to) = let( // Description: // Returns a transformation that maps one coordinate frame to another. You must specify two or three of `x`, `y`, and `z`. The specified // axes are mapped to the vectors you supplied. If you give two inputs, the third vector is mapped to the appropriate normal to maintain a right hand coordinate system. -// If the vectors you give are orthogonal the result will be a rotation. The `reverse` parameter will supply the inverse map, which enables you -// to map two arbitrary coordinate systems two each other by using the canonical coordinate system as an intermediary. +// If the vectors you give are orthogonal the result will be a rotation and the `reverse` parameter will supply the inverse map, which enables you +// to map two arbitrary coordinate systems two each other by using the canonical coordinate system as an intermediary. You cannot use the `reverse` option +// with non-orthogonal inputs. // Arguments: // x = Destination vector for x axis // y = Destination vector for y axis // z = Destination vector for z axis -// reverse = reverse direction of the map. Default: false +// reverse = reverse direction of the map for orthogonal inputs. Default: false // Examples: // T = affine_frame_map(x=[1,1,0], y=[-1,1]); // This map is just a rotation around the z axis // T = affine_frame_map(x=[1,0,0], y=[1,1]); // This map is not a rotation because x and y aren't orthogonal @@ -276,7 +277,12 @@ function affine_frame_map(x,y,z, reverse=false) = is_undef(z) ? [x, y, cross(x,y)] : [x, y, z] ) - reverse ? affine2d_to_3d(map) : affine2d_to_3d(transpose(map)); + reverse ? + let( ocheck = approx(map[0]*map[1],0) && approx(map[0]*map[2],0) && approx(map[1]*map[2],0)) + assert(ocheck, "Inputs must be orthogonal when reverse==true") + affine2d_to_3d(map) + : + affine2d_to_3d(transpose(map)); diff --git a/common.scad b/common.scad index 5d0a3c0..3467d1a 100644 --- a/common.scad +++ b/common.scad @@ -116,6 +116,17 @@ function is_consistent(list) = is_list(list) && is_list_of(list, list[0]); +// Function: same_shape() +// Usage: +// same_shape(a,b) +// Description: +// Tests whether the inputs `a` and `b` are both numeric and are the same shaped list. +// Example: +// same_shape([3,[4,5]],[7,[3,4]]); // Returns true +// same_shape([3,4,5], [7,[3,4]]); // Returns false +function same_shape(a,b) = a*0 == b*0; + + // Section: Handling `undef`s. diff --git a/math.scad b/math.scad index e6cee05..5bce93e 100644 --- a/math.scad +++ b/math.scad @@ -106,7 +106,9 @@ function factorial(n,d=1) = product([for (i=[n:-1:d]) i]); // // Points colored in ROYGBIV order. // rainbow(pts) translate($item) circle(d=3,$fn=8); function lerp(a,b,u) = + assert(same_shape(a,b), "Bad or inconsistent inputs to lerp") is_num(u)? (1-u)*a + u*b : + assert(is_vector(u), "Input u to lerp must be number or vector") [for (v = u) lerp(a,b,v)]; @@ -536,15 +538,17 @@ function mean(v) = sum(v)/len(v); // Description: // Solves the linear system Ax=b. If A is square and non-singular the unique solution is returned. If A is overdetermined // the least squares solution is returned. If A is underdetermined, the minimal norm solution is returned. -// If A is rank deficient or singular then linear_solve returns `undef`. +// If A is rank deficient or singular then linear_solve returns `undef`. If b is a matrix that is compatible with A +// then the problem is solved for the matrix valued right hand side and a matrix is returned. Note that if you +// want to solve Ax=b1 and Ax=b2 that you need to form the matrix transpose([b1,b2]) for the right hand side and then +// transpose the returned value. function linear_solve(A,b) = assert(is_matrix(A)) - assert(is_vector(b)) - let( - dim = array_dim(A), - m=dim[0], n=dim[1] - ) - assert(len(b)==m,str("Incompatible matrix and vector",dim,len(b))) + let( + m = len(A), + n = len(A[0]) + ) + assert(is_vector(b,m) || is_matrix(b,m),"Incompatible matrix and right hand side") let ( qr = m<n ? qr_factor(transpose(A)) : qr_factor(A), maxdim = max(n,m), @@ -555,7 +559,19 @@ function linear_solve(A,b) = ) zeros != [] ? undef : m<n ? Q*back_substitute(R,b,transpose=true) : - back_substitute(R, transpose(Q)*b); + back_substitute(R, transpose(Q)*b); + +// Function: matrix_inverse() +// Usage: +// matrix_inverse(A) +// Description: +// Compute the matrix inverse of the square matrix A. If A is singular, returns undef. +// Note that if you just want to solve a linear system of equations you should NOT +// use this function. Instead use linear_solve, or use qr_factor. The computation +// will be faster and more accurate. +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: submatrix() @@ -573,11 +589,9 @@ function submatrix(M,ind1,ind2) = [for(i=ind1) [for(j=ind2) M[i][j] ] ]; function qr_factor(A) = assert(is_matrix(A)) let( - dim = array_dim(A), - m = dim[0], - n = dim[1] + m = len(A), + n = len(A[0]) ) - assert(len(dim)==2) let( qr =_qr_factor(A, column=0, m = m, n=n, Q=ident(m)), Rzero = [ @@ -601,14 +615,18 @@ function _qr_factor(A,Q, column, m, n) = _qr_factor(Qf*A, Q*Qf, column+1, m, n); - // Function: back_substitute() // Usage: back_substitute(R, b, [transpose]) // Description: // Solves the problem Rx=b where R is an upper triangular square matrix. No check is made that the lower triangular entries -// are actually zero. If transpose==true then instead solve transpose(R)*x=b. +// are actually zero. If transpose==true then instead solve transpose(R)*x=b. +// You can supply a compatible matrix b and it will produce the solution for every column of b. Note that if you want to +// solve Rx=b1 and Rx=b2 you must set b to transpose([b1,b2]) and then take the transpose of the result. function back_substitute(R, b, x=[],transpose = false) = - let(n=len(b)) + assert(is_matrix(R, square=true)) + let(n=len(R)) + assert(is_vector(b,n) || is_matrix(b,n),"R and b are not compatible in back_substitute") + !is_vector(b) ? transpose([for(i=[0:len(b[0])-1]) back_substitute(R,subindex(b,i))]) : transpose? reverse(back_substitute( [for(i=[0:n-1]) [for(j=[0:n-1]) R[n-1-j][n-1-i]]], @@ -678,18 +696,27 @@ function determinant(M) = // Function: is_matrix() // Usage: -// is_matrix(A,[m],[n]) +// is_matrix(A,[m],[n],[square]) // Description: // Returns true if A is a numeric matrix of height m and width n. If m or n // are omitted or set to undef then true is returned for any positive dimension. -function is_matrix(A,m,n) = +// If `square` is true then the matrix is required to be square. Note if you +// specify m != n and require a square matrix then the result will always be false. +// Arguments: +// A = matrix to test +// m = optional height of matrix +// n = optional width of matrix +// square = set to true to require a square matrix. Default: false +function is_matrix(A,m,n, square=false) = is_list(A) && len(A)>0 && (is_undef(m) || len(A)==m) && is_vector(A[0]) && (is_undef(n) || len(A[0])==n) && + (!square || n==m) && is_consistent(A); + // Section: Comparisons and Logic // Function: approx()