extra/ori/ori.factor

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