1 ! Copyright (C) 2005, 2009 Slava Pestov.
2 ! See http://factorcode.org/license.txt for BSD license.
3 USING: accessors arrays columns kernel locals math math.bits
4 math.functions math.order math.vectors sequences
5 sequences.private fry ;
9 : zero-matrix ( m n -- matrix )
10 '[ _ 0 <array> ] replicate ;
12 : identity-matrix ( n -- matrix )
13 #! Make a nxn identity matrix.
14 dup [ [ = 1 0 ? ] with map ] curry map ;
16 :: rotation-matrix3 ( axis theta -- matrix )
19 axis first3 :> z :> y :> x
20 x sq 1.0 x sq - c * + x y * 1.0 c - * z s * - x z * 1.0 c - * y s * + 3array
21 x y * 1.0 c - * z s * + y sq 1.0 y sq - c * + y z * 1.0 c - * x s * - 3array
22 x z * 1.0 c - * y s * - y z * 1.0 c - * x s * + z sq 1.0 z sq - c * + 3array
25 :: rotation-matrix4 ( axis theta -- matrix )
28 axis first3 :> z :> y :> x
29 x sq 1.0 x sq - c * + x y * 1.0 c - * z s * - x z * 1.0 c - * y s * + 0 4array
30 x y * 1.0 c - * z s * + y sq 1.0 y sq - c * + y z * 1.0 c - * x s * - 0 4array
31 x z * 1.0 c - * y s * - y z * 1.0 c - * x s * + z sq 1.0 z sq - c * + 0 4array
32 { 0.0 0.0 0.0 1.0 } 4array ;
34 :: translation-matrix4 ( offset -- matrix )
35 offset first3 :> z :> y :> x
43 : >scale-factors ( number/sequence -- x y z )
44 dup number? [ dup dup ] [ first3 ] if ;
46 :: scale-matrix3 ( factors -- matrix )
47 factors >scale-factors :> z :> y :> x
54 :: scale-matrix4 ( factors -- matrix )
55 factors >scale-factors :> z :> y :> x
63 : ortho-matrix4 ( dim -- matrix )
64 [ recip ] map scale-matrix4 ;
66 :: frustum-matrix4 ( xy-dim near far -- matrix )
67 xy-dim first2 :> y :> x
70 near far + near far - /f :> zf
71 2 near far * * near far - /f :> wf
80 :: skew-matrix4 ( theta -- matrix )
91 : mneg ( m -- m ) [ vneg ] map ;
93 : n*m ( n m -- m ) [ n*v ] with map ;
94 : m*n ( m n -- m ) [ v*n ] curry map ;
95 : n/m ( n m -- m ) [ n/v ] with map ;
96 : m/n ( m n -- m ) [ v/n ] curry map ;
98 : m+ ( m m -- m ) [ v+ ] 2map ;
99 : m- ( m m -- m ) [ v- ] 2map ;
100 : m* ( m m -- m ) [ v* ] 2map ;
101 : m/ ( m m -- m ) [ v/ ] 2map ;
103 : v.m ( v m -- v ) flip [ v. ] with map ;
104 : m.v ( m v -- v ) [ v. ] curry map ;
105 : m. ( m m -- m ) flip [ swap m.v ] curry map ;
107 : m~ ( m m epsilon -- ? ) [ v~ ] curry 2all? ;
109 : mmin ( m -- n ) [ 1/0. ] dip [ [ min ] each ] each ;
110 : mmax ( m -- n ) [ -1/0. ] dip [ [ max ] each ] each ;
111 : mnorm ( m -- n ) dup mmax abs m/n ;
115 : x ( seq -- elt ) first ; inline
116 : y ( seq -- elt ) second ; inline
117 : z ( seq -- elt ) third ; inline
119 : i ( seq1 seq2 -- n ) [ [ y ] [ z ] bi* * ] [ [ z ] [ y ] bi* * ] 2bi - ;
120 : j ( seq1 seq2 -- n ) [ [ z ] [ x ] bi* * ] [ [ x ] [ z ] bi* * ] 2bi - ;
121 : k ( seq1 seq2 -- n ) [ [ y ] [ x ] bi* * ] [ [ x ] [ y ] bi* * ] 2bi - ;
125 : cross ( vec1 vec2 -- vec3 ) [ [ i ] [ j ] [ k ] 2tri ] keep 3sequence ;
128 [ [ v. ] [ norm-sq ] bi / ] keep n*v ;
130 : (gram-schmidt) ( v seq -- newseq )
131 [ dupd proj v- ] each ;
133 : gram-schmidt ( seq -- orthogonal )
134 V{ } clone [ over (gram-schmidt) over push ] reduce ;
136 : norm-gram-schmidt ( seq -- orthonormal )
137 gram-schmidt [ normalize ] map ;
139 : cross-zip ( seq1 seq2 -- seq1xseq2 )
140 [ [ 2array ] with map ] curry map ;
143 make-bits over first length identity-matrix
144 [ [ dupd m. ] when [ dup m. ] dip ] reduce nip ;