1 USING: kernel math arrays math.vectors math.matrices
3 math.constants math.functions
5 splitting grouping self math.trig
6 sequences accessors 4DNav.deep models ;
9 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
11 TUPLE: turtle pos ori ;
13 : <turtle> ( -- turtle )
16 3 identity-matrix >>ori
20 TUPLE: observer < turtle projection-mode collision-mode ;
22 : <observer> ( -- object )
24 0 <model> >>projection-mode
25 f <model> >>collision-mode
29 : turtle-pos> ( -- val ) self> pos>> ;
30 : >turtle-pos ( val -- ) self> (>>pos) ;
32 : turtle-ori> ( -- val ) self> ori>> ;
33 : >turtle-ori ( val -- ) self> (>>ori) ;
35 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
37 ! These rotation matrices are from
38 ! `Computer Graphics: Principles and Practice'
41 ! waiting for deep-cleave-quots
43 ! : Rz ( angle -- Rx ) deg>rad
44 ! { { [ cos ] [ sin neg ] 0 }
45 ! { [ sin ] [ cos ] 0 }
47 ! } deep-cleave-quots ;
49 ! : Ry ( angle -- Ry ) deg>rad
50 ! { { [ cos ] 0 [ sin ] }
52 ! { [ sin neg ] 0 [ cos ] }
53 ! } deep-cleave-quots ;
55 ! : Rx ( angle -- Rz ) deg>rad
57 ! { 0 [ cos ] [ sin neg ] }
58 ! { 0 [ sin ] [ cos ] }
59 ! } deep-cleave-quots ;
61 : Rz ( angle -- Rx ) deg>rad
62 [ dup cos , dup sin neg , 0 ,
63 dup sin , dup cos , 0 ,
64 0 , 0 , 1 , ] 3 make-matrix nip ;
66 : Ry ( angle -- Ry ) deg>rad
67 [ dup cos , 0 , dup sin ,
69 dup sin neg , 0 , dup cos , ] 3 make-matrix nip ;
71 : Rx ( angle -- Rz ) deg>rad
73 0 , dup cos , dup sin neg ,
74 0 , dup sin , dup cos , ] 3 make-matrix nip ;
77 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
79 : apply-rotation ( rotation -- ) turtle-ori> swap m. >turtle-ori ;
81 : rotate-x ( angle -- ) Rx apply-rotation ;
82 : rotate-y ( angle -- ) Ry apply-rotation ;
83 : rotate-z ( angle -- ) Rz apply-rotation ;
85 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
87 : pitch-up ( angle -- ) neg rotate-x ;
88 : pitch-down ( angle -- ) rotate-x ;
90 : turn-left ( angle -- ) rotate-y ;
91 : turn-right ( angle -- ) neg rotate-y ;
93 : roll-left ( angle -- ) neg rotate-z ;
94 : roll-right ( angle -- ) rotate-z ;
96 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
97 ! roll-until-horizontal
98 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
100 : V ( -- V ) { 0 1 0 } ;
102 : X ( -- 3array ) turtle-ori> [ first ] map ;
103 : Y ( -- 3array ) turtle-ori> [ second ] map ;
104 : Z ( -- 3array ) turtle-ori> [ third ] map ;
106 : set-X ( seq -- ) turtle-ori> [ set-first ] 2each ;
107 : set-Y ( seq -- ) turtle-ori> [ set-second ] 2each ;
108 : set-Z ( seq -- ) turtle-ori> [ set-third ] 2each ;
110 : roll-until-horizontal ( -- )
111 V Z cross normalize set-X
112 Z X cross normalize set-Y ;
114 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
116 : distance ( turtle turtle -- n ) pos>> swap pos>> v- [ sq ] map sum sqrt ;
118 : move-by ( point -- ) turtle-pos> v+ >turtle-pos ;
120 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
122 : reset-turtle ( -- )
123 { 0 0 0 } clone >turtle-pos 3 identity-matrix >turtle-ori ;
125 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
127 : step-vector ( length -- array ) { 0 0 1 } n*v ;
129 : step-turtle ( length -- )
130 step-vector turtle-ori> swap m.v turtle-pos> v+ >turtle-pos ;
132 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
134 : strafe-up ( length -- )
139 : strafe-down ( length -- )
144 : strafe-left ( length -- )
149 : strafe-right ( length -- )