////////////////////////////////////////////////////////////////////// // LibFile: linalg.scad // This file provides linear algebra, with support for matrix construction, // solutions to linear systems of equations, QR and Cholesky factorizations, and // matrix inverse. // Includes: // include <BOSL2/std.scad> // FileGroup: Math // FileSummary: Linear Algebra: solve linear systems, construct and modify matrices. // FileFootnotes: STD=Included in std.scad ////////////////////////////////////////////////////////////////////// // Section: Matrices // The matrix, a rectangular array of numbers which represents a linear transformation, // is the fundamental object in linear algebra. In OpenSCAD a matrix is a list of lists of numbers // with a rectangular structure. Because OpenSCAD treats all data the same, most of the functions that // index matrices or construct them will work on matrices (lists of lists) whose elements are not numbers but may be // arbitrary data: strings, booleans, or even other lists. It may even be acceptable in some cases if the structure is non-rectangular. // Of course, linear algebra computations and solutions require true matrices with rectangular structure, where all the entries are // finite numbers. // . // Matrices in OpenSCAD are lists of row vectors. However, a potential source of confusion is that OpenSCAD // treats vectors as either column vectors or row vectors as demanded by // context. Thus both `v*M` and `M*v` are valid if `M` is square and `v` has the right length. If you want to multiply // `M` on the left by `v` and `w` you can do this with `[v,w]*M` but if you want to multiply on the right side with `v` and `w` as // column vectors, you now need to use {{transpose()}} because OpenSCAD doesn't adjust matrices // contextually: `A=M*transpose([v,w])`. The solutions are now columns of A and you must extract // them with {{column()}} or take the transpose of `A`. // Section: Matrix testing and display // Function: is_matrix() // Synopsis: Check if input is a numeric matrix, optionally of specified size // Topics: Matrices // See Also: is_matrix_symmetric(), is_rotation() // Usage: // test = is_matrix(A, [m], [n], [square]) // Description: // Returns true if A is a numeric matrix of height m and width n with finite entries. If m or n // are omitted or set to undef then true is returned for any positive dimension. // Arguments: // A = The matrix to test. // m = If given, requires the matrix to have this height. // n = Is given, requires the matrix to have this width. // square = If true, matrix must have height equal to width. Default: false function is_matrix(A,m,n,square=false) = is_list(A) && (( is_undef(m) && len(A) ) || len(A)==m) && (!square || len(A) == len(A[0])) && is_vector(A[0],n) && is_consistent(A); // Function: is_matrix_symmetric() // Synopsis: Checks if matrix is symmetric // Topics: Matrices // See Also: is_matrix(), is_rotation() // Usage: // b = is_matrix_symmetric(A, [eps]) // Description: // Returns true if the input matrix is symmetric, meaning it approximately equals its transpose. // The matrix can have arbitrary entries. // Arguments: // A = matrix to test // eps = epsilon for comparing equality. Default: 1e-12 function is_matrix_symmetric(A,eps=1e-12) = approx(A,transpose(A), eps); // Function: is_rotation() // Synopsis: Check if a transformation matrix represents a rotation. // Topics: Affine, Matrices, Transforms // See Also: is_matrix(), is_matrix_symmetric(), is_rotation() // Usage: // b = is_rotation(A, [dim], [centered]) // Description: // Returns true if the input matrix is a square affine matrix that is a rotation around any point, // or around the origin if `centered` is true. // The matrix must be 3x3 (representing a 2d transformation) or 4x4 (representing a 3d transformation). // You can set `dim` to 2 to require a 2d transform (3x3 matrix) or to 3 to require a 3d transform (4x4 matrix). // Arguments: // A = matrix to test // dim = if set, specify dimension in which the transform operates (2 or 3) // centered = if true then require rotation to be around the origin. Default: false function is_rotation(A,dim,centered=false) = let(n=len(A)) is_matrix(A,square=true) && ( n==3 || n==4 && (is_undef(dim) || dim==n-1)) && ( let( rotpart = [for(i=[0:n-2]) [for(j=[0:n-2]) A[j][i]]] ) approx(determinant(rotpart),1) ) && (!centered || [for(row=[0:n-2]) if (!approx(A[row][n-1],0)) row]==[]); // Function&Module: echo_matrix() // Synopsis: Print a matrix neatly to the console. // Topics: Matrices // See Also: is_matrix(), is_matrix_symmetric(), is_rotation() // Usage: // echo_matrix(M, [description], [sig], [sep], [eps]); // dummy = echo_matrix(M, [description], [sig], [sep], [eps]), // Description: // Display a numerical matrix in a readable columnar format with `sig` significant // digits. Values smaller than eps display as zero. If you give a description // it is displayed at the top. You can change the space between columns by // setting `sep` to a number of spaces, which will use wide figure spaces the same // width as digits, or you can set it to any string to separate the columns. // Values that are NaN or INF will display as "nan" and "inf". Values which are // otherwise non-numerica display as two dashes. Note that this includes lists, so // a 3D array will display as a list of dashes. // Arguments: // M = matrix to display, which should be numerical // description = optional text to print before the matrix // sig = number of digits to display. Default: 4 // sep = number of spaces between columns or a text string to separate columns. Default: 1 // eps = numbers smaller than this display as zero. Default: 1e-9 function echo_matrix(M,description,sig=4,sep=1,eps=1e-9) = let( horiz_line = chr(8213), matstr = _format_matrix(M,sig=sig,sep=sep,eps=eps), separator = str_join(repeat(horiz_line,10)), dummy=echo(str(separator,is_def(description) ? str(" ",description) : "")) [for(row=matstr) echo(row)] ) echo(separator); module echo_matrix(M,description,sig=4,sep=1,eps=1e-9) { dummy = echo_matrix(M,description,sig,sep,eps); } // Section: Matrix indexing // Function: column() // Synopsis: Extract a column from a matrix. // Topics: Matrices, List Handling, Arrays // See Also: select(), slice() // Usage: // list = column(M, i); // Description: // Extracts entry `i` from each list in M, or equivalently column i from the matrix M, and returns it as a vector. // This function will return `undef` at all entry positions indexed by i not found in M. // Arguments: // M = The given list of lists. // i = The index to fetch // Example: // M = [[1,2,3,4],[5,6,7,8],[9,10,11,12],[13,14,15,16]]; // a = column(M,2); // Returns [3, 7, 11, 15] // b = column(M,0); // Returns [1, 5, 9, 13] // N = [ [1,2], [3], [4,5], [6,7,8] ]; // c = column(N,1); // Returns [1,undef,5,7] // data = [[1,[3,4]], [3, [9,3]], [4, [3,1]]]; // Matrix with non-numeric entries // d = column(data,0); // Returns [1,3,4] // e = column(data,1); // Returns [[3,4],[9,3],[3,1]] function column(M, i) = assert( is_list(M), "The input is not a list." ) assert( is_int(i) && i>=0, "Invalid index") [for(row=M) row[i]]; // Function: submatrix() // Synopsis: Extract a submatrix from a matrix // Topics: Matrices, Arrays // See Also: column(), block_matrix(), submatrix_set() // Usage: // mat = submatrix(M, idx1, idx2); // Description: // The input must be a list of lists (a matrix or 2d array). Returns a submatrix by selecting the rows listed in idx1 and columns listed in idx2. // Arguments: // M = Given list of lists // idx1 = rows index list or range // idx2 = column index list or range // Example: // M = [[ 1, 2, 3, 4, 5], // [ 6, 7, 8, 9,10], // [11,12,13,14,15], // [16,17,18,19,20], // [21,22,23,24,25]]; // submatrix(M,[1:2],[3:4]); // Returns [[9, 10], [14, 15]] // submatrix(M,[1], [3,4])); // Returns [[9,10]] // submatrix(M,1, [3,4])); // Returns [[9,10]] // submatrix(M,1,3)); // Returns [[9]] // submatrix(M, [3,4],1); // Returns [[17],[22]]); // submatrix(M, [1,3],[2,4]); // Returns [[8,10],[18,20]]); // A = [[true, 17, "test"], // [[4,2], 91, false], // [6, [3,4], undef]]; // submatrix(A,[0,2],[1,2]); // Returns [[17, "test"], [[3, 4], undef]] function submatrix(M,idx1,idx2) = [for(i=idx1) [for(j=idx2) M[i][j] ] ]; // Section: Matrix construction and modification // Function: ident() // Synopsis: Return identity matrix. // Topics: Affine, Matrices, Transforms // See Also: IDENT, submatrix(), column() // Usage: // mat = ident(n); // Description: // Create an `n` by `n` square identity matrix. // Arguments: // n = The size of the identity matrix square, `n` by `n`. // Example: // mat = ident(3); // // Returns: // // [ // // [1, 0, 0], // // [0, 1, 0], // // [0, 0, 1] // // ] // Example: // mat = ident(4); // // Returns: // // [ // // [1, 0, 0, 0], // // [0, 1, 0, 0], // // [0, 0, 1, 0], // // [0, 0, 0, 1] // // ] function ident(n) = [ for (i = [0:1:n-1]) [ for (j = [0:1:n-1]) (i==j)? 1 : 0 ] ]; // Function: diagonal_matrix() // Synopsis: Make a diagonal matrix. // Topics: Affine, Matrices // See Also: column(), submatrix() // Usage: // mat = 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. // Arguments: // diag = A list of items to put in the diagnal cells of the matrix. // offdiag = Value to put in non-diagonal matrix cells. function diagonal_matrix(diag, offdiag=0) = assert(is_list(diag) && len(diag)>0) [for(i=[0:1:len(diag)-1]) [for(j=[0:len(diag)-1]) i==j?diag[i] : offdiag]]; // Function: transpose() // Synopsis: Transpose a matrix // Topics: Linear Algebra, Matrices // See Also: submatrix(), block_matrix(), hstack(), flatten() // Usage: // M = transpose(M, [reverse]); // Description: // Returns the transpose of the given input matrix. The input can be a matrix with arbitrary entries or // a numerical vector. If you give a vector then transpose returns it unchanged. // When reverse=true, the transpose is done across to the secondary diagonal. (See example below.) // By default, reverse=false. // Example: // M = [ // [1, 2, 3], // [4, 5, 6], // [7, 8, 9] // ]; // t = transpose(M); // // Returns: // // [ // // [1, 4, 7], // // [2, 5, 8], // // [3, 6, 9] // // ] // Example: // M = [ // [1, 2, 3], // [4, 5, 6] // ]; // t = transpose(M); // // Returns: // // [ // // [1, 4], // // [2, 5], // // [3, 6], // // ] // Example: // M = [ // [1, 2, 3], // [4, 5, 6], // [7, 8, 9] // ]; // t = transpose(M, reverse=true); // // Returns: // // [ // // [9, 6, 3], // // [8, 5, 2], // // [7, 4, 1] // // ] // Example: Transpose on a list of numbers returns the list unchanged // transpose([3,4,5]); // Returns: [3,4,5] // Example: Transpose on non-numeric input // arr = [ // [ "a", "b", "c"], // [ "d", "e", "f"], // [[1,2],[3,4],[5,6]] // ]; // t = transpose(arr); // // Returns: // // [ // // ["a", "d", [1,2]], // // ["b", "e", [3,4]], // // ["c", "f", [5,6]], // // ] function transpose(M, reverse=false) = assert( is_list(M) && len(M)>0, "Input to transpose must be a nonempty list.") is_list(M[0]) ? let( len0 = len(M[0]) ) assert([for(a=M) if(!is_list(a) || len(a)!=len0) 1 ]==[], "Input to transpose has inconsistent row lengths." ) reverse ? [for (i=[0:1:len0-1]) [ for (j=[0:1:len(M)-1]) M[len(M)-1-j][len0-1-i] ] ] : [for (i=[0:1:len0-1]) [ for (j=[0:1:len(M)-1]) M[j][i] ] ] : assert( is_vector(M), "Input to transpose must be a vector or list of lists.") M; // Function: outer_product() // Synopsis: Compute the outer product of two vectors. // Topics: Linear Algebra, Matrices // See Also: submatrix(), determinant() // Usage: // x = outer_product(u,v); // Description: // Compute the outer product of two vectors, which is a matrix. // Usage: // M = outer_product(u,v); function outer_product(u,v) = assert(is_vector(u) && is_vector(v), "The inputs must be vectors.") [for(ui=u) ui*v]; // Function: submatrix_set() // Synopsis: Takes a matrix as input and change values in a submatrix. // Topics: Matrices, Arrays // See Also: column(), submatrix() // Usage: // mat = submatrix_set(M, A, [m], [n]); // Description: // Sets a submatrix of M equal to the matrix A. By default the top left corner of M is set to A, but // you can specify offset coordinates m and n. If A (as adjusted by m and n) extends beyond the bounds // of M then the extra entries are ignored. You can pass in `A=[[]]`, a null matrix, and M will be // returned unchanged. This function works on arbitrary lists of lists and the input M need not be rectangular in shape. // Arguments: // M = Original matrix. // A = Submatrix of new values to write into M // m = Row number of upper-left corner to place A at. Default: 0 // n = Column number of upper-left corner to place A at. Default: 0 function submatrix_set(M,A,m=0,n=0) = assert(is_list(M)) assert(is_list(A)) assert(is_int(m)) assert(is_int(n)) let( badrows = [for(i=idx(A)) if (!is_list(A[i])) i]) assert(badrows==[], str("Input submatrix malformed rows: ",badrows)) [for(i=[0:1:len(M)-1]) assert(is_list(M[i]), str("Row ",i," of input matrix is not a list")) [for(j=[0:1:len(M[i])-1]) i>=m && i <len(A)+m && j>=n && j<len(A[0])+n ? A[i-m][j-n] : M[i][j]]]; // Function: hstack() // Synopsis: Make a new matrix by stacking matrices horizontally. // Topics: Matrices, Arrays // See Also: column(), submatrix(), block_matrix() // Usage: // A = hstack(M1, M2) // A = hstack(M1, M2, M3) // A = hstack([M1, M2, M3, ...]) // Description: // Constructs a matrix by horizontally "stacking" together compatible matrices or vectors. Vectors are treated as columsn in the stack. // This command is the inverse of `column`. Note: strings given in vectors are broken apart into lists of characters. Strings given // in matrices are preserved as strings. If you need to combine vectors of strings use {{list_to_matrix()}} as shown below to convert the // vector into a column matrix. Also note that vertical stacking can be done directly with concat. // Arguments: // M1 = If given with other arguments, the first matrix (or vector) to stack. If given alone, a list of matrices/vectors to stack. // M2 = Second matrix/vector to stack // M3 = Third matrix/vector to stack. // Example: // M = ident(3); // v1 = [2,3,4]; // v2 = [5,6,7]; // v3 = [8,9,10]; // a = hstack(v1,v2); // Returns [[2, 5], [3, 6], [4, 7]] // b = hstack(v1,v2,v3); // Returns [[2, 5, 8], // // [3, 6, 9], // // [4, 7, 10]] // c = hstack([M,v1,M]); // Returns [[1, 0, 0, 2, 1, 0, 0], // // [0, 1, 0, 3, 0, 1, 0], // // [0, 0, 1, 4, 0, 0, 1]] // d = hstack(column(M,0), submatrix(M,idx(M),[1 2])); // Returns M // strvec = ["one","two"]; // strmat = [["three","four"], ["five","six"]]; // e = hstack(strvec,strvec); // Returns [["o", "n", "e", "o", "n", "e"], // // ["t", "w", "o", "t", "w", "o"]] // f = hstack(list_to_matrix(strvec,1), list_to_matrix(strvec,1)); // // Returns [["one", "one"], // // ["two", "two"]] // g = hstack(strmat,strmat); // Returns: [["three", "four", "three", "four"], // // [ "five", "six", "five", "six"]] function hstack(M1, M2, M3) = (M3!=undef)? hstack([M1,M2,M3]) : (M2!=undef)? hstack([M1,M2]) : assert(all([for(v=M1) is_list(v)]), "One of the inputs to hstack is not a list") let( minlen = min_length(M1), maxlen = max_length(M1) ) assert(minlen==maxlen, "Input vectors to hstack must have the same length") [for(row=[0:1:minlen-1]) [for(matrix=M1) each matrix[row] ] ]; // Function: block_matrix() // Synopsis: Make a new matrix from a block of matrices. // Topics: Matrices, Arrays // See Also: column(), submatrix() // Usage: // bmat = block_matrix([[M11, M12,...],[M21, M22,...], ... ]); // Description: // Create a block matrix by supplying a matrix of matrices, which will // be combined into one unified matrix. Every matrix in one row // must have the same height, and the combined width of the matrices // in each row must be equal. Strings will stay strings. // Example: // A = [[1,2], // [3,4]]; // B = ident(2); // C = block_matrix([[A,B],[B,A],[A,B]]); // // Returns: // // [[1, 2, 1, 0], // // [3, 4, 0, 1], // // [1, 0, 1, 2], // // [0, 1, 3, 4], // // [1, 2, 1, 0], // // [3, 4, 0, 1]]); // D = block_matrix([[A,B], ident(4)]); // // Returns: // // [[1, 2, 1, 0], // // [3, 4, 0, 1], // // [1, 0, 0, 0], // // [0, 1, 0, 0], // // [0, 0, 1, 0], // // [0, 0, 0, 1]]); // E = [["one", "two"], [3,4]]; // F = block_matrix([[E,E]]); // // Returns: // // [["one", "two", "one", "two"], // // [ 3, 4, 3, 4]] function block_matrix(M) = let( bigM = [for(bigrow = M) each hstack(bigrow)], len0 = len(bigM[0]), badrows = [for(row=bigM) if (len(row)!=len0) 1] ) assert(badrows==[], "Inconsistent or invalid input") bigM; // Section: Solving Linear Equations and Matrix Factorizations // Function: linear_solve() // Synopsis: Solve Ax=b or, for overdetermined case, solve the least square problem. // Topics: Matrices, Linear Algebra // See Also: linear_solve3(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky() // Usage: // solv = linear_solve(A,b,[pivot]) // 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 `[]`. 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. The solution is computed using QR factorization. If `pivot` is set to true (the default) then // pivoting is used in the QR factorization, which is slower but expected to be more accurate. // Arguments: // A = Matrix describing the linear system, which need not be square // b = right hand side for linear system, which can be a matrix to solve several cases simultaneously. Must be consistent with A. // pivot = if true use pivoting when computing the QR factorization. Default: true function linear_solve(A,b,pivot=true) = assert(is_matrix(A), "Input should be a matrix.") let( m = len(A), n = len(A[0]) ) assert(is_vector(b,m) || is_matrix(b,m),"Invalid right hand side or incompatible with the matrix") let ( qr = m<n? qr_factor(transpose(A),pivot) : qr_factor(A,pivot), maxdim = max(n,m), mindim = min(n,m), Q = submatrix(qr[0],[0:maxdim-1], [0:mindim-1]), R = submatrix(qr[1],[0:mindim-1], [0:mindim-1]), P = qr[2], zeros = [for(i=[0:mindim-1]) if (approx(R[i][i],0)) i] ) zeros != [] ? [] : m<n ? Q*back_substitute(R,transpose(P)*b,transpose=true) // Too messy to avoid input checks here : P*_back_substitute(R, transpose(Q)*b); // Calling internal version skips input checks // Function: linear_solve3() // Synopsis: Fast solution to Ax=b where A is 3x3. // Topics: Matrices, Linear Algebra // See Also: linear_solve(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky() // Usage: // x = linear_solve3(A,b) // Description: // Fast solution to a 3x3 linear system using Cramer's rule (which appears to be the fastest // method in OpenSCAD). The input `A` must be a 3x3 matrix. Returns undef if `A` is singular. // The input `b` must be a 3-vector. Note that Cramer's rule is not a stable algorithm, so for // the highest accuracy on ill-conditioned problems you may want to use the general solver, which is about ten times slower. // Arguments: // A = 3x3 matrix for linear system // b = length 3 vector, right hand side of linear system function linear_solve3(A,b) = // Arg sanity checking adds 7% overhead assert(b*0==[0,0,0], "Input b must be a 3-vector") assert(A*0==[[0,0,0],[0,0,0],[0,0,0]],"Input A must be a 3x3 matrix") let( Az = [for(i=[0:2])[A[i][0], A[i][1], b[i]]], Ay = [for(i=[0:2])[A[i][0], b[i], A[i][2]]], Ax = [for(i=[0:2])[b[i], A[i][1], A[i][2]]], detA = det3(A) ) detA==0 ? undef : [det3(Ax), det3(Ay), det3(Az)] / detA; // Function: matrix_inverse() // Synopsis: General matrix inverse. // Topics: Matrices, Linear Algebra // See Also: linear_solve(), linear_solve3(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky() // Usage: // mat = 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) && len(A)==len(A[0]),"Input to matrix_inverse() must be a square matrix") linear_solve(A,ident(len(A))); // Function: rot_inverse() // Synopsis: Invert 2d or 3d rotation transformations. // Topics: Matrices, Linear Algebra, Affine // See Also: linear_solve(), linear_solve3(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky() // Usage: // B = rot_inverse(A) // Description: // Inverts a 2d (3x3) or 3d (4x4) rotation matrix. The matrix can be a rotation around any center, // so it may include a translation. This is faster and likely to be more accurate than using `matrix_inverse()`. function rot_inverse(T) = assert(is_matrix(T,square=true),"Matrix must be square") let( n = len(T)) assert(n==3 || n==4, "Matrix must be 3x3 or 4x4") let( rotpart = [for(i=[0:n-2]) [for(j=[0:n-2]) T[j][i]]], transpart = [for(row=[0:n-2]) T[row][n-1]] ) assert(approx(determinant(T),1),"Matrix is not a rotation") concat(hstack(rotpart, -rotpart*transpart),[[for(i=[2:n]) 0, 1]]); // Function: null_space() // Synopsis: Return basis for the null space of A. // Topics: Matrices, Linear Algebra // See Also: linear_solve(), linear_solve3(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky() // Usage: // x = 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], zrows = [for(i=idx(R)) if (all_zero(R[i],eps)) i] ) len(zrows)==0 ? [] : select(transpose(Q_R[0]), zrows); // Function: qr_factor() // Synopsis: Compute QR factorization of a matrix. // Topics: Matrices, Linear Algebra // See Also: linear_solve(), linear_solve3(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky() // Usage: // qr = qr_factor(A,[pivot]); // Description: // Calculates the QR factorization of the input matrix A and returns it as the list [Q,R,P]. This factorization can be // used to solve linear systems of equations. The factorization is `A = Q*R*transpose(P)`. If pivot is false (the default) // then P is the identity matrix and A = Q*R. If pivot is true then column pivoting results in an R matrix where the diagonal // is non-decreasing. The use of pivoting is supposed to increase accuracy for poorly conditioned problems, and is necessary // for rank estimation or computation of the null space, but it may be slower. function qr_factor(A, pivot=false) = assert(is_matrix(A), "Input must be a matrix." ) let( m = len(A), n = len(A[0]) ) let( qr = _qr_factor(A, Q=ident(m),P=ident(n), pivot=pivot, col=0, m = m, n = n), Rzero = let( R = qr[1]) [ for(i=[0:m-1]) [ let( ri = R[i] ) for(j=[0:n-1]) i>j ? 0 : ri[j] ] ] ) [qr[0], Rzero, qr[2]]; function _qr_factor(A,Q,P, pivot, col, m, n) = col >= min(m-1,n) ? [Q,A,P] : let( swap = !pivot ? 1 : _swap_matrix(n,col,col+max_index([for(i=[col:n-1]) sqr([for(j=[col:m-1]) A[j][i]])])), A = pivot ? A*swap : A, x = [for(i=[col:1:m-1]) A[i][col]], alpha = (x[0]<=0 ? 1 : -1) * norm(x), u = x - concat([alpha],repeat(0,m-1)), v = alpha==0 ? u : u / norm(u), Qc = ident(len(x)) - 2*outer_product(v,v), Qf = [for(i=[0:m-1]) [for(j=[0:m-1]) i<col || j<col ? (i==j ? 1 : 0) : Qc[i-col][j-col]]] ) _qr_factor(Qf*A, Q*Qf, P*swap, pivot, col+1, m, n); // Produces an n x n matrix that swaps column i and j (when multiplied on the right) function _swap_matrix(n,i,j) = assert(i<n && j<n && i>=0 && j>=0, "Swap indices out of bounds") [for(y=[0:n-1]) [for (x=[0:n-1]) x==i ? (y==j ? 1 : 0) : x==j ? (y==i ? 1 : 0) : x==y ? 1 : 0]]; // Function: back_substitute() // Synopsis: Solve an upper triangular system, Rx=b. // Topics: Matrices, Linear Algebra // See Also: linear_solve(), linear_solve3(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky() // Usage: // x = back_substitute(R, b, [transpose]); // Description: // Solves the problem Rx=b where R is an upper triangular square matrix. The lower triangular entries of R are // ignored. 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. If the matrix // is singular (e.g. has a zero on the diagonal) then it returns []. function back_substitute(R, b, transpose = false) = assert(is_matrix(R, square=true)) let(n=len(R)) assert(is_vector(b,n) || is_matrix(b,n),str("R and b are not compatible in back_substitute ",n, len(b))) transpose ? reverse(_back_substitute(transpose(R, reverse=true), reverse(b))) : _back_substitute(R,b); function _back_substitute(R, b, x=[]) = let(n=len(R)) len(x) == n ? x : let(ind = n - len(x) - 1) R[ind][ind] == 0 ? [] : let( newvalue = len(x)==0 ? b[ind]/R[ind][ind] : (b[ind]-list_tail(R[ind],ind+1) * x)/R[ind][ind] ) _back_substitute(R, b, concat([newvalue],x)); // Function: cholesky() // Synopsis: Compute the Cholesky factorization of a matrix. // Topics: Matrices, Linear Algebra // See Also: linear_solve(), linear_solve3(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky() // Usage: // L = cholesky(A); // Description: // Compute the cholesky factor, L, of the symmetric positive definite matrix A. // The matrix L is lower triangular and `L * transpose(L) = A`. If the A is // not symmetric then an error is displayed. If the matrix is symmetric but // not positive definite then undef is returned. function cholesky(A) = assert(is_matrix(A,square=true),"A must be a square matrix") assert(is_matrix_symmetric(A),"Cholesky factorization requires a symmetric matrix") _cholesky(A,ident(len(A)), len(A)); function _cholesky(A,L,n) = A[0][0]<0 ? undef : // Matrix not positive definite len(A) == 1 ? submatrix_set(L,[[sqrt(A[0][0])]], n-1,n-1): let( i = n+1-len(A) ) let( sqrtAii = sqrt(A[0][0]), Lnext = [for(j=[0:n-1]) [for(k=[0:n-1]) j<i-1 || k<i-1 ? (j==k ? 1 : 0) : j==i-1 && k==i-1 ? sqrtAii : j==i-1 ? 0 : k==i-1 ? A[j-(i-1)][0]/sqrtAii : j==k ? 1 : 0]], Anext = submatrix(A,[1:n-1], [1:n-1]) - outer_product(list_tail(A[0]), list_tail(A[0]))/A[0][0] ) _cholesky(Anext,L*Lnext,n); // Section: Matrix Properties: Determinants, Norm, Trace // Function: det2() // Synopsis: Compute determinant of 2x2 matrix. // Topics: Matrices, Linear Algebra // See Also: det2(), det3(), det4(), determinant(), norm_fro(), matrix_trace() // Usage: // d = det2(M); // Description: // Rturns the determinant for the given 2x2 matrix. // Arguments: // M = The 2x2 matrix to get the determinant of. // Example: // M = [ [6,-2], [1,8] ]; // det = det2(M); // Returns: 50 function det2(M) = assert(is_def(M) && M*0==[[0,0],[0,0]], "Expected square matrix (2x2)") cross(M[0],M[1]); // Function: det3() // Synopsis: Compute determinant of 3x3 matrix. // Topics: Matrices, Linear Algebra // See Also: det2(), det3(), det4(), determinant(), norm_fro(), matrix_trace() // Usage: // d = det3(M); // Description: // Returns the determinant for the given 3x3 matrix. // Arguments: // M = The 3x3 square matrix to get the determinant of. // Example: // M = [ [6,4,-2], [1,-2,8], [1,5,7] ]; // det = det3(M); // Returns: -334 function det3(M) = assert(is_def(M) && M*0==[[0,0,0],[0,0,0],[0,0,0]], "Expected square matrix (3x3).") M[0][0] * (M[1][1]*M[2][2]-M[2][1]*M[1][2]) - M[1][0] * (M[0][1]*M[2][2]-M[2][1]*M[0][2]) + M[2][0] * (M[0][1]*M[1][2]-M[1][1]*M[0][2]); // Function: det4() // Synopsis: Compute determinant of 4x4 matrix. // Topics: Matrices, Linear Algebra // See Also: det2(), det3(), det4(), determinant(), norm_fro(), matrix_trace() // Usage: // d = det4(M); // Description: // Returns the determinant for the given 4x4 matrix. // Arguments: // M = The 4x4 square matrix to get the determinant of. // Example: // M = [ [6,4,-2,1], [1,-2,8,-3], [1,5,7,4], [2,3,4,7] ]; // det = det4(M); // Returns: -1773 function det4(M) = assert(is_def(M) && M*0==[[0,0,0,0],[0,0,0,0],[0,0,0,0],[0,0,0,0]], "Expected square matrix (4x4).") M[0][0]*M[1][1]*M[2][2]*M[3][3] + M[0][0]*M[1][2]*M[2][3]*M[3][1] + M[0][0]*M[1][3]*M[2][1]*M[3][2] + M[0][1]*M[1][0]*M[2][3]*M[3][2] + M[0][1]*M[1][2]*M[2][0]*M[3][3] + M[0][1]*M[1][3]*M[2][2]*M[3][0] + M[0][2]*M[1][0]*M[2][1]*M[3][3] + M[0][2]*M[1][1]*M[2][3]*M[3][0] + M[0][2]*M[1][3]*M[2][0]*M[3][1] + M[0][3]*M[1][0]*M[2][2]*M[3][1] + M[0][3]*M[1][1]*M[2][0]*M[3][2] + M[0][3]*M[1][2]*M[2][1]*M[3][0] - M[0][0]*M[1][1]*M[2][3]*M[3][2] - M[0][0]*M[1][2]*M[2][1]*M[3][3] - M[0][0]*M[1][3]*M[2][2]*M[3][1] - M[0][1]*M[1][0]*M[2][2]*M[3][3] - M[0][1]*M[1][2]*M[2][3]*M[3][0] - M[0][1]*M[1][3]*M[2][0]*M[3][2] - M[0][2]*M[1][0]*M[2][3]*M[3][1] - M[0][2]*M[1][1]*M[2][0]*M[3][3] - M[0][2]*M[1][3]*M[2][1]*M[3][0] - M[0][3]*M[1][0]*M[2][1]*M[3][2] - M[0][3]*M[1][1]*M[2][2]*M[3][0] - M[0][3]*M[1][2]*M[2][0]*M[3][1]; // Function: determinant() // Synopsis: compute determinant of an arbitrary square matrix. // Topics: Matrices, Linear Algebra // See Also: det2(), det3(), det4(), determinant(), norm_fro(), matrix_trace() // Usage: // d = determinant(M); // Description: // Returns the determinant for the given square matrix. // Arguments: // M = The NxN square matrix to get the determinant of. // Example: // M = [ [6,4,-2,9], [1,-2,8,3], [1,5,7,6], [4,2,5,1] ]; // det = determinant(M); // Returns: 2267 function determinant(M) = assert(is_list(M), "Input must be a square matrix." ) len(M)==1? M[0][0] : len(M)==2? det2(M) : len(M)==3? det3(M) : len(M)==4? det4(M) : assert(is_matrix(M, square=true), "Input must be a square matrix." ) sum( [for (col=[0:1:len(M)-1]) ((col%2==0)? 1 : -1) * M[col][0] * determinant( [for (r=[1:1:len(M)-1]) [for (c=[0:1:len(M)-1]) if (c!=col) M[c][r] ] ] ) ] ); // Function: norm_fro() // Synopsis: Compute Frobenius norm of a matrix // Topics: Matrices, Linear Algebra // See Also: det2(), det3(), det4(), determinant(), norm_fro(), matrix_trace() // Usage: // norm_fro(A) // Description: // Computes frobenius norm of input matrix. The frobenius norm is the square root of the sum of the // squares of all of the entries of the matrix. On vectors it is the same as the usual 2-norm. // This is an easily computed norm that is convenient for comparing two matrices. function norm_fro(A) = assert(is_matrix(A) || is_vector(A)) norm(flatten(A)); // Function: matrix_trace() // Synopsis: Compute the trace of a square matrix. // Topics: Matrices, Linear Algebra // See Also: det2(), det3(), det4(), determinant(), norm_fro(), matrix_trace() // Usage: // matrix_trace(M) // Description: // Computes the trace of a square matrix, the sum of the entries on the diagonal. function matrix_trace(M) = assert(is_matrix(M,square=true), "Input to trace must be a square matrix") [for(i=[0:1:len(M)-1])1] * [for(i=[0:1:len(M)-1]) M[i][i]]; // vim: expandtab tabstop=4 shiftwidth=4 softtabstop=4 nowrap