1 ! Copyright (C) 2005, 2010 Joe Groff, Slava Pestov.
2 ! See http://factorcode.org/license.txt for BSD license.
3 USING: arrays combinators kernel locals math math.functions
4 math.libm math.order math.vectors sequences ;
15 GENERIC: (q*sign) ( q -- q' )
16 M: object (q*sign) { -1 1 1 1 } v* ; inline
22 [ [ { 1 0 0 0 } vshuffle ] [ { 1 1 2 3 } vshuffle ] bi* v* ]
23 [ [ { 2 1 2 3 } vshuffle ] [ { 2 0 0 0 } vshuffle ] bi* v* v+ ]
24 [ [ { 3 2 3 1 } vshuffle ] [ { 3 3 1 2 } vshuffle ] bi* v* v+ ]
25 [ [ { 0 3 1 2 } vshuffle ] [ { 0 2 3 1 } vshuffle ] bi* v* v- ]
26 } 2cleave (q*sign) ; inline
28 GENERIC: qconjugate ( u -- u' )
29 M: object qconjugate ( u -- u' )
30 { 1 -1 -1 -1 } v* ; inline
33 qconjugate dup norm-sq v/n ; inline
47 : n>q-like ( c exemplar -- q )
48 [ 0 0 0 ] dip 4sequence ; inline
51 >rect 0 0 4array ; inline
53 : c>q-like ( c exemplar -- q )
54 [ >rect 0 0 ] dip 4sequence ; inline
60 : (euler) ( theta exemplar shuffle -- q )
62 [ 0.5 * [ fcos ] [ fsin ] bi 0.0 0.0 ] [ call ] [ 4sequence ] tri* ; inline
66 : euler-like ( phi theta psi exemplar -- q )
67 [ [ ] (euler) ] [ [ swapd ] (euler) ] [ [ rot ] (euler) ] tri-curry tri* q* q* ; inline
69 : euler ( phi theta psi -- q )
70 { } euler-like ; inline
72 :: slerp ( q0 q1 t -- qt )
73 q0 q1 v. -1.0 1.0 clamp :> dot
74 dot facos t * :> omega
75 q1 dot q0 n*v v- normalize :> qt'
76 omega fcos q0 n*v omega fsin qt' n*v v+ ; inline