unmaintained: New home for misfit Factor vocabularies.

[factor-unmaintained.git] / 4DNav / turtle / turtle.factor

diff --git a/4DNav/turtle/turtle.factor b/4DNav/turtle/turtle.factor
new file mode 100644 (file)
index 0000000..4f89964
--- /dev/null
+++ b/4DNav/turtle/turtle.factor
@@ -0,0 +1,159 @@
+USING: kernel math arrays math.vectors math.matrices namespaces make
+math.constants math.functions splitting grouping math.trig sequences
+accessors 4DNav.deep models vars ;
+IN: 4DNav.turtle
+
+! replacement of self
+
+VAR: self
+
+: with-self ( quot obj -- ) [ >self call ] with-scope ; inline
+
+: save-self ( quot -- ) self> [ self> clone >self call ] dip >self ; inline
+
+! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+
+TUPLE: turtle pos ori ;
+
+: <turtle> ( -- turtle )
+    turtle new
+    { 0 0 0 } clone >>pos
+    3 identity-matrix >>ori
+;
+
+
+TUPLE: observer < turtle projection-mode collision-mode ;
+
+: <observer> ( -- object ) 
+     observer new
+    0 <model> >>projection-mode 
+    f <model> >>collision-mode
+    ;
+
+
+: turtle-pos> ( -- val ) self> pos>> ;
+: >turtle-pos ( val -- ) self> pos<< ;
+
+: turtle-ori> ( -- val ) self> ori>> ;
+: >turtle-ori ( val -- ) self> ori<< ;
+
+! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+
+! These rotation matrices are from
+! `Computer Graphics: Principles and Practice'
+
+
+! waiting for deep-cleave-quots  
+
+! : Rz ( angle -- Rx ) deg>rad
+!    {   { [ cos ] [ sin neg ]   0 }
+!        { [ sin ] [ cos ]      0  }
+!        {   0       0           1 } 
+!    } deep-cleave-quots  ;
+
+! : Ry ( angle -- Ry ) deg>rad
+!    {   { [ cos ]      0 [ sin ] }
+!        {   0          1 0       }
+!        { [  sin neg ] 0 [ cos ] }
+!    } deep-cleave-quots  ;
+  
+! : Rx ( angle -- Rz ) deg>rad
+!   {   { 1     0        0        }
+!        { 0   [ cos ] [ sin neg ] }
+!        { 0   [ sin ] [ cos ]     }
+!    } deep-cleave-quots ;
+
+: Rz ( angle -- Rx ) deg>rad
+[ dup cos ,     dup sin neg ,   0 ,
+  dup sin ,     dup cos ,       0 ,
+  0 ,           0 ,             1 , ] 3 make-matrix nip ;
+
+: Ry ( angle -- Ry ) deg>rad
+[ dup cos ,     0 ,             dup sin ,
+  0 ,           1 ,             0 ,
+  dup sin neg , 0 ,             dup cos , ] 3 make-matrix nip ;
+
+: Rx ( angle -- Rz ) deg>rad
+[ 1 ,           0 ,             0 ,
+  0 ,           dup cos ,       dup sin neg ,
+  0 ,           dup sin ,       dup cos , ] 3 make-matrix nip ;
+
+
+! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+
+: apply-rotation ( rotation -- ) 
+    turtle-ori> swap m. >turtle-ori ;
+: rotate-x ( angle -- ) Rx apply-rotation ;
+: rotate-y ( angle -- ) Ry apply-rotation ;
+: rotate-z ( angle -- ) Rz apply-rotation ;
+
+! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+
+: pitch-up   ( angle -- ) neg rotate-x ;
+: pitch-down ( angle -- )     rotate-x ;
+
+: turn-left ( angle -- )      rotate-y ;
+: turn-right ( angle -- ) neg rotate-y ;
+
+: roll-left  ( angle -- ) neg rotate-z ;
+: roll-right ( angle -- )     rotate-z ;
+
+! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+! roll-until-horizontal
+! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+
+: V ( -- V ) { 0 1 0 } ;
+
+: X ( -- 3array ) turtle-ori> [ first  ] map ;
+: Y ( -- 3array ) turtle-ori> [ second ] map ;
+: Z ( -- 3array ) turtle-ori> [ third  ] map ;
+
+: set-X ( seq -- ) turtle-ori> [ set-first ] 2each ;
+: set-Y ( seq -- ) turtle-ori> [ set-second ] 2each ;
+: set-Z ( seq -- ) turtle-ori> [ set-third ] 2each ;
+
+: roll-until-horizontal ( -- )
+    V Z cross normalize set-X
+    Z X cross normalize set-Y ;
+
+! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+
+: distance ( turtle turtle -- n ) 
+    pos>> swap pos>> v- [ sq ] map-sum sqrt ;
+
+: move-by ( point -- ) turtle-pos> v+ >turtle-pos ;
+
+! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+
+: reset-turtle ( -- ) 
+    { 0 0 0 } clone >turtle-pos 3 identity-matrix >turtle-ori ;
+
+! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+
+: step-vector ( length -- array ) { 0 0 1 } n*v ;
+
+: step-turtle ( length -- ) 
+    step-vector turtle-ori> swap m.v 
+    turtle-pos> v+ >turtle-pos ;
+
+! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+
+: strafe-up ( length -- )
+    90 pitch-up
+    step-turtle
+    90 pitch-down ;
+
+: strafe-down ( length -- )
+    90 pitch-down
+    step-turtle
+    90 pitch-up ;
+
+: strafe-left ( length -- )
+    90 turn-left
+    step-turtle
+    90 turn-right ;
+
+: strafe-right ( length -- )
+    90 turn-right
+    step-turtle
+    90 turn-left ;