1 REQUIRES: libs/math libs/vars ;
2 USING: kernel math namespaces sequences arrays math-contrib vars ;
5 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
7 TUPLE: turtle position orientation ;
9 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
13 : position> ( -- position ) turtle> turtle-position ;
15 : >position ( position -- ) turtle> set-turtle-position ;
17 : orientation> ( -- orientation ) turtle> turtle-orientation ;
19 : >orientation ( orientation -- ) turtle> set-turtle-orientation ;
21 : with-turtle ( quot turtle -- ) [ >turtle call ] with-scope ;
23 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
25 : reset-turtle ( -- ) { 0 0 0 } >position 3 identity-matrix >orientation ;
27 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
29 C: turtle ( -- ) [ reset-turtle ] over with-turtle ;
31 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
33 : make-matrix >r { } make r> group ;
35 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
37 ! These rotation matrices are from
38 ! `Computer Graphics: Principles and Practice'
40 : Rz ( angle -- Rx ) deg>rad
41 [ dup cos , dup sin neg , 0 ,
42 dup sin , dup cos , 0 ,
43 0 , 0 , 1 , ] 3 make-matrix nip ;
45 : Ry ( angle -- Ry ) deg>rad
46 [ dup cos , 0 , dup sin ,
48 dup sin neg , 0 , dup cos , ] 3 make-matrix nip ;
50 : Rx ( angle -- Rz ) deg>rad
52 0 , dup cos , dup sin neg ,
53 0 , dup sin , dup cos , ] 3 make-matrix nip ;
55 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
57 : apply-rotation ( rotation -- ) orientation> swap m. >orientation ;
59 : rotate-x ( angle -- ) Rx apply-rotation ;
60 : rotate-y ( angle -- ) Ry apply-rotation ;
61 : rotate-z ( angle -- ) Rz apply-rotation ;
63 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
65 : pitch-up ( angle -- ) neg rotate-x ;
66 : pitch-down ( angle -- ) rotate-x ;
68 : turn-left ( angle -- ) rotate-y ;
69 : turn-right ( angle -- ) neg rotate-y ;
71 : roll-left ( angle -- ) neg rotate-z ;
72 : roll-right ( angle -- ) rotate-z ;
74 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
76 : step-vector ( length -- array ) { 0 0 1 } n*v ;
78 : step-turtle ( length -- )
79 step-vector orientation> swap m.v position> v+ >position ;
81 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
83 : strafe-up ( length -- )
88 : strafe-down ( length -- )
93 : strafe-left ( length -- )
98 : strafe-right ( length -- )
103 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
107 : init-turtle-stack ( -- ) V{ } clone >turtle-stack ;
109 : push-turtle ( -- ) turtle> clone turtle-stack> push ;
111 ! : pop-turtle ( -- ) turtle-stack> pop >turtle ;
114 turtle-stack> pop dup
115 turtle-position >position
116 turtle-orientation >orientation ;
118 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
119 ! roll-until-horizontal
120 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
122 : V ( -- V ) { 0 1 0 } ;
124 : X ( -- 3array ) orientation> [ first ] map ;
125 : Y ( -- 3array ) orientation> [ second ] map ;
126 : Z ( -- 3array ) orientation> [ third ] map ;
128 : set-X ( seq -- ) orientation> [ 0 swap set-nth ] 2each ;
129 : set-Y ( seq -- ) orientation> [ 1 swap set-nth ] 2each ;
130 : set-Z ( seq -- ) orientation> [ 2 swap set-nth ] 2each ;
132 : roll-until-horizontal ( -- )
133 V Z cross normalize set-X
134 Z X cross normalize set-Y ;