factor/extra/ori/ori.factor

79 lines
2.2 KiB
Factor

USING: kernel namespaces make accessors
math math.constants math.functions math.matrices math.vectors
sequences splitting grouping self math.trig ;
IN: ori
TUPLE: ori val ;
C: <ori> ori
! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
: ori> ( -- val ) self> val>> ;
: >ori ( val -- ) self> (>>val) ;
! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
: make-matrix ( quot width -- matrix ) >r { } make r> group ; inline
! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
! These rotation matrices are from
! `Computer Graphics: Principles and Practice'
: Rz ( angle -- Rx ) deg>rad
[ dup cos , dup sin neg , 0 ,
dup sin , dup cos , 0 ,
0 , 0 , 1 , ] 3 make-matrix nip ;
: Ry ( angle -- Ry ) deg>rad
[ dup cos , 0 , dup sin ,
0 , 1 , 0 ,
dup sin neg , 0 , dup cos , ] 3 make-matrix nip ;
: Rx ( angle -- Rz ) deg>rad
[ 1 , 0 , 0 ,
0 , dup cos , dup sin neg ,
0 , dup sin , dup cos , ] 3 make-matrix nip ;
! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
: apply-rotation ( rotation -- ) ori> swap m. >ori ;
: rotate-x ( angle -- ) Rx apply-rotation ;
: rotate-y ( angle -- ) Ry apply-rotation ;
: rotate-z ( angle -- ) Rz apply-rotation ;
! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
: pitch-up ( angle -- ) neg rotate-x ;
: pitch-down ( angle -- ) rotate-x ;
: turn-left ( angle -- ) rotate-y ;
: turn-right ( angle -- ) neg rotate-y ;
: roll-left ( angle -- ) neg rotate-z ;
: roll-right ( angle -- ) rotate-z ;
! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
! roll-until-horizontal
! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
: V ( -- V ) { 0 1 0 } ;
: X ( -- 3array ) ori> [ first ] map ;
: Y ( -- 3array ) ori> [ second ] map ;
: Z ( -- 3array ) ori> [ third ] map ;
: set-X ( seq -- ) ori> [ set-first ] 2each ;
: set-Y ( seq -- ) ori> [ set-second ] 2each ;
: set-Z ( seq -- ) ori> [ set-third ] 2each ;
: roll-until-horizontal ( -- )
V Z cross normalize set-X
Z X cross normalize set-Y ;