1//////////////////////////////////////////////////////////////////////
2// LibFile: linalg.scad
3// This file provides linear algebra, with support for matrix construction,
4// solutions to linear systems of equations, QR and Cholesky factorizations, and
5// matrix inverse.
6// Includes:
7// include <BOSL2/std.scad>
8// FileGroup: Math
9// FileSummary: Linear Algebra: solve linear systems, construct and modify matrices.
10// FileFootnotes: STD=Included in std.scad
11//////////////////////////////////////////////////////////////////////
12
13// Section: Matrices
14// The matrix, a rectangular array of numbers which represents a linear transformation,
15// is the fundamental object in linear algebra. In OpenSCAD a matrix is a list of lists of numbers
16// with a rectangular structure. Because OpenSCAD treats all data the same, most of the functions that
17// index matrices or construct them will work on matrices (lists of lists) whose elements are not numbers but may be
18// arbitrary data: strings, booleans, or even other lists. It may even be acceptable in some cases if the structure is non-rectangular.
19// Of course, linear algebra computations and solutions require true matrices with rectangular structure, where all the entries are
20// finite numbers.
21// .
22// Matrices in OpenSCAD are lists of row vectors. However, a potential source of confusion is that OpenSCAD
23// treats vectors as either column vectors or row vectors as demanded by
24// 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
25// `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
26// column vectors, you now need to use {{transpose()}} because OpenSCAD doesn't adjust matrices
27// contextually: `A=M*transpose([v,w])`. The solutions are now columns of A and you must extract
28// them with {{column()}} or take the transpose of `A`.
29
30
31// Section: Matrix testing and display
32
33// Function: is_matrix()
34// Synopsis: Check if input is a numeric matrix, optionally of specified size
35// Topics: Matrices
36// See Also: is_matrix_symmetric(), is_rotation()
37// Usage:
38// test = is_matrix(A, [m], [n], [square])
39// Description:
40// Returns true if A is a numeric matrix of height m and width n with finite entries. If m or n
41// are omitted or set to undef then true is returned for any positive dimension.
42// Arguments:
43// A = The matrix to test.
44// m = If given, requires the matrix to have this height.
45// n = Is given, requires the matrix to have this width.
46// square = If true, matrix must have height equal to width. Default: false
47function is_matrix(A,m,n,square=false) =
48 is_list(A)
49 && (( is_undef(m) && len(A) ) || len(A)==m)
50 && (!square || len(A) == len(A[0]))
51 && is_vector(A[0],n)
52 && is_consistent(A);
53
54
55// Function: is_matrix_symmetric()
56// Synopsis: Checks if matrix is symmetric
57// Topics: Matrices
58// See Also: is_matrix(), is_rotation()
59// Usage:
60// b = is_matrix_symmetric(A, [eps])
61// Description:
62// Returns true if the input matrix is symmetric, meaning it approximately equals its transpose.
63// The matrix can have arbitrary entries.
64// Arguments:
65// A = matrix to test
66// eps = epsilon for comparing equality. Default: 1e-12
67function is_matrix_symmetric(A,eps=1e-12) =
68 approx(A,transpose(A), eps);
69
70
71// Function: is_rotation()
72// Synopsis: Check if a transformation matrix represents a rotation.
73// Topics: Affine, Matrices, Transforms
74// See Also: is_matrix(), is_matrix_symmetric(), is_rotation()
75// Usage:
76// b = is_rotation(A, [dim], [centered])
77// Description:
78// Returns true if the input matrix is a square affine matrix that is a rotation around any point,
79// or around the origin if `centered` is true.
80// The matrix must be 3x3 (representing a 2d transformation) or 4x4 (representing a 3d transformation).
81// You can set `dim` to 2 to require a 2d transform (3x3 matrix) or to 3 to require a 3d transform (4x4 matrix).
82// Arguments:
83// A = matrix to test
84// dim = if set, specify dimension in which the transform operates (2 or 3)
85// centered = if true then require rotation to be around the origin. Default: false
86function is_rotation(A,dim,centered=false) =
87 let(n=len(A))
88 is_matrix(A,square=true)
89 && ( n==3 || n==4 && (is_undef(dim) || dim==n-1))
90 &&
91 (
92 let(
93 rotpart = [for(i=[0:n-2]) [for(j=[0:n-2]) A[j][i]]]
94 )
95 approx(determinant(rotpart),1)
96 )
97 &&
98 (!centered || [for(row=[0:n-2]) if (!approx(A[row][n-1],0)) row]==[]);
99
100
101// Function&Module: echo_matrix()
102// Synopsis: Print a matrix neatly to the console.
103// Topics: Matrices
104// See Also: is_matrix(), is_matrix_symmetric(), is_rotation()
105// Usage:
106// echo_matrix(M, [description], [sig], [sep], [eps]);
107// dummy = echo_matrix(M, [description], [sig], [sep], [eps]),
108// Description:
109// Display a numerical matrix in a readable columnar format with `sig` significant
110// digits. Values smaller than eps display as zero. If you give a description
111// it is displayed at the top. You can change the space between columns by
112// setting `sep` to a number of spaces, which will use wide figure spaces the same
113// width as digits, or you can set it to any string to separate the columns.
114// Values that are NaN or INF will display as "nan" and "inf". Values which are
115// otherwise non-numerica display as two dashes. Note that this includes lists, so
116// a 3D array will display as a list of dashes.
117// Arguments:
118// M = matrix to display, which should be numerical
119// description = optional text to print before the matrix
120// sig = number of digits to display. Default: 4
121// sep = number of spaces between columns or a text string to separate columns. Default: 1
122// eps = numbers smaller than this display as zero. Default: 1e-9
123function echo_matrix(M,description,sig=4,sep=1,eps=1e-9) =
124 let(
125 horiz_line = chr(8213),
126 matstr = _format_matrix(M,sig=sig,sep=sep,eps=eps),
127 separator = str_join(repeat(horiz_line,10)),
128 dummy=echo(str(separator,is_def(description) ? str(" ",description) : ""))
129 [for(row=matstr) echo(row)]
130 )
131 echo(separator);
132
133module echo_matrix(M,description,sig=4,sep=1,eps=1e-9)
134{
135 dummy = echo_matrix(M,description,sig,sep,eps);
136}
137
138
139// Section: Matrix indexing
140
141// Function: column()
142// Synopsis: Extract a column from a matrix.
143// Topics: Matrices, List Handling, Arrays
144// See Also: select(), slice()
145// Usage:
146// list = column(M, i);
147// Description:
148// Extracts entry `i` from each list in M, or equivalently column i from the matrix M, and returns it as a vector.
149// This function will return `undef` at all entry positions indexed by i not found in M.
150// Arguments:
151// M = The given list of lists.
152// i = The index to fetch
153// Example:
154// M = [[1,2,3,4],[5,6,7,8],[9,10,11,12],[13,14,15,16]];
155// a = column(M,2); // Returns [3, 7, 11, 15]
156// b = column(M,0); // Returns [1, 5, 9, 13]
157// N = [ [1,2], [3], [4,5], [6,7,8] ];
158// c = column(N,1); // Returns [1,undef,5,7]
159// data = [[1,[3,4]], [3, [9,3]], [4, [3,1]]]; // Matrix with non-numeric entries
160// d = column(data,0); // Returns [1,3,4]
161// e = column(data,1); // Returns [[3,4],[9,3],[3,1]]
162function column(M, i) =
163 assert( is_list(M), "The input is not a list." )
164 assert( is_int(i) && i>=0, "Invalid index")
165 [for(row=M) row[i]];
166
167
168// Function: submatrix()
169// Synopsis: Extract a submatrix from a matrix
170// Topics: Matrices, Arrays
171// See Also: column(), block_matrix(), submatrix_set()
172// Usage:
173// mat = submatrix(M, idx1, idx2);
174// Description:
175// 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.
176// Arguments:
177// M = Given list of lists
178// idx1 = rows index list or range
179// idx2 = column index list or range
180// Example:
181// M = [[ 1, 2, 3, 4, 5],
182// [ 6, 7, 8, 9,10],
183// [11,12,13,14,15],
184// [16,17,18,19,20],
185// [21,22,23,24,25]];
186// submatrix(M,[1:2],[3:4]); // Returns [[9, 10], [14, 15]]
187// submatrix(M,[1], [3,4])); // Returns [[9,10]]
188// submatrix(M,1, [3,4])); // Returns [[9,10]]
189// submatrix(M,1,3)); // Returns [[9]]
190// submatrix(M, [3,4],1); // Returns [[17],[22]]);
191// submatrix(M, [1,3],[2,4]); // Returns [[8,10],[18,20]]);
192// A = [[true, 17, "test"],
193// [[4,2], 91, false],
194// [6, [3,4], undef]];
195// submatrix(A,[0,2],[1,2]); // Returns [[17, "test"], [[3, 4], undef]]
196function submatrix(M,idx1,idx2) =
197 [for(i=idx1) [for(j=idx2) M[i][j] ] ];
198
199
200// Section: Matrix construction and modification
201
202// Function: ident()
203// Synopsis: Return identity matrix.
204// Topics: Affine, Matrices, Transforms
205// See Also: IDENT, submatrix(), column()
206// Usage:
207// mat = ident(n);
208// Description:
209// Create an `n` by `n` square identity matrix.
210// Arguments:
211// n = The size of the identity matrix square, `n` by `n`.
212// Example:
213// mat = ident(3);
214// // Returns:
215// // [
216// // [1, 0, 0],
217// // [0, 1, 0],
218// // [0, 0, 1]
219// // ]
220// Example:
221// mat = ident(4);
222// // Returns:
223// // [
224// // [1, 0, 0, 0],
225// // [0, 1, 0, 0],
226// // [0, 0, 1, 0],
227// // [0, 0, 0, 1]
228// // ]
229function ident(n) = [
230 for (i = [0:1:n-1]) [
231 for (j = [0:1:n-1]) (i==j)? 1 : 0
232 ]
233];
234
235
236// Function: diagonal_matrix()
237// Synopsis: Make a diagonal matrix.
238// Topics: Affine, Matrices
239// See Also: column(), submatrix()
240// Usage:
241// mat = diagonal_matrix(diag, [offdiag]);
242// Description:
243// Creates a square matrix with the items in the list `diag` on
244// its diagonal. The off diagonal entries are set to offdiag,
245// which is zero by default.
246// Arguments:
247// diag = A list of items to put in the diagnal cells of the matrix.
248// offdiag = Value to put in non-diagonal matrix cells.
249function diagonal_matrix(diag, offdiag=0) =
250 assert(is_list(diag) && len(diag)>0)
251 [for(i=[0:1:len(diag)-1]) [for(j=[0:len(diag)-1]) i==j?diag[i] : offdiag]];
252
253
254// Function: transpose()
255// Synopsis: Transpose a matrix
256// Topics: Linear Algebra, Matrices
257// See Also: submatrix(), block_matrix(), hstack(), flatten()
258// Usage:
259// M = transpose(M, [reverse]);
260// Description:
261// Returns the transpose of the given input matrix. The input can be a matrix with arbitrary entries or
262// a numerical vector. If you give a vector then transpose returns it unchanged.
263// When reverse=true, the transpose is done across to the secondary diagonal. (See example below.)
264// By default, reverse=false.
265// Example:
266// M = [
267// [1, 2, 3],
268// [4, 5, 6],
269// [7, 8, 9]
270// ];
271// t = transpose(M);
272// // Returns:
273// // [
274// // [1, 4, 7],
275// // [2, 5, 8],
276// // [3, 6, 9]
277// // ]
278// Example:
279// M = [
280// [1, 2, 3],
281// [4, 5, 6]
282// ];
283// t = transpose(M);
284// // Returns:
285// // [
286// // [1, 4],
287// // [2, 5],
288// // [3, 6],
289// // ]
290// Example:
291// M = [
292// [1, 2, 3],
293// [4, 5, 6],
294// [7, 8, 9]
295// ];
296// t = transpose(M, reverse=true);
297// // Returns:
298// // [
299// // [9, 6, 3],
300// // [8, 5, 2],
301// // [7, 4, 1]
302// // ]
303// Example: Transpose on a list of numbers returns the list unchanged
304// transpose([3,4,5]); // Returns: [3,4,5]
305// Example: Transpose on non-numeric input
306// arr = [
307// [ "a", "b", "c"],
308// [ "d", "e", "f"],
309// [[1,2],[3,4],[5,6]]
310// ];
311// t = transpose(arr);
312// // Returns:
313// // [
314// // ["a", "d", [1,2]],
315// // ["b", "e", [3,4]],
316// // ["c", "f", [5,6]],
317// // ]
318
319function transpose(M, reverse=false) =
320 assert( is_list(M) && len(M)>0, "Input to transpose must be a nonempty list.")
321 is_list(M[0])
322 ? let( len0 = len(M[0]) )
323 assert([for(a=M) if(!is_list(a) || len(a)!=len0) 1 ]==[], "Input to transpose has inconsistent row lengths." )
324 reverse
325 ? [for (i=[0:1:len0-1])
326 [ for (j=[0:1:len(M)-1]) M[len(M)-1-j][len0-1-i] ] ]
327 : [for (i=[0:1:len0-1])
328 [ for (j=[0:1:len(M)-1]) M[j][i] ] ]
329 : assert( is_vector(M), "Input to transpose must be a vector or list of lists.")
330 M;
331
332
333// Function: outer_product()
334// Synopsis: Compute the outer product of two vectors.
335// Topics: Linear Algebra, Matrices
336// See Also: submatrix(), determinant()
337// Usage:
338// x = outer_product(u,v);
339// Description:
340// Compute the outer product of two vectors, which is a matrix.
341// Usage:
342// M = outer_product(u,v);
343function outer_product(u,v) =
344 assert(is_vector(u) && is_vector(v), "The inputs must be vectors.")
345 [for(ui=u) ui*v];
346
347// Function: submatrix_set()
348// Synopsis: Takes a matrix as input and change values in a submatrix.
349// Topics: Matrices, Arrays
350// See Also: column(), submatrix()
351// Usage:
352// mat = submatrix_set(M, A, [m], [n]);
353// Description:
354// Sets a submatrix of M equal to the matrix A. By default the top left corner of M is set to A, but
355// you can specify offset coordinates m and n. If A (as adjusted by m and n) extends beyond the bounds
356// of M then the extra entries are ignored. You can pass in `A=[[]]`, a null matrix, and M will be
357// returned unchanged. This function works on arbitrary lists of lists and the input M need not be rectangular in shape.
358// Arguments:
359// M = Original matrix.
360// A = Submatrix of new values to write into M
361// m = Row number of upper-left corner to place A at. Default: 0
362// n = Column number of upper-left corner to place A at. Default: 0
363function submatrix_set(M,A,m=0,n=0) =
364 assert(is_list(M))
365 assert(is_list(A))
366 assert(is_int(m))
367 assert(is_int(n))
368 let( badrows = [for(i=idx(A)) if (!is_list(A[i])) i])
369 assert(badrows==[], str("Input submatrix malformed rows: ",badrows))
370 [for(i=[0:1:len(M)-1])
371 assert(is_list(M[i]), str("Row ",i," of input matrix is not a list"))
372 [for(j=[0:1:len(M[i])-1])
373 i>=m && i <len(A)+m && j>=n && j<len(A[0])+n ? A[i-m][j-n] : M[i][j]]];
374
375
376// Function: hstack()
377// Synopsis: Make a new matrix by stacking matrices horizontally.
378// Topics: Matrices, Arrays
379// See Also: column(), submatrix(), block_matrix()
380// Usage:
381// A = hstack(M1, M2)
382// A = hstack(M1, M2, M3)
383// A = hstack([M1, M2, M3, ...])
384// Description:
385// Constructs a matrix by horizontally "stacking" together compatible matrices or vectors. Vectors are treated as columsn in the stack.
386// This command is the inverse of `column`. Note: strings given in vectors are broken apart into lists of characters. Strings given
387// in matrices are preserved as strings. If you need to combine vectors of strings use {{list_to_matrix()}} as shown below to convert the
388// vector into a column matrix. Also note that vertical stacking can be done directly with concat.
389// Arguments:
390// M1 = If given with other arguments, the first matrix (or vector) to stack. If given alone, a list of matrices/vectors to stack.
391// M2 = Second matrix/vector to stack
392// M3 = Third matrix/vector to stack.
393// Example:
394// M = ident(3);
395// v1 = [2,3,4];
396// v2 = [5,6,7];
397// v3 = [8,9,10];
398// a = hstack(v1,v2); // Returns [[2, 5], [3, 6], [4, 7]]
399// b = hstack(v1,v2,v3); // Returns [[2, 5, 8],
400// // [3, 6, 9],
401// // [4, 7, 10]]
402// c = hstack([M,v1,M]); // Returns [[1, 0, 0, 2, 1, 0, 0],
403// // [0, 1, 0, 3, 0, 1, 0],
404// // [0, 0, 1, 4, 0, 0, 1]]
405// d = hstack(column(M,0), submatrix(M,idx(M),[1 2])); // Returns M
406// strvec = ["one","two"];
407// strmat = [["three","four"], ["five","six"]];
408// e = hstack(strvec,strvec); // Returns [["o", "n", "e", "o", "n", "e"],
409// // ["t", "w", "o", "t", "w", "o"]]
410// f = hstack(list_to_matrix(strvec,1), list_to_matrix(strvec,1));
411// // Returns [["one", "one"],
412// // ["two", "two"]]
413// g = hstack(strmat,strmat); // Returns: [["three", "four", "three", "four"],
414// // [ "five", "six", "five", "six"]]
415function hstack(M1, M2, M3) =
416 (M3!=undef)? hstack([M1,M2,M3]) :
417 (M2!=undef)? hstack([M1,M2]) :
418 assert(all([for(v=M1) is_list(v)]), "One of the inputs to hstack is not a list")
419 let(
420 minlen = min_length(M1),
421 maxlen = max_length(M1)
422 )
423 assert(minlen==maxlen, "Input vectors to hstack must have the same length")
424 [for(row=[0:1:minlen-1])
425 [for(matrix=M1)
426 each matrix[row]
427 ]
428 ];
429
430
431// Function: block_matrix()
432// Synopsis: Make a new matrix from a block of matrices.
433// Topics: Matrices, Arrays
434// See Also: column(), submatrix()
435// Usage:
436// bmat = block_matrix([[M11, M12,...],[M21, M22,...], ... ]);
437// Description:
438// Create a block matrix by supplying a matrix of matrices, which will
439// be combined into one unified matrix. Every matrix in one row
440// must have the same height, and the combined width of the matrices
441// in each row must be equal. Strings will stay strings.
442// Example:
443// A = [[1,2],
444// [3,4]];
445// B = ident(2);
446// C = block_matrix([[A,B],[B,A],[A,B]]);
447// // Returns:
448// // [[1, 2, 1, 0],
449// // [3, 4, 0, 1],
450// // [1, 0, 1, 2],
451// // [0, 1, 3, 4],
452// // [1, 2, 1, 0],
453// // [3, 4, 0, 1]]);
454// D = block_matrix([[A,B], ident(4)]);
455// // Returns:
456// // [[1, 2, 1, 0],
457// // [3, 4, 0, 1],
458// // [1, 0, 0, 0],
459// // [0, 1, 0, 0],
460// // [0, 0, 1, 0],
461// // [0, 0, 0, 1]]);
462// E = [["one", "two"], [3,4]];
463// F = block_matrix([[E,E]]);
464// // Returns:
465// // [["one", "two", "one", "two"],
466// // [ 3, 4, 3, 4]]
467function block_matrix(M) =
468 let(
469 bigM = [for(bigrow = M) each hstack(bigrow)],
470 len0 = len(bigM[0]),
471 badrows = [for(row=bigM) if (len(row)!=len0) 1]
472 )
473 assert(badrows==[], "Inconsistent or invalid input")
474 bigM;
475
476
477// Section: Solving Linear Equations and Matrix Factorizations
478
479// Function: linear_solve()
480// Synopsis: Solve Ax=b or, for overdetermined case, solve the least square problem.
481// Topics: Matrices, Linear Algebra
482// See Also: linear_solve3(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky()
483// Usage:
484// solv = linear_solve(A,b,[pivot])
485// Description:
486// Solves the linear system Ax=b. If `A` is square and non-singular the unique solution is returned. If `A` is overdetermined
487// the least squares solution is returned. If `A` is underdetermined, the minimal norm solution is returned.
488// If `A` is rank deficient or singular then linear_solve returns `[]`. If `b` is a matrix that is compatible with `A`
489// then the problem is solved for the matrix valued right hand side and a matrix is returned. Note that if you
490// 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
491// transpose the returned value. The solution is computed using QR factorization. If `pivot` is set to true (the default) then
492// pivoting is used in the QR factorization, which is slower but expected to be more accurate.
493// Arguments:
494// A = Matrix describing the linear system, which need not be square
495// b = right hand side for linear system, which can be a matrix to solve several cases simultaneously. Must be consistent with A.
496// pivot = if true use pivoting when computing the QR factorization. Default: true
497function linear_solve(A,b,pivot=true) =
498 assert(is_matrix(A), "Input should be a matrix.")
499 let(
500 m = len(A),
501 n = len(A[0])
502 )
503 assert(is_vector(b,m) || is_matrix(b,m),"Invalid right hand side or incompatible with the matrix")
504 let (
505 qr = m<n? qr_factor(transpose(A),pivot) : qr_factor(A,pivot),
506 maxdim = max(n,m),
507 mindim = min(n,m),
508 Q = submatrix(qr[0],[0:maxdim-1], [0:mindim-1]),
509 R = submatrix(qr[1],[0:mindim-1], [0:mindim-1]),
510 P = qr[2],
511 zeros = [for(i=[0:mindim-1]) if (approx(R[i][i],0)) i]
512 )
513 zeros != [] ? [] :
514 m<n ? Q*back_substitute(R,transpose(P)*b,transpose=true) // Too messy to avoid input checks here
515 : P*_back_substitute(R, transpose(Q)*b); // Calling internal version skips input checks
516
517
518// Function: linear_solve3()
519// Synopsis: Fast solution to Ax=b where A is 3x3.
520// Topics: Matrices, Linear Algebra
521// See Also: linear_solve(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky()
522// Usage:
523// x = linear_solve3(A,b)
524// Description:
525// Fast solution to a 3x3 linear system using Cramer's rule (which appears to be the fastest
526// method in OpenSCAD). The input `A` must be a 3x3 matrix. Returns undef if `A` is singular.
527// The input `b` must be a 3-vector. Note that Cramer's rule is not a stable algorithm, so for
528// the highest accuracy on ill-conditioned problems you may want to use the general solver, which is about ten times slower.
529// Arguments:
530// A = 3x3 matrix for linear system
531// b = length 3 vector, right hand side of linear system
532function linear_solve3(A,b) =
533 // Arg sanity checking adds 7% overhead
534 assert(b*0==[0,0,0], "Input b must be a 3-vector")
535 assert(A*0==[[0,0,0],[0,0,0],[0,0,0]],"Input A must be a 3x3 matrix")
536 let(
537 Az = [for(i=[0:2])[A[i][0], A[i][1], b[i]]],
538 Ay = [for(i=[0:2])[A[i][0], b[i], A[i][2]]],
539 Ax = [for(i=[0:2])[b[i], A[i][1], A[i][2]]],
540 detA = det3(A)
541 )
542 detA==0 ? undef : [det3(Ax), det3(Ay), det3(Az)] / detA;
543
544
545// Function: matrix_inverse()
546// Synopsis: General matrix inverse.
547// Topics: Matrices, Linear Algebra
548// See Also: linear_solve(), linear_solve3(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky()
549// Usage:
550// mat = matrix_inverse(A)
551// Description:
552// Compute the matrix inverse of the square matrix `A`. If `A` is singular, returns `undef`.
553// Note that if you just want to solve a linear system of equations you should NOT use this function.
554// Instead use {{linear_solve()}}, or use {{qr_factor()}}. The computation
555// will be faster and more accurate.
556function matrix_inverse(A) =
557 assert(is_matrix(A) && len(A)==len(A[0]),"Input to matrix_inverse() must be a square matrix")
558 linear_solve(A,ident(len(A)));
559
560
561// Function: rot_inverse()
562// Synopsis: Invert 2d or 3d rotation transformations.
563// Topics: Matrices, Linear Algebra, Affine
564// See Also: linear_solve(), linear_solve3(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky()
565// Usage:
566// B = rot_inverse(A)
567// Description:
568// Inverts a 2d (3x3) or 3d (4x4) rotation matrix. The matrix can be a rotation around any center,
569// so it may include a translation. This is faster and likely to be more accurate than using `matrix_inverse()`.
570function rot_inverse(T) =
571 assert(is_matrix(T,square=true),"Matrix must be square")
572 let( n = len(T))
573 assert(n==3 || n==4, "Matrix must be 3x3 or 4x4")
574 let(
575 rotpart = [for(i=[0:n-2]) [for(j=[0:n-2]) T[j][i]]],
576 transpart = [for(row=[0:n-2]) T[row][n-1]]
577 )
578 assert(approx(determinant(T),1),"Matrix is not a rotation")
579 concat(hstack(rotpart, -rotpart*transpart),[[for(i=[2:n]) 0, 1]]);
580
581
582
583
584// Function: null_space()
585// Synopsis: Return basis for the null space of A.
586// Topics: Matrices, Linear Algebra
587// See Also: linear_solve(), linear_solve3(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky()
588// Usage:
589// x = null_space(A)
590// Description:
591// Returns an orthonormal basis for the null space of `A`, namely the vectors {x} such that Ax=0.
592// If the null space is just the origin then returns an empty list.
593function null_space(A,eps=1e-12) =
594 assert(is_matrix(A))
595 let(
596 Q_R = qr_factor(transpose(A),pivot=true),
597 R = Q_R[1],
598 zrows = [for(i=idx(R)) if (all_zero(R[i],eps)) i]
599 )
600 len(zrows)==0 ? [] :
601 select(transpose(Q_R[0]), zrows);
602
603// Function: qr_factor()
604// Synopsis: Compute QR factorization of a matrix.
605// Topics: Matrices, Linear Algebra
606// See Also: linear_solve(), linear_solve3(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky()
607// Usage:
608// qr = qr_factor(A,[pivot]);
609// Description:
610// Calculates the QR factorization of the input matrix A and returns it as the list [Q,R,P]. This factorization can be
611// used to solve linear systems of equations. The factorization is `A = Q*R*transpose(P)`. If pivot is false (the default)
612// 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
613// is non-decreasing. The use of pivoting is supposed to increase accuracy for poorly conditioned problems, and is necessary
614// for rank estimation or computation of the null space, but it may be slower.
615function qr_factor(A, pivot=false) =
616 assert(is_matrix(A), "Input must be a matrix." )
617 let(
618 m = len(A),
619 n = len(A[0])
620 )
621 let(
622 qr = _qr_factor(A, Q=ident(m),P=ident(n), pivot=pivot, col=0, m = m, n = n),
623 Rzero = let( R = qr[1]) [
624 for(i=[0:m-1]) [
625 let( ri = R[i] )
626 for(j=[0:n-1]) i>j ? 0 : ri[j]
627 ]
628 ]
629 ) [qr[0], Rzero, qr[2]];
630
631function _qr_factor(A,Q,P, pivot, col, m, n) =
632 col >= min(m-1,n) ? [Q,A,P] :
633 let(
634 swap = !pivot ? 1
635 : _swap_matrix(n,col,col+max_index([for(i=[col:n-1]) sqr([for(j=[col:m-1]) A[j][i]])])),
636 A = pivot ? A*swap : A,
637 x = [for(i=[col:1:m-1]) A[i][col]],
638 alpha = (x[0]<=0 ? 1 : -1) * norm(x),
639 u = x - concat([alpha],repeat(0,m-1)),
640 v = alpha==0 ? u : u / norm(u),
641 Qc = ident(len(x)) - 2*outer_product(v,v),
642 Qf = [for(i=[0:m-1]) [for(j=[0:m-1]) i<col || j<col ? (i==j ? 1 : 0) : Qc[i-col][j-col]]]
643 )
644 _qr_factor(Qf*A, Q*Qf, P*swap, pivot, col+1, m, n);
645
646// Produces an n x n matrix that swaps column i and j (when multiplied on the right)
647function _swap_matrix(n,i,j) =
648 assert(i<n && j<n && i>=0 && j>=0, "Swap indices out of bounds")
649 [for(y=[0:n-1]) [for (x=[0:n-1])
650 x==i ? (y==j ? 1 : 0)
651 : x==j ? (y==i ? 1 : 0)
652 : x==y ? 1 : 0]];
653
654
655
656// Function: back_substitute()
657// Synopsis: Solve an upper triangular system, Rx=b.
658// Topics: Matrices, Linear Algebra
659// See Also: linear_solve(), linear_solve3(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky()
660// Usage:
661// x = back_substitute(R, b, [transpose]);
662// Description:
663// Solves the problem Rx=b where R is an upper triangular square matrix. The lower triangular entries of R are
664// ignored. If transpose==true then instead solve transpose(R)*x=b.
665// You can supply a compatible matrix b and it will produce the solution for every column of b. Note that if you want to
666// 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
667// is singular (e.g. has a zero on the diagonal) then it returns [].
668function back_substitute(R, b, transpose = false) =
669 assert(is_matrix(R, square=true))
670 let(n=len(R))
671 assert(is_vector(b,n) || is_matrix(b,n),str("R and b are not compatible in back_substitute ",n, len(b)))
672 transpose
673 ? reverse(_back_substitute(transpose(R, reverse=true), reverse(b)))
674 : _back_substitute(R,b);
675
676function _back_substitute(R, b, x=[]) =
677 let(n=len(R))
678 len(x) == n ? x
679 : let(ind = n - len(x) - 1)
680 R[ind][ind] == 0 ? []
681 : let(
682 newvalue = len(x)==0
683 ? b[ind]/R[ind][ind]
684 : (b[ind]-list_tail(R[ind],ind+1) * x)/R[ind][ind]
685 )
686 _back_substitute(R, b, concat([newvalue],x));
687
688
689
690// Function: cholesky()
691// Synopsis: Compute the Cholesky factorization of a matrix.
692// Topics: Matrices, Linear Algebra
693// See Also: linear_solve(), linear_solve3(), matrix_inverse(), rot_inverse(), back_substitute(), cholesky()
694// Usage:
695// L = cholesky(A);
696// Description:
697// Compute the cholesky factor, L, of the symmetric positive definite matrix A.
698// The matrix L is lower triangular and `L * transpose(L) = A`. If the A is
699// not symmetric then an error is displayed. If the matrix is symmetric but
700// not positive definite then undef is returned.
701function cholesky(A) =
702 assert(is_matrix(A,square=true),"A must be a square matrix")
703 assert(is_matrix_symmetric(A),"Cholesky factorization requires a symmetric matrix")
704 _cholesky(A,ident(len(A)), len(A));
705
706function _cholesky(A,L,n) =
707 A[0][0]<0 ? undef : // Matrix not positive definite
708 len(A) == 1 ? submatrix_set(L,[[sqrt(A[0][0])]], n-1,n-1):
709 let(
710 i = n+1-len(A)
711 )
712 let(
713 sqrtAii = sqrt(A[0][0]),
714 Lnext = [for(j=[0:n-1])
715 [for(k=[0:n-1])
716 j<i-1 || k<i-1 ? (j==k ? 1 : 0)
717 : j==i-1 && k==i-1 ? sqrtAii
718 : j==i-1 ? 0
719 : k==i-1 ? A[j-(i-1)][0]/sqrtAii
720 : j==k ? 1 : 0]],
721 Anext = submatrix(A,[1:n-1], [1:n-1]) - outer_product(list_tail(A[0]), list_tail(A[0]))/A[0][0]
722 )
723 _cholesky(Anext,L*Lnext,n);
724
725
726// Section: Matrix Properties: Determinants, Norm, Trace
727
728// Function: det2()
729// Synopsis: Compute determinant of 2x2 matrix.
730// Topics: Matrices, Linear Algebra
731// See Also: det2(), det3(), det4(), determinant(), norm_fro(), matrix_trace()
732// Usage:
733// d = det2(M);
734// Description:
735// Rturns the determinant for the given 2x2 matrix.
736// Arguments:
737// M = The 2x2 matrix to get the determinant of.
738// Example:
739// M = [ [6,-2], [1,8] ];
740// det = det2(M); // Returns: 50
741function det2(M) =
742 assert(is_def(M) && M*0==[[0,0],[0,0]], "Expected square matrix (2x2)")
743 cross(M[0],M[1]);
744
745
746// Function: det3()
747// Synopsis: Compute determinant of 3x3 matrix.
748// Topics: Matrices, Linear Algebra
749// See Also: det2(), det3(), det4(), determinant(), norm_fro(), matrix_trace()
750// Usage:
751// d = det3(M);
752// Description:
753// Returns the determinant for the given 3x3 matrix.
754// Arguments:
755// M = The 3x3 square matrix to get the determinant of.
756// Example:
757// M = [ [6,4,-2], [1,-2,8], [1,5,7] ];
758// det = det3(M); // Returns: -334
759function det3(M) =
760 assert(is_def(M) && M*0==[[0,0,0],[0,0,0],[0,0,0]], "Expected square matrix (3x3).")
761 M[0][0] * (M[1][1]*M[2][2]-M[2][1]*M[1][2]) -
762 M[1][0] * (M[0][1]*M[2][2]-M[2][1]*M[0][2]) +
763 M[2][0] * (M[0][1]*M[1][2]-M[1][1]*M[0][2]);
764
765// Function: det4()
766// Synopsis: Compute determinant of 4x4 matrix.
767// Topics: Matrices, Linear Algebra
768// See Also: det2(), det3(), det4(), determinant(), norm_fro(), matrix_trace()
769// Usage:
770// d = det4(M);
771// Description:
772// Returns the determinant for the given 4x4 matrix.
773// Arguments:
774// M = The 4x4 square matrix to get the determinant of.
775// Example:
776// M = [ [6,4,-2,1], [1,-2,8,-3], [1,5,7,4], [2,3,4,7] ];
777// det = det4(M); // Returns: -1773
778function det4(M) =
779 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).")
780 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]
781 + 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]
782 + 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]
783 + 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]
784 - 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]
785 - 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]
786 - 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]
787 - 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];
788
789// Function: determinant()
790// Synopsis: compute determinant of an arbitrary square matrix.
791// Topics: Matrices, Linear Algebra
792// See Also: det2(), det3(), det4(), determinant(), norm_fro(), matrix_trace()
793// Usage:
794// d = determinant(M);
795// Description:
796// Returns the determinant for the given square matrix.
797// Arguments:
798// M = The NxN square matrix to get the determinant of.
799// Example:
800// M = [ [6,4,-2,9], [1,-2,8,3], [1,5,7,6], [4,2,5,1] ];
801// det = determinant(M); // Returns: 2267
802function determinant(M) =
803 assert(is_list(M), "Input must be a square matrix." )
804 len(M)==1? M[0][0] :
805 len(M)==2? det2(M) :
806 len(M)==3? det3(M) :
807 len(M)==4? det4(M) :
808 assert(is_matrix(M, square=true), "Input must be a square matrix." )
809 sum(
810 [for (col=[0:1:len(M)-1])
811 ((col%2==0)? 1 : -1) *
812 M[col][0] *
813 determinant(
814 [for (r=[1:1:len(M)-1])
815 [for (c=[0:1:len(M)-1])
816 if (c!=col) M[c][r]
817 ]
818 ]
819 )
820 ]
821 );
822
823
824// Function: norm_fro()
825// Synopsis: Compute Frobenius norm of a matrix
826// Topics: Matrices, Linear Algebra
827// See Also: det2(), det3(), det4(), determinant(), norm_fro(), matrix_trace()
828// Usage:
829// norm_fro(A)
830// Description:
831// Computes frobenius norm of input matrix. The frobenius norm is the square root of the sum of the
832// squares of all of the entries of the matrix. On vectors it is the same as the usual 2-norm.
833// This is an easily computed norm that is convenient for comparing two matrices.
834function norm_fro(A) =
835 assert(is_matrix(A) || is_vector(A))
836 norm(flatten(A));
837
838
839// Function: matrix_trace()
840// Synopsis: Compute the trace of a square matrix.
841// Topics: Matrices, Linear Algebra
842// See Also: det2(), det3(), det4(), determinant(), norm_fro(), matrix_trace()
843// Usage:
844// matrix_trace(M)
845// Description:
846// Computes the trace of a square matrix, the sum of the entries on the diagonal.
847function matrix_trace(M) =
848 assert(is_matrix(M,square=true), "Input to trace must be a square matrix")
849 [for(i=[0:1:len(M)-1])1] * [for(i=[0:1:len(M)-1]) M[i][i]];
850
851
852
853// vim: expandtab tabstop=4 shiftwidth=4 softtabstop=4 nowrap
854