basis/math/matrices/matrices.factor

   1 ! Copyright (C) 2005, 2010 Slava Pestov, Joe Groff.
   2 ! See http://factorcode.org/license.txt for BSD license.
   3 USING: accessors arrays columns kernel locals math math.bits
   4 math.functions math.order math.vectors sequences
   5 sequences.private fry ;
   6 IN: math.matrices
   7
   8 ! Matrices
   9 : zero-matrix ( m n -- matrix )
  10     '[ _ 0 <array> ] replicate ;
  11
  12 : diagonal-matrix ( diagonal-seq -- matrix )
  13     dup length dup zero-matrix
  14     [ '[ dup _ nth set-nth ] each-index ] keep ; inline
  15
  16 : identity-matrix ( n -- matrix )
  17     1 <repetition> diagonal-matrix ; inline
  18
  19 : eye ( m n k -- matrix )
  20     [ [ iota ] bi@ ] dip neg '[ _ + = 1 0 ? ] cartesian-map ;
  21
  22 : hilbert-matrix ( m n -- matrix )
  23     [ iota ] bi@ [ + 1 + recip ] cartesian-map ;
  24
  25 : toeplitz-matrix ( n -- matrix )
  26     iota dup [ - abs 1 + ] cartesian-map ;
  27
  28 : hankel-matrix ( n -- matrix )
  29     [ iota dup ] keep '[ + abs 1 + dup _ > [ drop 0 ] when ] cartesian-map ;
  30
  31 : box-matrix ( r -- matrix )
  32     2 * 1 + dup '[ _ 1 <array> ] replicate ;
  33
  34 : van-der-monde-matrix ( u n -- matrix )
  35     iota [ v^n ] with map reverse flip ;
  36
  37 :: rotation-matrix3 ( axis theta -- matrix )
  38     theta cos :> c
  39     theta sin :> s
  40     axis first3 :> ( x y z )
  41     x sq 1.0 x sq - c * +     x y * 1.0 c - * z s * -   x z * 1.0 c - * y s * + 3array
  42     x y * 1.0 c - * z s * +   y sq 1.0 y sq - c * +     y z * 1.0 c - * x s * - 3array
  43     x z * 1.0 c - * y s * -   y z * 1.0 c - * x s * +   z sq 1.0 z sq - c * +   3array
  44     3array ;
  45
  46 :: rotation-matrix4 ( axis theta -- matrix )
  47     theta cos :> c
  48     theta sin :> s
  49     axis first3 :> ( x y z )
  50     x sq 1.0 x sq - c * +     x y * 1.0 c - * z s * -   x z * 1.0 c - * y s * +   0 4array
  51     x y * 1.0 c - * z s * +   y sq 1.0 y sq - c * +     y z * 1.0 c - * x s * -   0 4array
  52     x z * 1.0 c - * y s * -   y z * 1.0 c - * x s * +   z sq 1.0 z sq - c * +     0 4array
  53     { 0.0 0.0 0.0 1.0 } 4array ;
  54
  55 :: translation-matrix4 ( offset -- matrix )
  56     offset first3 :> ( x y z )
  57     {
  58         { 1.0 0.0 0.0 x   }
  59         { 0.0 1.0 0.0 y   }
  60         { 0.0 0.0 1.0 z   }
  61         { 0.0 0.0 0.0 1.0 }
  62     } ;
  63
  64 : >scale-factors ( number/sequence -- x y z )
  65     dup number? [ dup dup ] [ first3 ] if ;
  66
  67 :: scale-matrix3 ( factors -- matrix )
  68     factors >scale-factors :> ( x y z )
  69     {
  70         { x   0.0 0.0 }
  71         { 0.0 y   0.0 }
  72         { 0.0 0.0 z   }
  73     } ;
  74
  75 :: scale-matrix4 ( factors -- matrix )
  76     factors >scale-factors :> ( x y z )
  77     {
  78         { x   0.0 0.0 0.0 }
  79         { 0.0 y   0.0 0.0 }
  80         { 0.0 0.0 z   0.0 }
  81         { 0.0 0.0 0.0 1.0 }
  82     } ;
  83
  84 : ortho-matrix4 ( dim -- matrix )
  85     [ recip ] map scale-matrix4 ;
  86
  87 :: frustum-matrix4 ( xy-dim near far -- matrix )
  88     xy-dim first2 :> ( x y )
  89     near x /f :> xf
  90     near y /f :> yf
  91     near far + near far - /f :> zf
  92     2 near far * * near far - /f :> wf
  93
  94     {
  95         { xf  0.0  0.0 0.0 }
  96         { 0.0 yf   0.0 0.0 }
  97         { 0.0 0.0  zf  wf  }
  98         { 0.0 0.0 -1.0 0.0 }
  99     } ;
 100
 101 :: skew-matrix4 ( theta -- matrix )
 102     theta tan :> zf
 103
 104     {
 105         { 1.0 0.0 0.0 0.0 }
 106         { 0.0 1.0 0.0 0.0 }
 107         { 0.0 zf  1.0 0.0 }
 108         { 0.0 0.0 0.0 1.0 }
 109     } ;
 110
 111 ! Matrix operations
 112 : mneg ( m -- m ) [ vneg ] map ;
 113
 114 : n*m ( n m -- m ) [ n*v ] with map ;
 115 : m*n ( m n -- m ) [ v*n ] curry map ;
 116 : n/m ( n m -- m ) [ n/v ] with map ;
 117 : m/n ( m n -- m ) [ v/n ] curry map ;
 118
 119 : m+   ( m m -- m ) [ v+ ] 2map ;
 120 : m-   ( m m -- m ) [ v- ] 2map ;
 121 : m*   ( m m -- m ) [ v* ] 2map ;
 122 : m/   ( m m -- m ) [ v/ ] 2map ;
 123
 124 : v.m ( v m -- v ) flip [ v. ] with map ;
 125 : m.v ( m v -- v ) [ v. ] curry map ;
 126 : m.  ( m m -- m ) flip [ swap m.v ] curry map ;
 127
 128 : m~  ( m m epsilon -- ? ) [ v~ ] curry 2all? ;
 129
 130 : mmin ( m -- n ) [ 1/0. ] dip [ [ min ] each ] each ;
 131 : mmax ( m -- n ) [ -1/0. ] dip [ [ max ] each ] each ;
 132 : mnorm ( m -- n ) dup mmax abs m/n ;
 133
 134 : cross ( vec1 vec2 -- vec3 )
 135     [ [ { 1 2 0 } vshuffle ] [ { 2 0 1 } vshuffle ] bi* v* ]
 136     [ [ { 2 0 1 } vshuffle ] [ { 1 2 0 } vshuffle ] bi* v* ] 2bi v- ; inline
 137
 138 :: normal ( vec1 vec2 vec3 -- vec4 )
 139     vec2 vec1 v- vec3 vec1 v- cross normalize ; inline
 140
 141 : proj ( v u -- w )
 142     [ [ v. ] [ norm-sq ] bi / ] keep n*v ;
 143
 144 : perp ( v u -- w )
 145     dupd proj v- ;
 146
 147 : angle-between ( v u -- a )
 148     [ normalize ] bi@ h. acos ;
 149
 150 : (gram-schmidt) ( v seq -- newseq )
 151     [ dupd proj v- ] each ;
 152
 153 : gram-schmidt ( seq -- orthogonal )
 154     V{ } clone [ over (gram-schmidt) over push ] reduce ;
 155
 156 : norm-gram-schmidt ( seq -- orthonormal )
 157     gram-schmidt [ normalize ] map ;
 158
 159 : m^n ( m n -- n )
 160     make-bits over first length identity-matrix
 161     [ [ dupd m. ] when [ dup m. ] dip ] reduce nip ;
 162
 163 : stitch ( m -- m' )
 164     [ ] [ [ append ] 2map ] map-reduce ;
 165
 166 : kron ( m1 m2 -- m )
 167     '[ [ _ n*m  ] map ] map stitch stitch ;