extra/math/matrices/simd/simd.factor

   1 ! (c)Joe Groff bsd license
   2 USING: accessors classes.struct 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 :: 2map-rows ( a b quot -- c )
  34     matrix4 (struct) :> c
  35
  36     a rows :> a4 :> a3 :> a2 :> a1
  37     b rows :> b4 :> b3 :> b2 :> b1
  38
  39     a1 b1 quot call
  40     a2 b2 quot call
  41     a3 b3 quot call
  42     a4 b4 quot call
  43
  44     c set-rows ; inline
  45
  46 :: map-rows ( a quot -- c )
  47     matrix4 (struct) :> c
  48
  49     a rows :> a4 :> a3 :> a2 :> a1
  50
  51     a1 quot call
  52     a2 quot call
  53     a3 quot call
  54     a4 quot call
  55
  56     c set-rows ; inline
  57
  58 PRIVATE>
  59
  60 TYPED: m4+ ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v+ ] 2map-rows ;
  61 TYPED: m4- ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v- ] 2map-rows ;
  62 TYPED: m4* ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v* ] 2map-rows ;
  63 TYPED: m4/ ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v/ ] 2map-rows ;
  64
  65 TYPED: m4*n ( a: matrix4 b: float -- c: matrix4 ) [ v*n ] curry map-rows ;
  66 TYPED: m4/n ( a: matrix4 b: float -- c: matrix4 ) [ v/n ] curry map-rows ;
  67 TYPED: n*m4 ( a: float b: matrix4 -- c: matrix4 ) [ n*v ] with map-rows ;
  68 TYPED: n/m4 ( a: float b: matrix4 -- c: matrix4 ) [ n/v ] with map-rows ;
  69
  70 TYPED:: m4. ( a: matrix4 b: matrix4 -- c: matrix4 )
  71     matrix4 (struct) :> c
  72
  73     a rows :> a4 :> a3 :> a2 :> a1
  74     b rows :> b4 :> b3 :> b2 :> b1
  75
  76     a1 first  b1 n*v :> c1a
  77     a2 first  b1 n*v :> c2a
  78     a3 first  b1 n*v :> c3a
  79     a4 first  b1 n*v :> c4a
  80
  81     a1 second b2 n*v c1a v+ :> c1b
  82     a2 second b2 n*v c2a v+ :> c2b
  83     a3 second b2 n*v c3a v+ :> c3b
  84     a4 second b2 n*v c4a v+ :> c4b
  85
  86     a1 third  b3 n*v c1b v+ :> c1c
  87     a2 third  b3 n*v c2b v+ :> c2c
  88     a3 third  b3 n*v c3b v+ :> c3c
  89     a4 third  b3 n*v c4b v+ :> c4c
  90
  91     a1 fourth b4 n*v c1c v+
  92     a2 fourth b4 n*v c2c v+
  93     a3 fourth b4 n*v c3c v+
  94     a4 fourth b4 n*v c4c v+
  95
  96     c set-rows ;
  97
  98 CONSTANT: identity-matrix4
  99     S{ matrix4 f
 100         float-4-array{
 101             float-4{ 1.0 0.0 0.0 0.0 }
 102             float-4{ 0.0 1.0 0.0 0.0 }
 103             float-4{ 0.0 0.0 1.0 0.0 }
 104             float-4{ 0.0 0.0 0.0 1.0 }
 105         }
 106     }
 107
 108 CONSTANT: zero-matrix4
 109     S{ matrix4 f
 110         float-4-array{
 111             float-4{ 0.0 0.0 0.0 0.0 }
 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         }
 116     }
 117
 118 TYPED:: m4^n ( m: matrix4 n: fixnum -- m^n: matrix4 )
 119     identity-matrix4 n [ m m4. ] times ;
 120
 121 TYPED:: scale-matrix4 ( factors: float-4 -- matrix: matrix4 )
 122     matrix4 (struct) :> c
 123
 124     factors { t t t f } vmask :> factors'
 125
 126     factors' { 0 3 3 3 } vshuffle
 127     factors' { 3 1 3 3 } vshuffle
 128     factors' { 3 3 2 3 } vshuffle
 129     float-4{ 0.0 0.0 0.0 1.0 }
 130
 131     c set-rows ;
 132
 133 : ortho-matrix4 ( factors -- matrix )
 134     float-4{ 1.0 1.0 1.0 1.0 } swap v/ scale-matrix4 ; inline
 135
 136 TYPED:: translation-matrix4 ( offset: float-4 -- matrix: matrix4 )
 137     matrix4 (struct) :> c
 138
 139     float-4{ 0.0 0.0 0.0 1.0 } :> c4
 140     { t t t f } offset c4 v? :> offset'
 141
 142     offset' { 3 3 3 0 } vshuffle { t f f t } vmask
 143     offset' { 3 3 3 1 } vshuffle { f t f t } vmask
 144     offset' { 3 3 3 2 } vshuffle { f f t t } vmask
 145     c4
 146
 147     c set-rows ;
 148
 149 TYPED:: rotation-matrix4 ( axis: float-4 theta: float -- matrix: matrix4 )
 150     !   x*x + c*(1.0 - x*x)   x*y*(1.0 - c) - s*z   x*z*(1.0 - c) + s*y   0
 151     !   x*y*(1.0 - c) + s*z   y*y + c*(1.0 - y*y)   y*z*(1.0 - c) - s*x   0
 152     !   x*z*(1.0 - c) - s*y   y*z*(1.0 - c) + s*x   z*z + c*(1.0 - z*z)   0
 153     !   0                     0                     0                     1
 154     matrix4 (struct) :> triangle-m
 155     theta cos :> c
 156     theta sin :> s
 157
 158     float-4{  1.0 -1.0  1.0 0.0 } :> triangle-sign
 159
 160     c float-4-with :> cc
 161     s float-4-with :> ss
 162     1.0 float-4-with :> ones
 163     ones cc v- :> 1-c
 164     axis axis v* :> axis2
 165
 166     axis2 cc ones axis2 v- v* v+ :> diagonal
 167
 168     axis { 0 0 1 3 } vshuffle axis { 1 2 2 3 } vshuffle v* 1-c v*
 169     { t t t f } vmask :> triangle-a
 170     ss { 2 1 0 3 } vshuffle triangle-sign v* :> triangle-b
 171     triangle-a triangle-b v+ :> triangle-lo
 172     triangle-a triangle-b v- :> triangle-hi
 173
 174     diagonal scale-matrix4 :> diagonal-m
 175
 176     triangle-hi { 3 0 1 3 } vshuffle
 177     triangle-hi { 3 3 2 3 } vshuffle triangle-lo { 0 3 3 3 } vshuffle v+
 178     triangle-lo { 1 2 3 3 } vshuffle
 179     float-4 new
 180
 181     triangle-m set-rows drop
 182
 183     diagonal-m triangle-m m4+ ;
 184
 185 TYPED:: frustum-matrix4 ( xy: float-4 near: float far: float -- matrix: matrix4 )
 186     matrix4 (struct) :> c
 187
 188     near near near far + 2 near far * * float-4-boa :> num
 189     { t t f f } xy near far - float-4-with v? :> denom
 190     num denom v/ :> fov
 191
 192     fov { 0 0 0 0 } vshuffle { t f f f } vmask
 193     fov { 1 1 1 1 } vshuffle { f t f f } vmask
 194     fov { 2 2 2 3 } vshuffle { f f t t } vmask
 195     float-4{ 0.0 0.0 -1.0 0.0 }
 196
 197     c set-rows ;
 198