1 USING: kernel math arrays math.vectors math.matrices namespaces make
2 math.constants math.functions splitting grouping math.trig sequences
3 accessors 4DNav.deep models vars ;
10 : with-self ( quot obj -- ) [ >self call ] with-scope ; inline
12 : save-self ( quot -- ) self> [ self> clone >self call ] dip >self ; inline
14 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
16 TUPLE: turtle pos ori ;
18 : <turtle> ( -- turtle )
21 3 identity-matrix >>ori
25 TUPLE: observer < turtle projection-mode collision-mode ;
27 : <observer> ( -- object )
29 0 <model> >>projection-mode
30 f <model> >>collision-mode
34 : turtle-pos> ( -- val ) self> pos>> ;
35 : >turtle-pos ( val -- ) self> pos<< ;
37 : turtle-ori> ( -- val ) self> ori>> ;
38 : >turtle-ori ( val -- ) self> ori<< ;
40 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
42 ! These rotation matrices are from
43 ! `Computer Graphics: Principles and Practice'
46 ! waiting for deep-cleave-quots
48 ! : Rz ( angle -- Rx ) deg>rad
49 ! { { [ cos ] [ sin neg ] 0 }
50 ! { [ sin ] [ cos ] 0 }
52 ! } deep-cleave-quots ;
54 ! : Ry ( angle -- Ry ) deg>rad
55 ! { { [ cos ] 0 [ sin ] }
57 ! { [ sin neg ] 0 [ cos ] }
58 ! } deep-cleave-quots ;
60 ! : Rx ( angle -- Rz ) deg>rad
62 ! { 0 [ cos ] [ sin neg ] }
63 ! { 0 [ sin ] [ cos ] }
64 ! } deep-cleave-quots ;
66 : Rz ( angle -- Rx ) deg>rad
67 [ dup cos , dup sin neg , 0 ,
68 dup sin , dup cos , 0 ,
69 0 , 0 , 1 , ] 3 make-matrix nip ;
71 : Ry ( angle -- Ry ) deg>rad
72 [ dup cos , 0 , dup sin ,
74 dup sin neg , 0 , dup cos , ] 3 make-matrix nip ;
76 : Rx ( angle -- Rz ) deg>rad
78 0 , dup cos , dup sin neg ,
79 0 , dup sin , dup cos , ] 3 make-matrix nip ;
82 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
84 : apply-rotation ( rotation -- )
85 turtle-ori> swap m. >turtle-ori ;
86 : rotate-x ( angle -- ) Rx apply-rotation ;
87 : rotate-y ( angle -- ) Ry apply-rotation ;
88 : rotate-z ( angle -- ) Rz apply-rotation ;
90 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
92 : pitch-up ( angle -- ) neg rotate-x ;
93 : pitch-down ( angle -- ) rotate-x ;
95 : turn-left ( angle -- ) rotate-y ;
96 : turn-right ( angle -- ) neg rotate-y ;
98 : roll-left ( angle -- ) neg rotate-z ;
99 : roll-right ( angle -- ) rotate-z ;
101 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
102 ! roll-until-horizontal
103 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
105 : V ( -- V ) { 0 1 0 } ;
107 : X ( -- 3array ) turtle-ori> [ first ] map ;
108 : Y ( -- 3array ) turtle-ori> [ second ] map ;
109 : Z ( -- 3array ) turtle-ori> [ third ] map ;
111 : set-X ( seq -- ) turtle-ori> [ set-first ] 2each ;
112 : set-Y ( seq -- ) turtle-ori> [ set-second ] 2each ;
113 : set-Z ( seq -- ) turtle-ori> [ set-third ] 2each ;
115 : roll-until-horizontal ( -- )
116 V Z cross normalize set-X
117 Z X cross normalize set-Y ;
119 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
121 : distance ( turtle turtle -- n )
122 pos>> swap pos>> v- [ sq ] map-sum sqrt ;
124 : move-by ( point -- ) turtle-pos> v+ >turtle-pos ;
126 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
128 : reset-turtle ( -- )
129 { 0 0 0 } clone >turtle-pos 3 identity-matrix >turtle-ori ;
131 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
133 : step-vector ( length -- array ) { 0 0 1 } n*v ;
135 : step-turtle ( length -- )
136 step-vector turtle-ori> swap m.v
137 turtle-pos> v+ >turtle-pos ;
139 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
141 : strafe-up ( length -- )
146 : strafe-down ( length -- )
151 : strafe-left ( length -- )
156 : strafe-right ( length -- )