1 ! Copyright (C) 2006, 2008 Slava Pestov.
2 ! See http://factorcode.org/license.txt for BSD license.
3 USING: kernel math math.vectors math.matrices namespaces
5 IN: math.matrices.elimination
9 : with-matrix ( matrix quot -- )
10 [ swap matrix set call matrix get ] with-scope ; inline
12 : nth-row ( row# -- seq ) matrix get nth ;
14 : change-row ( row# quot: ( seq -- seq ) -- )
15 matrix get swap change-nth ; inline
17 : exchange-rows ( row# row# -- ) matrix get exchange ;
19 : rows ( -- n ) matrix get length ;
21 : cols ( -- n ) 0 nth-row length ;
23 : skip ( i seq quot -- n )
24 over [ find-from drop ] dip length or ; inline
26 : first-col ( row# -- n )
27 #! First non-zero column
28 0 swap nth-row [ zero? not ] skip ;
30 : clear-scale ( col# pivot-row i-row -- n )
31 [ over ] dip nth dup zero? [
34 [ nth dup zero? ] dip swap [
41 : (clear-col) ( col# pivot-row i -- )
42 [ [ clear-scale ] 2keep [ n*v ] dip v+ ] change-row ;
44 : rows-from ( row# -- slice )
47 : clear-col ( col# row# rows -- )
48 [ nth-row ] dip [ [ 2dup ] dip (clear-col) ] each 2drop ;
50 : do-row ( exchange-with row# -- )
51 [ exchange-rows ] keep
53 dup 1 + rows-from clear-col ;
55 : find-row ( row# quot -- i elt )
56 [ rows-from ] dip find ; inline
58 : pivot-row ( col# row# -- n )
59 [ dupd nth-row nth zero? not ] find-row 2nip ;
61 : (echelon) ( col# row# -- )
62 over cols < over rows < and [
63 2dup pivot-row [ over do-row 1 + ] when*
69 : echelon ( matrix -- matrix' )
70 [ 0 0 (echelon) ] with-matrix ;
72 : nonzero-rows ( matrix -- matrix' )
73 [ [ zero? ] all? not ] filter ;
75 : null/rank ( matrix -- null rank )
76 echelon dup length swap nonzero-rows length [ - ] keep ;
78 : leading ( seq -- n elt ) [ zero? not ] find ;
80 : reduced ( matrix' -- matrix'' )
83 dup nth-row leading drop
84 dup [ swap dup clear-col ] [ 2drop ] if
88 : basis-vector ( row col# -- )
90 [ swap nth neg recip ] 2keep
91 [ 0 spin set-nth ] 2keep
95 : nullspace ( matrix -- seq )
96 echelon reduced dup empty? [
97 dup first length identity-matrix [
100 dup [ basis-vector ] [ 2drop ] if
102 ] with-matrix flip nonzero-rows
105 : 1-pivots ( matrix -- matrix )
106 [ dup leading nip [ recip v*n ] when* ] map ;
108 : solution ( matrix -- matrix )
109 echelon nonzero-rows reduced 1-pivots ;
111 : inverse ( matrix -- matrix ) ! Assumes an invertible matrix
113 [ identity-matrix [ append ] 2map solution ] keep