extra/math/matrices/simd/simd.factor

   1 ! (c)Joe Groff bsd license
   2 USING: accessors classes.struct fry generalizations kernel locals
   3 math math.combinatorics math.functions math.matrices.simd math.vectors
   4 math.vectors.simd sequences sequences.private specialized-arrays
   5 typed ;
   6 QUALIFIED-WITH: alien.c-types c
   7 SIMD: c:float
   8 SPECIALIZED-ARRAY: float-4
   9 IN: math.matrices.simd
  10
  11 STRUCT: matrix4
  12     { rows float-4[4] } ;
  13
  14 INSTANCE: matrix4 immutable-sequence
  15
  16 M: matrix4 length drop 4 ; inline
  17 M: matrix4 nth-unsafe rows>> nth-unsafe ; inline
  18 M: matrix4 new-sequence 2drop matrix4 (struct) ; inline
  19
  20 <PRIVATE
  21
  22 : rows ( a -- a1 a2 a3 a4 )
  23     rows>> 4 firstn ; inline
  24
  25 :: set-rows ( c1 c2 c3 c4 c -- c )
  26     c rows>> :> rows
  27     c1 rows set-first
  28     c2 rows set-second
  29     c3 rows set-third
  30     c4 rows set-fourth
  31     c ; inline
  32
  33 : make-matrix4 ( quot: ( -- c1 c2 c3 c4 ) -- c )
  34     matrix4 (struct) swap dip set-rows ; inline
  35
  36 :: 2map-rows ( a b quot -- c )
  37     [
  38         a rows :> a4 :> a3 :> a2 :> a1
  39         b rows :> b4 :> b3 :> b2 :> b1
  40
  41         a1 b1 quot call
  42         a2 b2 quot call
  43         a3 b3 quot call
  44         a4 b4 quot call
  45     ] make-matrix4 ; inline
  46
  47 : map-rows ( a quot -- c )
  48     '[ rows _ 4 napply ] make-matrix4 ; inline
  49
  50 PRIVATE>
  51
  52 TYPED: m4+ ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v+ ] 2map-rows ;
  53 TYPED: m4- ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v- ] 2map-rows ;
  54 TYPED: m4* ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v* ] 2map-rows ;
  55 TYPED: m4/ ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v/ ] 2map-rows ;
  56
  57 TYPED: m4*n ( a: matrix4 b: float -- c: matrix4 ) [ v*n ] curry map-rows ;
  58 TYPED: m4/n ( a: matrix4 b: float -- c: matrix4 ) [ v/n ] curry map-rows ;
  59 TYPED: n*m4 ( a: float b: matrix4 -- c: matrix4 ) [ n*v ] with map-rows ;
  60 TYPED: n/m4 ( a: float b: matrix4 -- c: matrix4 ) [ n/v ] with map-rows ;
  61
  62 TYPED:: m4. ( a: matrix4 b: matrix4 -- c: matrix4 )
  63     [
  64         a rows :> a4 :> a3 :> a2 :> a1
  65         b rows :> b4 :> b3 :> b2 :> b1
  66
  67         a1 first  b1 n*v :> c1a
  68         a2 first  b1 n*v :> c2a
  69         a3 first  b1 n*v :> c3a
  70         a4 first  b1 n*v :> c4a
  71
  72         a1 second b2 n*v c1a v+ :> c1b
  73         a2 second b2 n*v c2a v+ :> c2b
  74         a3 second b2 n*v c3a v+ :> c3b
  75         a4 second b2 n*v c4a v+ :> c4b
  76
  77         a1 third  b3 n*v c1b v+ :> c1c
  78         a2 third  b3 n*v c2b v+ :> c2c
  79         a3 third  b3 n*v c3b v+ :> c3c
  80         a4 third  b3 n*v c4b v+ :> c4c
  81
  82         a1 fourth b4 n*v c1c v+
  83         a2 fourth b4 n*v c2c v+
  84         a3 fourth b4 n*v c3c v+
  85         a4 fourth b4 n*v c4c v+
  86     ] make-matrix4 ;
  87
  88 TYPED:: v.m4 ( a: float-4 b: matrix4 -- c: float-4 )
  89     b rows :> b4 :> b3 :> b2 :> b1
  90
  91     a first  b1 n*v
  92     a second b2 n*v v+
  93     a third  b3 n*v v+
  94     a fourth b4 n*v v+ ;
  95
  96 TYPED:: m4.v ( a: matrix4 b: float-4 -- c: float-4 )
  97     a rows [ b v. ] 4 napply float-4-boa ;
  98
  99 CONSTANT: identity-matrix4
 100     S{ matrix4 f
 101         float-4-array{
 102             float-4{ 1.0 0.0 0.0 0.0 }
 103             float-4{ 0.0 1.0 0.0 0.0 }
 104             float-4{ 0.0 0.0 1.0 0.0 }
 105             float-4{ 0.0 0.0 0.0 1.0 }
 106         }
 107     }
 108
 109 CONSTANT: zero-matrix4
 110     S{ matrix4 f
 111         float-4-array{
 112             float-4{ 0.0 0.0 0.0 0.0 }
 113             float-4{ 0.0 0.0 0.0 0.0 }
 114             float-4{ 0.0 0.0 0.0 0.0 }
 115             float-4{ 0.0 0.0 0.0 0.0 }
 116         }
 117     }
 118
 119 TYPED:: m4^n ( m: matrix4 n: fixnum -- m^n: matrix4 )
 120     identity-matrix4 n [ m m4. ] times ;
 121
 122 : vmerge-diagonal* ( x y -- h t )
 123     [ (vmerge-head) ] [ swap (vmerge-tail) ] 2bi ; inline
 124 : vmerge-diagonal ( x -- h t )
 125     0.0 float-4-with vmerge-diagonal* ; inline
 126
 127 TYPED: diagonal-matrix4 ( diagonal: float-4 -- matrix: matrix4 )
 128     [ vmerge-diagonal [ vmerge-diagonal ] bi@ ] make-matrix4 ;
 129
 130 : vmerge-transpose ( a b c d -- a' b' c' d' )
 131     [ (vmerge) ] bi-curry@ bi* ; inline
 132
 133 TYPED: transpose-matrix4 ( matrix: matrix4 -- matrix: matrix4 )
 134     [ rows vmerge-transpose vmerge-transpose ] make-matrix4 ;
 135
 136 : scale-matrix4 ( factors -- matrix )
 137     [ float-4{ t t t f } ] dip float-4{ 0.0 0.0 0.0 1.0 } v?
 138     diagonal-matrix4 ; inline
 139
 140 : ortho-matrix4 ( factors -- matrix )
 141     float-4{ 1.0 1.0 1.0 1.0 } swap v/ scale-matrix4 ; inline
 142
 143 TYPED:: translation-matrix4 ( offset: float-4 -- matrix: matrix4 )
 144     [
 145         float-4{ 1.0 1.0 1.0 1.0 } :> diagonal
 146
 147         offset 0 float-4-with (vmerge)
 148         [ 0 float-4-with swap (vmerge) ] bi@ drop :> z :> y :> x
 149
 150         diagonal y vmerge-diagonal*
 151         [ x vmerge-diagonal* ]
 152         [ z vmerge-diagonal* ] bi*
 153     ] make-matrix4 ;
 154
 155 TYPED:: rotation-matrix4 ( axis: float-4 theta: float -- matrix: matrix4 )
 156     !   x*x + c*(1.0 - x*x)   x*y*(1.0 - c) - s*z   x*z*(1.0 - c) + s*y   0
 157     !   x*y*(1.0 - c) + s*z   y*y + c*(1.0 - y*y)   y*z*(1.0 - c) - s*x   0
 158     !   x*z*(1.0 - c) - s*y   y*z*(1.0 - c) + s*x   z*z + c*(1.0 - z*z)   0
 159     !   0                     0                     0                     1
 160     matrix4 (struct) :> triangle-m
 161     theta cos :> c
 162     theta sin :> s
 163
 164     float-4{  1.0 -1.0  1.0 0.0 } :> triangle-sign
 165
 166     c float-4-with :> cc
 167     s float-4-with :> ss
 168     1.0 float-4-with :> ones
 169     ones cc v- :> 1-c
 170     axis axis v* :> axis2
 171
 172     axis2 cc ones axis2 v- v* v+ :> diagonal
 173
 174     axis { 1 0 0 3 } vshuffle axis { 2 2 1 3 } vshuffle v* 1-c v*
 175     float-4{ t t t f } vbitand :> triangle-a
 176     ss axis v* triangle-sign v* :> triangle-b
 177     triangle-a triangle-b v+ :> triangle-lo
 178     triangle-a triangle-b v- :> triangle-hi
 179
 180     diagonal scale-matrix4 :> diagonal-m
 181
 182     triangle-hi { 3 2 1 3 } vshuffle
 183     triangle-hi { 3 3 0 3 } vshuffle triangle-lo { 2 3 3 3 } vshuffle v+
 184     triangle-lo { 1 0 3 3 } vshuffle
 185     float-4 new
 186
 187     triangle-m set-rows drop
 188
 189     diagonal-m triangle-m m4+ ;
 190
 191 TYPED:: frustum-matrix4 ( xy: float-4 near: float far: float -- matrix: matrix4 )
 192     [
 193         near near near far + 2 near far * * float-4-boa ! num
 194         float-4{ t t f f } xy near far - float-4-with v? ! denom
 195         v/ :> fov
 196
 197         fov 0.0 float-4-with (vmerge-head) vmerge-diagonal
 198         fov float-4{ f f t t } vand
 199         float-4{ 0.0 0.0 -1.0 0.0 }
 200     ] make-matrix4 ;
 201