extra/math/matrices/simd/simd.factor

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