extra/math/matrices/elimination/elimination.factor

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