basis/math/matrices/matrices.factor

   1 ! Copyright (C) 2005, 2009 Slava Pestov.
   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 : identity-matrix ( n -- matrix )
  13     #! Make a nxn identity matrix.
  14     dup [ [ = 1 0 ? ] with map ] curry map ;
  15
  16 :: rotation-matrix3 ( axis theta -- matrix )
  17     theta cos :> c
  18     theta sin :> s
  19     axis first3 :> z :> y :> x
  20     x sq 1.0 x sq - c * +     x y * 1.0 c - * z s * -   x z * 1.0 c - * y s * + 3array
  21     x y * 1.0 c - * z s * +   y sq 1.0 y sq - c * +     y z * 1.0 c - * x s * - 3array
  22     x z * 1.0 c - * y s * -   y z * 1.0 c - * x s * +   z sq 1.0 z sq - c * +   3array
  23     3array ;
  24
  25 :: rotation-matrix4 ( axis theta -- matrix )
  26     theta cos :> c
  27     theta sin :> s
  28     axis first3 :> z :> y :> x
  29     x sq 1.0 x sq - c * +     x y * 1.0 c - * z s * -   x z * 1.0 c - * y s * +   0 4array
  30     x y * 1.0 c - * z s * +   y sq 1.0 y sq - c * +     y z * 1.0 c - * x s * -   0 4array
  31     x z * 1.0 c - * y s * -   y z * 1.0 c - * x s * +   z sq 1.0 z sq - c * +     0 4array
  32     { 0.0 0.0 0.0 1.0 } 4array ;
  33
  34 :: translation-matrix4 ( offset -- matrix )
  35     offset first3 :> z :> y :> x
  36     {
  37         { 1.0 0.0 0.0 x   }
  38         { 0.0 1.0 0.0 y   }
  39         { 0.0 0.0 1.0 z   }
  40         { 0.0 0.0 0.0 1.0 }
  41     } ;
  42
  43 : >scale-factors ( number/sequence -- x y z )
  44     dup number? [ dup dup ] [ first3 ] if ;
  45
  46 :: scale-matrix3 ( factors -- matrix )
  47     factors >scale-factors :> z :> y :> x
  48     {
  49         { x   0.0 0.0 }
  50         { 0.0 y   0.0 }
  51         { 0.0 0.0 z   }
  52     } ;
  53
  54 :: scale-matrix4 ( factors -- matrix )
  55     factors >scale-factors :> z :> y :> x
  56     {
  57         { x   0.0 0.0 0.0 }
  58         { 0.0 y   0.0 0.0 }
  59         { 0.0 0.0 z   0.0 }
  60         { 0.0 0.0 0.0 1.0 }
  61     } ;
  62
  63 : ortho-matrix4 ( dim -- matrix )
  64     [ recip ] map scale-matrix4 ;
  65
  66 :: frustum-matrix4 ( xy-dim near far -- matrix )
  67     xy-dim first2 :> y :> x
  68     near x /f :> xf
  69     near y /f :> yf
  70     near far + near far - /f :> zf
  71     2 near far * * near far - /f :> wf
  72
  73     {
  74         { xf  0.0  0.0 0.0 }
  75         { 0.0 yf   0.0 0.0 }
  76         { 0.0 0.0  zf  wf  }
  77         { 0.0 0.0 -1.0 0.0 }
  78     } ;
  79
  80 :: skew-matrix4 ( theta -- matrix )
  81     theta tan :> zf
  82
  83     {
  84         { 1.0 0.0 0.0 0.0 }
  85         { 0.0 1.0 0.0 0.0 }
  86         { 0.0 zf  1.0 0.0 }
  87         { 0.0 0.0 0.0 1.0 }
  88     } ;
  89
  90 ! Matrix operations
  91 : mneg ( m -- m ) [ vneg ] map ;
  92
  93 : n*m ( n m -- m ) [ n*v ] with map ;
  94 : m*n ( m n -- m ) [ v*n ] curry map ;
  95 : n/m ( n m -- m ) [ n/v ] with map ;
  96 : m/n ( m n -- m ) [ v/n ] curry map ;
  97
  98 : m+   ( m m -- m ) [ v+ ] 2map ;
  99 : m-   ( m m -- m ) [ v- ] 2map ;
 100 : m*   ( m m -- m ) [ v* ] 2map ;
 101 : m/   ( m m -- m ) [ v/ ] 2map ;
 102
 103 : v.m ( v m -- v ) flip [ v. ] with map ;
 104 : m.v ( m v -- v ) [ v. ] curry map ;
 105 : m.  ( m m -- m ) flip [ swap m.v ] curry map ;
 106
 107 : mmin ( m -- n ) [ 1/0. ] dip [ [ min ] each ] each ;
 108 : mmax ( m -- n ) [ -1/0. ] dip [ [ max ] each ] each ;
 109 : mnorm ( m -- n ) dup mmax abs m/n ;
 110
 111 <PRIVATE
 112
 113 : x ( seq -- elt ) first ; inline
 114 : y ( seq -- elt ) second ; inline
 115 : z ( seq -- elt ) third ; inline
 116
 117 : i ( seq1 seq2 -- n ) [ [ y ] [ z ] bi* * ] [ [ z ] [ y ] bi* * ] 2bi - ;
 118 : j ( seq1 seq2 -- n ) [ [ z ] [ x ] bi* * ] [ [ x ] [ z ] bi* * ] 2bi - ;
 119 : k ( seq1 seq2 -- n ) [ [ y ] [ x ] bi* * ] [ [ x ] [ y ] bi* * ] 2bi - ;
 120
 121 PRIVATE>
 122
 123 : cross ( vec1 vec2 -- vec3 ) [ [ i ] [ j ] [ k ] 2tri ] keep 3sequence ;
 124
 125 : proj ( v u -- w )
 126     [ [ v. ] [ norm-sq ] bi / ] keep n*v ;
 127
 128 : (gram-schmidt) ( v seq -- newseq )
 129     [ dupd proj v- ] each ;
 130
 131 : gram-schmidt ( seq -- orthogonal )
 132     V{ } clone [ over (gram-schmidt) over push ] reduce ;
 133
 134 : norm-gram-schmidt ( seq -- orthonormal )
 135     gram-schmidt [ normalize ] map ;
 136
 137 : cross-zip ( seq1 seq2 -- seq1xseq2 )
 138     [ [ 2array ] with map ] curry map ;
 139
 140 : m^n ( m n -- n )
 141     make-bits over first length identity-matrix
 142     [ [ dupd m. ] when [ dup m. ] dip ] reduce nip ;