1 ! (c)Joe Groff bsd license
2 USING: accessors classes.struct generalizations kernel locals
3 math math.functions math.matrices.simd math.vectors
4 math.vectors.simd sequences sequences.private specialized-arrays
6 QUALIFIED-WITH: alien.c-types c
8 SPECIALIZED-ARRAY: float-4
14 INSTANCE: matrix4 immutable-sequence
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
22 : rows ( a -- a1 a2 a3 a4 )
23 rows>> first4 ; inline
25 :: set-rows ( c1 c2 c3 c4 c -- c )
33 :: 2map-rows ( a b quot -- c )
36 a rows :> a4 :> a3 :> a2 :> a1
37 b rows :> b4 :> b3 :> b2 :> b1
46 :: map-rows ( a quot -- c )
49 a rows :> a4 :> a3 :> a2 :> a1
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 ;
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 ;
70 TYPED:: m4. ( a: matrix4 b: matrix4 -- c: matrix4 )
73 a rows :> a4 :> a3 :> a2 :> a1
74 b rows :> b4 :> b3 :> b2 :> b1
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
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
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
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+
98 CONSTANT: identity-matrix4
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 }
108 TYPED:: scale-matrix4 ( factors: float-4 -- matrix: matrix4 )
109 matrix4 (struct) :> c
111 factors { t t t f } vmask :> factors'
113 factors' { 0 3 3 3 } vshuffle
114 factors' { 3 1 3 3 } vshuffle
115 factors' { 3 3 2 3 } vshuffle
116 float-4{ 0.0 0.0 0.0 1.0 }
120 : ortho-matrix4 ( factors -- matrix )
121 float-4{ 1.0 1.0 1.0 1.0 } swap v/ scale-matrix4 ; inline
123 TYPED:: translation-matrix4 ( offset: float-4 -- matrix: matrix4 )
124 matrix4 (struct) :> c
126 float-4{ 0.0 0.0 0.0 1.0 } :> c4
127 { t t t f } offset c4 v? :> offset'
129 offset' { 3 3 3 0 } vshuffle { t f f t } vmask
130 offset' { 3 3 3 1 } vshuffle { f t f t } vmask
131 offset' { 3 3 3 2 } vshuffle { f f t t } vmask
136 TYPED:: rotation-matrix4 ( axis: float-4 theta: float -- matrix: matrix4 )
137 ! x*x + c*(1.0 - x*x) x*y*(1.0 - c) - s*z x*z*(1.0 - c) + s*y 0
138 ! x*y*(1.0 - c) + s*z y*y + c*(1.0 - y*y) y*z*(1.0 - c) - s*x 0
139 ! x*z*(1.0 - c) - s*y y*z*(1.0 - c) + s*x z*z + c*(1.0 - z*z) 0
141 matrix4 (struct) :> triangle-m
145 float-4{ 1.0 -1.0 1.0 0.0 } :> triangle-sign
149 1.0 float-4-with :> ones
151 axis axis v* :> axis2
153 axis2 cc ones axis2 v- v* v+ :> diagonal
155 axis { 0 0 1 3 } vshuffle axis { 1 2 2 3 } vshuffle v* 1-c v*
156 { t t t f } vmask :> triangle-a
157 ss { 2 1 0 3 } vshuffle triangle-sign v* :> triangle-b
158 triangle-a triangle-b v+ :> triangle-lo
159 triangle-a triangle-b v- :> triangle-hi
161 diagonal scale-matrix4 :> diagonal-m
163 triangle-hi { 3 0 1 3 } vshuffle
164 triangle-hi { 3 3 2 3 } vshuffle triangle-lo { 0 3 3 3 } vshuffle v+
165 triangle-lo { 1 2 3 3 } vshuffle
168 triangle-m set-rows drop
170 diagonal-m triangle-m m4+ ;
172 TYPED:: frustum-matrix4 ( xy: float-4 near: float far: float -- matrix: matrix4 )
173 matrix4 (struct) :> c
175 near near near far + 2 near far * * float-4-boa :> num
176 { t t f f } xy near far - float-4-with v? :> denom
179 fov { 0 0 0 0 } vshuffle { t f f f } vmask
180 fov { 1 1 1 1 } vshuffle { f t f f } vmask
181 fov { 2 2 2 3 } vshuffle { f f t t } vmask
182 float-4{ 0.0 0.0 -1.0 0.0 }