extra/math/matrices/simd/simd.factor

   1 ! (c)Joe Groff bsd license
   2 USING: accessors classes.struct fry generalizations kernel
   3 locals math math.combinatorics math.functions math.matrices.simd
   4 math.vectors math.vectors.simd math.quaternions sequences
   5 sequences.generalizations sequences.private specialized-arrays
   6 typed ;
   7 FROM: sequences.private => nth-unsafe ;
   8 FROM: math.quaternions.private => (q*sign) ;
   9 QUALIFIED-WITH: alien.c-types c
  10 SPECIALIZED-ARRAY: float-4
  11 IN: math.matrices.simd
  12
  13 STRUCT: matrix4
  14     { columns float-4[4] } ;
  15
  16 INSTANCE: matrix4 immutable-sequence
  17
  18 M: matrix4 length drop 4 ; inline
  19 M: matrix4 nth-unsafe columns>> nth-unsafe ; inline
  20 M: matrix4 new-sequence 2drop matrix4 (struct) ; inline
  21
  22 <PRIVATE
  23
  24 : columns ( a -- a1 a2 a3 a4 )
  25     columns>> first4 ; inline
  26
  27 :: set-columns ( c1 c2 c3 c4 c -- c )
  28     c columns>> :> columns
  29     c1 c2 c3 c4 columns 4 set-firstn-unsafe
  30     c ; inline
  31
  32 : make-matrix4 ( ..a quot: ( ..a -- ..b c1 c2 c3 c4 ) -- ..b c )
  33     matrix4 (struct) swap dip set-columns ; inline
  34
  35 :: 2map-columns ( a b quot -- c )
  36     [
  37         a columns :> ( a1 a2 a3 a4 )
  38         b columns :> ( b1 b2 b3 b4 )
  39
  40         a1 b1 quot call
  41         a2 b2 quot call
  42         a3 b3 quot call
  43         a4 b4 quot call
  44     ] make-matrix4 ; inline
  45
  46 : map-columns ( ... a quot: ( ... col -- ... newcol ) -- ... c )
  47     '[ columns _ 4 napply ] make-matrix4 ; inline
  48
  49 PRIVATE>
  50
  51 TYPED: m4+ ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v+ ] 2map-columns ;
  52 TYPED: m4- ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v- ] 2map-columns ;
  53 TYPED: m4* ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v* ] 2map-columns ;
  54 TYPED: m4/ ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v/ ] 2map-columns ;
  55
  56 TYPED: m4*n ( a: matrix4 b: float -- c: matrix4 ) [ v*n ] curry map-columns ;
  57 TYPED: m4/n ( a: matrix4 b: float -- c: matrix4 ) [ v/n ] curry map-columns ;
  58 TYPED: n*m4 ( a: float b: matrix4 -- c: matrix4 ) [ n*v ] with map-columns ;
  59 TYPED: n/m4 ( a: float b: matrix4 -- c: matrix4 ) [ n/v ] with map-columns ;
  60
  61 TYPED:: m4. ( a: matrix4 b: matrix4 -- c: matrix4 )
  62     [
  63         a columns :> ( a1 a2 a3 a4 )
  64         b columns :> ( b1 b2 b3 b4 )
  65
  66         b1 first  a1 n*v :> c1a
  67         b2 first  a1 n*v :> c2a
  68         b3 first  a1 n*v :> c3a
  69         b4 first  a1 n*v :> c4a
  70
  71         b1 second a2 n*v c1a v+ :> c1b
  72         b2 second a2 n*v c2a v+ :> c2b
  73         b3 second a2 n*v c3a v+ :> c3b
  74         b4 second a2 n*v c4a v+ :> c4b
  75
  76         b1 third  a3 n*v c1b v+ :> c1c
  77         b2 third  a3 n*v c2b v+ :> c2c
  78         b3 third  a3 n*v c3b v+ :> c3c
  79         b4 third  a3 n*v c4b v+ :> c4c
  80
  81         b1 fourth a4 n*v c1c v+
  82         b2 fourth a4 n*v c2c v+
  83         b3 fourth a4 n*v c3c v+
  84         b4 fourth a4 n*v c4c v+
  85     ] make-matrix4 ;
  86
  87 TYPED:: m4.v ( m: matrix4 v: float-4 -- v': float-4 )
  88     m columns :> ( m1 m2 m3 m4 )
  89
  90     v first  m1 n*v
  91     v second m2 n*v v+
  92     v third  m3 n*v v+
  93     v fourth m4 n*v v+ ;
  94
  95 TYPED:: v.m4 ( v: float-4 m: matrix4 -- c: float-4 )
  96     m columns [ v v. ] 4 napply float-4-boa ;
  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 : vmerge-diagonal* ( x y -- h t )
 122     [ (vmerge-head) ] [ swap (vmerge-tail) ] 2bi ; inline
 123 : vmerge-diagonal ( x -- h t )
 124     0.0 float-4-with vmerge-diagonal* ; inline
 125
 126 TYPED: diagonal-matrix4 ( diagonal: float-4 -- matrix: matrix4 )
 127     [ vmerge-diagonal [ vmerge-diagonal ] bi@ ] make-matrix4 ;
 128
 129 : vmerge-transpose ( a b c d -- a' b' c' d' )
 130     [ (vmerge) ] bi-curry@ bi* ; inline
 131
 132 TYPED: transpose-matrix4 ( matrix: matrix4 -- matrix: matrix4 )
 133     [ columns vmerge-transpose vmerge-transpose ] make-matrix4 ;
 134
 135 : linear>homogeneous ( v -- v' )
 136     [ float-4{ t t t f } ] dip float-4{ 0.0 0.0 0.0 1.0 } v? ; inline
 137
 138 : scale-matrix4 ( factors -- matrix )
 139     linear>homogeneous diagonal-matrix4 ; inline
 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     [
 146         linear>homogeneous
 147         [
 148             float-4{ 1.0 0.0 0.0 0.0 }
 149             float-4{ 0.0 1.0 0.0 0.0 }
 150             float-4{ 0.0 0.0 1.0 0.0 }
 151         ] dip
 152     ] make-matrix4 ;
 153
 154 :: (rotation-matrix4) ( diagonal triangle-hi triangle-lo -- matrix )
 155     matrix4 (struct) :> triangle-m
 156     diagonal scale-matrix4 :> diagonal-m
 157
 158     triangle-hi { 3 2 1 3 } vshuffle
 159     triangle-hi { 3 3 0 3 } vshuffle triangle-lo { 2 3 3 3 } vshuffle vbitor
 160                                      triangle-lo { 1 0 3 3 } vshuffle
 161     float-4 new
 162
 163     triangle-m set-columns drop
 164
 165     diagonal-m triangle-m m4+ ; inline
 166
 167 TYPED:: rotation-matrix4 ( axis: float-4 theta: float -- matrix: matrix4 )
 168     !   x*x + c*(1.0 - x*x)   x*y*(1.0 - c) + s*z   x*z*(1.0 - c) - s*y   0
 169     !   x*y*(1.0 - c) - s*z   y*y + c*(1.0 - y*y)   y*z*(1.0 - c) + s*x   0
 170     !   x*z*(1.0 - c) + s*y   y*z*(1.0 - c) - s*x   z*z + c*(1.0 - z*z)   0
 171     !   0                     0                     0                     1
 172     theta cos :> c
 173     theta sin :> s
 174
 175     float-4{ -1.0  1.0 -1.0 0.0 } :> triangle-sign
 176
 177     c float-4-with :> cc
 178     s float-4-with :> ss
 179     1.0 float-4-with :> ones
 180     ones cc v- :> 1-c
 181     axis axis v* :> axis2
 182
 183     axis2 cc ones axis2 v- v* v+ :> diagonal
 184
 185     axis { 1 0 0 3 } vshuffle axis { 2 2 1 3 } vshuffle v* 1-c v*
 186     float-4{ t t t f } vbitand :> triangle-a
 187     ss axis v* triangle-sign v* :> triangle-b
 188     triangle-a triangle-b v+ :> triangle-lo
 189     triangle-a triangle-b v- :> triangle-hi
 190
 191     diagonal triangle-hi triangle-lo (rotation-matrix4) ;
 192
 193 TYPED:: frustum-matrix4 ( xy: float-4 near: float far: float -- matrix: matrix4 )
 194     [
 195         near near near far + 2 near far * * float-4-boa ! num
 196         float-4{ t t f f } xy near far - float-4-with v? ! denom
 197         v/ :> fov
 198
 199         float-4{ 0.0 -1.0 0.0 0.0 } :> negone
 200
 201         fov vmerge-diagonal
 202         [ vmerge-diagonal ]
 203         [ negone (vmerge) ] bi*
 204     ] make-matrix4 ;
 205
 206 ! interface with quaternions
 207 M: float-4 (q*sign)
 208     float-4{ -0.0  0.0  0.0  0.0 } vbitxor ; inline
 209 M: float-4 qconjugate
 210     float-4{  0.0 -0.0 -0.0 -0.0 } vbitxor ; inline
 211
 212 : euler4 ( phi theta psi -- q )
 213     float-4{ 0 0 0 0 } euler-like ; inline
 214
 215 TYPED:: q>matrix4 ( q: float-4 -- matrix: matrix4 )
 216     !   a*a + b*b - c*c - d*d  2*b*c - 2*a*d          2*b*d + 2*a*c          0
 217     !   2*b*c + 2*a*d          a*a - b*b + c*c - d*d  2*c*d - 2*a*b          0
 218     !   2*b*d - 2*a*c          2*c*d + 2*a*b          a*a - b*b - c*c + d*d  0
 219     !   0                      0                      0                      1
 220     q { 2 1 1 3 } vshuffle  q { 3 3 2 3 } vshuffle  v*  :> triangle-a
 221     q { 0 0 0 3 } vshuffle  q { 1 2 3 3 } vshuffle  v*  :> triangle-b
 222
 223     triangle-a float-4{ 2.0 2.0 2.0 0.0 } v*  triangle-b float-4{ -2.0 2.0 -2.0 0.0 } v*
 224     [ v- ] [ v+ ] 2bi :> ( triangle-hi triangle-lo )
 225
 226     q q v* first4 {
 227         [ [ + ] [ - ] [ - ] tri* ]
 228         [ [ - ] [ + ] [ - ] tri* ]
 229         [ [ - ] [ - ] [ + ] tri* ]
 230     } 4 ncleave 1.0 float-4-boa :> diagonal
 231
 232     diagonal triangle-hi triangle-lo (rotation-matrix4) ;