extra/ori/ori.factor

   1
   2 USING: kernel namespaces
   3        math math.constants math.functions math.matrices math.vectors
   4        sequences splitting self ;
   5
   6 IN: ori
   7
   8 TUPLE: ori val ;
   9
  10 C: <ori> ori
  11
  12 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  13
  14 ! Temporarily defined here until math-contrib gets moved to extra/
  15
  16 : deg>rad pi * 180 / ; inline
  17 : rad>deg 180 * pi / ; inline
  18
  19 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  20
  21 : ori> ( -- val ) self> ori-val ;
  22
  23 : >ori ( val -- ) self> set-ori-val ;
  24
  25 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  26
  27 : make-matrix ( quot width -- matrix ) >r { } make r> group ;
  28
  29 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  30
  31 ! These rotation matrices are from
  32 ! `Computer Graphics: Principles and Practice'
  33
  34 : Rz ( angle -- Rx ) deg>rad
  35 [ dup cos ,     dup sin neg ,   0 ,
  36   dup sin ,     dup cos ,       0 ,
  37   0 ,           0 ,             1 , ] 3 make-matrix nip ;
  38
  39 : Ry ( angle -- Ry ) deg>rad
  40 [ dup cos ,     0 ,             dup sin ,
  41   0 ,           1 ,             0 ,
  42   dup sin neg , 0 ,             dup cos , ] 3 make-matrix nip ;
  43
  44 : Rx ( angle -- Rz ) deg>rad
  45 [ 1 ,           0 ,             0 ,
  46   0 ,           dup cos ,       dup sin neg ,
  47   0 ,           dup sin ,       dup cos , ] 3 make-matrix nip ;
  48
  49 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  50
  51 : apply-rotation ( rotation -- ) ori> swap m. >ori ;
  52
  53 : rotate-x ( angle -- ) Rx apply-rotation ;
  54 : rotate-y ( angle -- ) Ry apply-rotation ;
  55 : rotate-z ( angle -- ) Rz apply-rotation ;
  56
  57 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  58
  59 : pitch-up   ( angle -- ) neg rotate-x ;
  60 : pitch-down ( angle -- )     rotate-x ;
  61
  62 : turn-left ( angle -- )      rotate-y ;
  63 : turn-right ( angle -- ) neg rotate-y ;
  64
  65 : roll-left  ( angle -- ) neg rotate-z ;
  66 : roll-right ( angle -- )     rotate-z ;
  67
  68 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  69 ! roll-until-horizontal
  70 ! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  71
  72 : V ( -- V ) { 0 1 0 } ;
  73
  74 : X ( -- 3array ) ori> [ first  ] map ;
  75 : Y ( -- 3array ) ori> [ second ] map ;
  76 : Z ( -- 3array ) ori> [ third  ] map ;
  77
  78 : set-X ( seq -- ) ori> [ set-first ] 2each ;
  79 : set-Y ( seq -- ) ori> [ set-second ] 2each ;
  80 : set-Z ( seq -- ) ori> [ set-third ] 2each ;
  81
  82 : roll-until-horizontal ( -- )
  83 V Z cross normalize set-X
  84 Z X cross normalize set-Y ;
  85