1 ! Copyright (C) 2009, 2011 Joe Groff.
2 ! See http://factorcode.org/license.txt for BSD license.
3 USING: accessors classes.struct generalizations kernel
4 math math.functions math.matrices.simd math.vectors
5 math.vectors.simd math.quaternions sequences
6 sequences.generalizations sequences.private specialized-arrays
8 FROM: sequences.private => nth-unsafe ;
9 FROM: math.quaternions.private => (q*sign) ;
10 QUALIFIED-WITH: alien.c-types c
11 SPECIALIZED-ARRAY: float-4
12 IN: math.matrices.simd
15 { columns float-4[4] } ;
17 INSTANCE: matrix4 immutable-sequence
19 M: matrix4 length drop 4 ; inline
20 M: matrix4 nth-unsafe columns>> nth-unsafe ; inline
21 M: matrix4 new-sequence 2drop matrix4 (struct) ; inline
25 : columns ( a -- a1 a2 a3 a4 )
26 columns>> first4 ; inline
28 :: set-columns ( c1 c2 c3 c4 c -- c )
29 c columns>> :> columns
30 c1 c2 c3 c4 columns 4 set-firstn-unsafe
33 : make-matrix4 ( ..a quot: ( ..a -- ..b c1 c2 c3 c4 ) -- ..b c )
34 matrix4 (struct) swap dip set-columns ; inline
36 :: 2map-columns ( a b quot -- c )
38 a columns :> ( a1 a2 a3 a4 )
39 b columns :> ( b1 b2 b3 b4 )
45 ] make-matrix4 ; inline
47 : map-columns ( ... a quot: ( ... col -- ... newcol ) -- ... c )
48 '[ columns _ 4 napply ] make-matrix4 ; inline
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 TYPED: m4/ ( a: matrix4 b: matrix4 -- c: matrix4 ) [ v/ ] 2map-columns ;
57 TYPED: m4*n ( a: matrix4 b: float -- c: matrix4 ) [ v*n ] curry map-columns ;
58 TYPED: m4/n ( a: matrix4 b: float -- c: matrix4 ) [ v/n ] curry map-columns ;
59 TYPED: n*m4 ( a: float b: matrix4 -- c: matrix4 ) [ n*v ] with map-columns ;
60 TYPED: n/m4 ( a: float b: matrix4 -- c: matrix4 ) [ n/v ] with map-columns ;
62 TYPED:: m4. ( a: matrix4 b: matrix4 -- c: matrix4 )
64 a columns :> ( a1 a2 a3 a4 )
65 b columns :> ( b1 b2 b3 b4 )
67 b1 first a1 n*v :> c1a
68 b2 first a1 n*v :> c2a
69 b3 first a1 n*v :> c3a
70 b4 first a1 n*v :> c4a
72 b1 second a2 n*v c1a v+ :> c1b
73 b2 second a2 n*v c2a v+ :> c2b
74 b3 second a2 n*v c3a v+ :> c3b
75 b4 second a2 n*v c4a v+ :> c4b
77 b1 third a3 n*v c1b v+ :> c1c
78 b2 third a3 n*v c2b v+ :> c2c
79 b3 third a3 n*v c3b v+ :> c3c
80 b4 third a3 n*v c4b v+ :> c4c
82 b1 fourth a4 n*v c1c v+
83 b2 fourth a4 n*v c2c v+
84 b3 fourth a4 n*v c3c v+
85 b4 fourth a4 n*v c4c v+
88 TYPED:: m4.v ( m: matrix4 v: float-4 -- v': float-4 )
89 m columns :> ( m1 m2 m3 m4 )
96 TYPED:: v.m4 ( v: float-4 m: matrix4 -- c: float-4 )
97 m columns [ v vdot ] 4 napply float-4-boa ;
99 CONSTANT: identity-matrix4
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 }
109 CONSTANT: zero-matrix4
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 }
119 TYPED:: m4^n ( m: matrix4 n: fixnum -- m^n: matrix4 )
120 identity-matrix4 n [ m m4. ] times ;
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
127 TYPED: diagonal-matrix4 ( diagonal: float-4 -- matrix: matrix4 )
128 [ vmerge-diagonal [ vmerge-diagonal ] bi@ ] make-matrix4 ;
130 : vmerge-transpose ( a b c d -- a' b' c' d' )
131 [ (vmerge) ] bi-curry@ bi* ; inline
133 TYPED: transpose-matrix4 ( matrix: matrix4 -- matrix: matrix4 )
134 [ columns vmerge-transpose vmerge-transpose ] make-matrix4 ;
136 : linear>homogeneous ( v -- v' )
137 [ float-4{ t t t f } ] dip float-4{ 0.0 0.0 0.0 1.0 } v? ; inline
139 : scale-matrix4 ( factors -- matrix )
140 linear>homogeneous diagonal-matrix4 ; inline
142 : ortho-matrix4 ( factors -- matrix )
143 float-4{ 1.0 1.0 1.0 1.0 } swap v/ scale-matrix4 ; inline
145 TYPED: translation-matrix4 ( offset: float-4 -- matrix: matrix4 )
149 float-4{ 1.0 0.0 0.0 0.0 }
150 float-4{ 0.0 1.0 0.0 0.0 }
151 float-4{ 0.0 0.0 1.0 0.0 }
155 :: (rotation-matrix4) ( diagonal triangle-hi triangle-lo -- matrix )
156 matrix4 (struct) :> triangle-m
157 diagonal scale-matrix4 :> diagonal-m
159 triangle-hi { 3 2 1 3 } vshuffle
160 triangle-hi { 3 3 0 3 } vshuffle triangle-lo { 2 3 3 3 } vshuffle vbitor
161 triangle-lo { 1 0 3 3 } vshuffle
164 triangle-m set-columns drop
166 diagonal-m triangle-m m4+ ; inline
168 TYPED:: rotation-matrix4 ( axis: float-4 theta: float -- matrix: matrix4 )
169 ! x*x + c*(1.0 - x*x) x*y*(1.0 - c) + s*z x*z*(1.0 - c) - s*y 0
170 ! x*y*(1.0 - c) - s*z y*y + c*(1.0 - y*y) y*z*(1.0 - c) + s*x 0
171 ! x*z*(1.0 - c) + s*y y*z*(1.0 - c) - s*x z*z + c*(1.0 - z*z) 0
176 float-4{ -1.0 1.0 -1.0 0.0 } :> triangle-sign
180 1.0 float-4-with :> ones
182 axis axis v* :> axis2
184 axis2 cc ones axis2 v- v* v+ :> diagonal
186 axis { 1 0 0 3 } vshuffle axis { 2 2 1 3 } vshuffle v* 1-c v*
187 float-4{ t t t f } vbitand :> triangle-a
188 ss axis v* triangle-sign v* :> triangle-b
189 triangle-a triangle-b v+ :> triangle-lo
190 triangle-a triangle-b v- :> triangle-hi
192 diagonal triangle-hi triangle-lo (rotation-matrix4) ;
194 TYPED:: frustum-matrix4 ( xy: float-4 near: float far: float -- matrix: matrix4 )
196 near near near far + 2 near far * * float-4-boa ! num
197 float-4{ t t f f } xy near far - float-4-with v? ! denom
200 float-4{ 0.0 -1.0 0.0 0.0 } :> negone
204 [ negone (vmerge) ] bi*
207 ! interface with quaternions
209 float-4{ -0.0 0.0 0.0 0.0 } vbitxor ; inline
210 M: float-4 qconjugate
211 float-4{ 0.0 -0.0 -0.0 -0.0 } vbitxor ; inline
213 : euler4 ( phi theta psi -- q )
214 float-4{ 0 0 0 0 } euler-like ; inline
216 TYPED:: q>matrix4 ( q: float-4 -- matrix: matrix4 )
217 ! a*a + b*b - c*c - d*d 2*b*c - 2*a*d 2*b*d + 2*a*c 0
218 ! 2*b*c + 2*a*d a*a - b*b + c*c - d*d 2*c*d - 2*a*b 0
219 ! 2*b*d - 2*a*c 2*c*d + 2*a*b a*a - b*b - c*c + d*d 0
221 q { 2 1 1 3 } vshuffle q { 3 3 2 3 } vshuffle v* :> triangle-a
222 q { 0 0 0 3 } vshuffle q { 1 2 3 3 } vshuffle v* :> triangle-b
224 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*
225 [ v- ] [ v+ ] 2bi :> ( triangle-hi triangle-lo )
228 [ [ + ] [ - ] [ - ] tri* ]
229 [ [ - ] [ + ] [ - ] tri* ]
230 [ [ - ] [ - ] [ + ] tri* ]
231 } 4 ncleave 1.0 float-4-boa :> diagonal
233 diagonal triangle-hi triangle-lo (rotation-matrix4) ;