unmaintained/4DNav/turtle/turtle.factor

   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 ;
   4 IN: 4DNav.turtle
   5
   6 ! replacement of self
   7
   8 VAR: self
   9
  10 : with-self ( quot obj -- ) [ >self call ] with-scope ; inline
  11
  12 : save-self ( quot -- ) self> [ self> clone >self call ] dip >self ; inline
  13
  14 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  15
  16 TUPLE: turtle pos ori ;
  17
  18 : <turtle> ( -- turtle )
  19     turtle new
  20     { 0 0 0 } clone >>pos
  21     3 identity-matrix >>ori
  22 ;
  23
  24
  25 TUPLE: observer < turtle projection-mode collision-mode ;
  26
  27 : <observer> ( -- object )
  28      observer new
  29     0 <model> >>projection-mode
  30     f <model> >>collision-mode
  31     ;
  32
  33
  34 : turtle-pos> ( -- val ) self> pos>> ;
  35 : >turtle-pos ( val -- ) self> pos<< ;
  36
  37 : turtle-ori> ( -- val ) self> ori>> ;
  38 : >turtle-ori ( val -- ) self> ori<< ;
  39
  40 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  41
  42 ! These rotation matrices are from
  43 ! `Computer Graphics: Principles and Practice'
  44
  45
  46 ! waiting for deep-cleave-quots
  47
  48 ! : Rz ( angle -- Rx ) deg>rad
  49 !    {   { [ cos ] [ sin neg ]   0 }
  50 !        { [ sin ] [ cos ]      0  }
  51 !        {   0       0           1 }
  52 !    } deep-cleave-quots  ;
  53
  54 ! : Ry ( angle -- Ry ) deg>rad
  55 !    {   { [ cos ]      0 [ sin ] }
  56 !        {   0          1 0       }
  57 !        { [  sin neg ] 0 [ cos ] }
  58 !    } deep-cleave-quots  ;
  59
  60 ! : Rx ( angle -- Rz ) deg>rad
  61 !   {   { 1     0        0        }
  62 !        { 0   [ cos ] [ sin neg ] }
  63 !        { 0   [ sin ] [ cos ]     }
  64 !    } deep-cleave-quots ;
  65
  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 ;
  70
  71 : Ry ( angle -- Ry ) deg>rad
  72 [ dup cos ,     0 ,             dup sin ,
  73   0 ,           1 ,             0 ,
  74   dup sin neg , 0 ,             dup cos , ] 3 make-matrix nip ;
  75
  76 : Rx ( angle -- Rz ) deg>rad
  77 [ 1 ,           0 ,             0 ,
  78   0 ,           dup cos ,       dup sin neg ,
  79   0 ,           dup sin ,       dup cos , ] 3 make-matrix nip ;
  80
  81
  82 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  83
  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 ;
  89
  90 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  91
  92 : pitch-up   ( angle -- ) neg rotate-x ;
  93 : pitch-down ( angle -- )     rotate-x ;
  94
  95 : turn-left ( angle -- )      rotate-y ;
  96 : turn-right ( angle -- ) neg rotate-y ;
  97
  98 : roll-left  ( angle -- ) neg rotate-z ;
  99 : roll-right ( angle -- )     rotate-z ;
 100
 101 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 102 ! roll-until-horizontal
 103 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 104
 105 : V ( -- V ) { 0 1 0 } ;
 106
 107 : X ( -- 3array ) turtle-ori> [ first  ] map ;
 108 : Y ( -- 3array ) turtle-ori> [ second ] map ;
 109 : Z ( -- 3array ) turtle-ori> [ third  ] map ;
 110
 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 ;
 114
 115 : roll-until-horizontal ( -- )
 116     V Z cross normalize set-X
 117     Z X cross normalize set-Y ;
 118
 119 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 120
 121 : distance ( turtle turtle -- n )
 122     pos>> swap pos>> v- [ sq ] map-sum sqrt ;
 123
 124 : move-by ( point -- ) turtle-pos> v+ >turtle-pos ;
 125
 126 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 127
 128 : reset-turtle ( -- )
 129     { 0 0 0 } clone >turtle-pos 3 identity-matrix >turtle-ori ;
 130
 131 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 132
 133 : step-vector ( length -- array ) { 0 0 1 } n*v ;
 134
 135 : step-turtle ( length -- )
 136     step-vector turtle-ori> swap m.v
 137     turtle-pos> v+ >turtle-pos ;
 138
 139 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 140
 141 : strafe-up ( length -- )
 142     90 pitch-up
 143     step-turtle
 144     90 pitch-down ;
 145
 146 : strafe-down ( length -- )
 147     90 pitch-down
 148     step-turtle
 149     90 pitch-up ;
 150
 151 : strafe-left ( length -- )
 152     90 turn-left
 153     step-turtle
 154     90 turn-right ;
 155
 156 : strafe-right ( length -- )
 157     90 turn-right
 158     step-turtle
 159     90 turn-left ;