unmaintained/4DNav/turtle/turtle.factor

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