extra/math/matrices/elimination/elimination.factor

   1 ! Copyright (C) 2006, 2007 Slava Pestov.
   2 ! See http://factorcode.org/license.txt for BSD license.
   3 USING: kernel math math.vectors math.matrices namespaces
   4 sequences parser ;
   5 IN: math.matrices.elimination
   6
   7 SYMBOL: matrix
   8
   9 : with-matrix ( matrix quot -- )
  10     [ swap matrix set call matrix get ] with-scope ; inline
  11
  12 : nth-row ( row# -- seq ) matrix get nth ;
  13
  14 : change-row ( row# quot -- | quot: seq -- seq )
  15     matrix get swap change-nth ; inline
  16
  17 : exchange-rows ( row# row# -- ) matrix get exchange ;
  18
  19 : rows ( -- n ) matrix get length ;
  20
  21 : cols ( -- n ) 0 nth-row length ;
  22
  23 : first-col ( row# -- n )
  24     #! First non-zero column
  25     0 swap nth-row [ zero? not ] skip ;
  26
  27 : clear-scale ( col# pivot-row i-row -- n )
  28     >r over r> nth dup zero? [
  29         3drop 0
  30     ] [
  31         >r nth dup zero? [
  32             r> 2drop 0
  33         ] [
  34             r> swap / neg
  35         ] if
  36     ] if ;
  37
  38 : (clear-col) ( col# pivot-row i -- )
  39     [ [ clear-scale ] 2keep >r n*v r> v+ ] change-row ;
  40
  41 : rows-from ( row# -- slice )
  42     rows dup <slice> ;
  43
  44 : clear-col ( col# row# rows -- )
  45     >r nth-row r> [ >r 2dup r> (clear-col) ] each 2drop ;
  46
  47 : do-row ( exchange-with row# -- )
  48     [ exchange-rows ] keep
  49     [ first-col ] keep
  50     dup 1+ rows-from clear-col ;
  51
  52 : find-row ( row# quot -- i elt )
  53     >r rows-from r> find ; inline
  54
  55 : pivot-row ( col# row# -- n )
  56     [ dupd nth-row nth zero? not ] find-row 2nip ;
  57
  58 : (echelon) ( col# row# -- )
  59     over cols < over rows < and [
  60         2dup pivot-row [ over do-row 1+ ] when*
  61         >r 1+ r> (echelon)
  62     ] [
  63         2drop
  64     ] if ;
  65
  66 : echelon ( matrix -- matrix' )
  67     [ 0 0 (echelon) ] with-matrix ;
  68
  69 : nonzero-rows [ [ zero? ] all? not ] subset ;
  70
  71 : null/rank ( matrix -- null rank )
  72     echelon dup length swap nonzero-rows length [ - ] keep ;
  73
  74 : leading ( seq -- n elt ) [ zero? not ] find ;
  75
  76 : reduced ( matrix' -- matrix'' )
  77     [
  78         rows <reversed> [
  79             dup nth-row leading drop
  80             dup [ swap dup clear-col ] [ 2drop ] if
  81         ] each
  82     ] with-matrix ;
  83
  84 : basis-vector ( row col# -- )
  85     >r clone r>
  86     [ swap nth neg recip ] 2keep
  87     [ 0 spin set-nth ] 2keep
  88     >r n*v r>
  89     matrix get set-nth ;
  90
  91 : nullspace ( matrix -- seq )
  92     echelon reduced dup empty? [
  93         dup first length identity-matrix [
  94             [
  95                 dup leading drop
  96                 dup [ basis-vector ] [ 2drop ] if
  97             ] each
  98         ] with-matrix flip nonzero-rows
  99     ] unless ;
 100
 101 : 1-pivots ( matrix -- matrix )
 102     [ dup leading nip [ recip v*n ] when* ] map ;
 103
 104 : solution ( matrix -- matrix )
 105     echelon nonzero-rows reduced 1-pivots ;
 106
 107 : inverse ( matrix -- matrix ) ! Assumes an invertible matrix
 108     dup length
 109     [ identity-matrix [ append ] 2map solution ] keep
 110     [ tail ] curry map ;