basis/math/quaternions/quaternions.factor

   1 ! Copyright (C) 2005, 2010 Joe Groff, Slava Pestov.
   2 ! See http://factorcode.org/license.txt for BSD license.
   3 USING: arrays combinators kernel math
   4 math.libm math.order math.vectors sequences ;
   5 IN: math.quaternions
   6
   7 : q+ ( u v -- u+v )
   8     v+ ; inline
   9
  10 : q- ( u v -- u-v )
  11     v- ; inline
  12
  13 <PRIVATE
  14
  15 GENERIC: (q*sign) ( q -- q' )
  16 M: object (q*sign) { -1 1 1 1 } v* ; inline
  17
  18 PRIVATE>
  19
  20 : q* ( u v -- u*v )
  21     {
  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
  27
  28 GENERIC: qconjugate ( u -- u' )
  29 M: object qconjugate
  30     { 1 -1 -1 -1 } v* ; inline
  31
  32 : qrecip ( u -- 1/u )
  33     qconjugate dup norm-sq v/n ; inline
  34
  35 : q/ ( u v -- u/v )
  36     qrecip q* ; inline
  37
  38 : n*q ( n q -- r )
  39     n*v ; inline
  40
  41 : q*n ( q n -- r )
  42     v*n ; inline
  43
  44 : n>q ( n -- q )
  45     0 0 0 4array ; inline
  46
  47 : n>q-like ( c exemplar -- q )
  48     [ 0 0 0 ] dip 4sequence ; inline
  49
  50 : c>q ( c -- q )
  51     >rect 0 0 4array ; inline
  52
  53 : c>q-like ( c exemplar -- q )
  54     [ >rect 0 0 ] dip 4sequence ; inline
  55
  56 ! Euler angles
  57
  58 <PRIVATE
  59
  60 : (euler) ( theta exemplar shuffle -- q )
  61     swap
  62     [ 0.5 * [ fcos ] [ fsin ] bi 0.0 0.0 ] [ call ] [ 4sequence ] tri* ; inline
  63
  64 PRIVATE>
  65
  66 : euler-like ( phi theta psi exemplar -- q )
  67     [ [ ] (euler) ] [ [ swapd ] (euler) ] [ [ rot ] (euler) ] tri-curry tri* q* q* ; inline
  68
  69 : euler ( phi theta psi -- q )
  70     { } euler-like ; inline
  71
  72 :: slerp ( q0 q1 t -- qt )
  73     q0 q1 vdot -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