contrib/lindenmayer/turtle.factor

   1 REQUIRES: contrib/math contrib/vars ;
   2 USING: kernel math namespaces sequences arrays math-contrib vars ;
   3 IN: turtle
   4
   5 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
   6
   7 TUPLE: turtle position orientation ;
   8
   9 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  10
  11 VAR: turtle
  12
  13 : position> ( -- position ) turtle> turtle-position ;
  14
  15 : >position ( position -- ) turtle> set-turtle-position ;
  16
  17 : orientation> ( -- orientation ) turtle> turtle-orientation ;
  18
  19 : >orientation ( orientation -- ) turtle> set-turtle-orientation ;
  20
  21 : with-turtle ( quot turtle -- ) [ >turtle call ] with-scope ;
  22
  23 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  24
  25 : reset-turtle ( -- ) { 0 0 0 } >position 3 identity-matrix >orientation ;
  26
  27 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  28
  29 C: turtle ( -- ) [ reset-turtle ] over with-turtle ;
  30
  31 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  32
  33 : make-matrix >r { } make r> group ;
  34
  35 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  36
  37 ! These rotation matrices are from
  38 ! `Computer Graphics: Principles and Practice'
  39
  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 ;
  44
  45 : Ry ( angle -- Ry ) deg>rad
  46 [ dup cos ,     0 ,             dup sin ,
  47   0 ,           1 ,             0 ,
  48   dup sin neg , 0 ,             dup cos , ] 3 make-matrix nip ;
  49
  50 : Rx ( angle -- Rz ) deg>rad
  51 [ 1 ,           0 ,             0 ,
  52   0 ,           dup cos ,       dup sin neg ,
  53   0 ,           dup sin ,       dup cos , ] 3 make-matrix nip ;
  54
  55 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  56
  57 : apply-rotation ( rotation -- ) orientation> swap m. >orientation ;
  58
  59 : rotate-x ( angle -- ) Rx apply-rotation ;
  60 : rotate-y ( angle -- ) Ry apply-rotation ;
  61 : rotate-z ( angle -- ) Rz apply-rotation ;
  62
  63 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  64
  65 : pitch-up   ( angle -- ) neg rotate-x ;
  66 : pitch-down ( angle -- )     rotate-x ;
  67
  68 : turn-left ( angle -- )      rotate-y ;
  69 : turn-right ( angle -- ) neg rotate-y ;
  70
  71 : roll-left  ( angle -- ) neg rotate-z ;
  72 : roll-right ( angle -- )     rotate-z ;
  73
  74 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  75
  76 : step-vector ( length -- array ) { 0 0 1 } n*v ;
  77
  78 : step-turtle ( length -- )
  79 step-vector orientation> swap m.v position> v+ >position ;
  80
  81 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  82
  83 : strafe-up ( length -- )
  84 90 pitch-up
  85 step-turtle
  86 90 pitch-down ;
  87
  88 : strafe-down ( length -- )
  89 90 pitch-down
  90 step-turtle
  91 90 pitch-up ;
  92
  93 : strafe-left ( length -- )
  94 90 turn-left
  95 step-turtle
  96 90 turn-right ;
  97
  98 : strafe-right ( length -- )
  99 90 turn-right
 100 step-turtle
 101 90 turn-left ;
 102
 103 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 104
 105 VAR: turtle-stack
 106
 107 : init-turtle-stack ( -- ) V{ } clone >turtle-stack ;
 108
 109 : push-turtle ( -- ) turtle> clone turtle-stack> push ;
 110
 111 ! : pop-turtle ( -- ) turtle-stack> pop >turtle ;
 112
 113 : pop-turtle ( -- )
 114 turtle-stack> pop dup
 115 turtle-position >position
 116 turtle-orientation >orientation ;
 117
 118 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 119 ! roll-until-horizontal
 120 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 121
 122 : V ( -- V ) { 0 1 0 } ;
 123
 124 : X ( -- 3array ) orientation> [ first  ] map ;
 125 : Y ( -- 3array ) orientation> [ second ] map ;
 126 : Z ( -- 3array ) orientation> [ third  ] map ;
 127
 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 ;
 131
 132 : roll-until-horizontal ( -- )
 133 V Z cross normalize set-X
 134 Z X cross normalize set-Y ;
 135