basis/math/quaternions/quaternions.factor

   1 ! Copyright (C) 2005, 2007 Slava Pestov.
   2 ! See http://factorcode.org/license.txt for BSD license.
   3 USING: arrays combinators kernel math math.functions math.libm math.vectors sequences ;
   4 IN: math.quaternions
   5
   6 : q+ ( u v -- u+v )
   7     v+ ; inline
   8
   9 : q- ( u v -- u-v )
  10     v- ; inline
  11
  12 : q* ( u v -- u*v )
  13     {
  14         [ [ { 1 0 0 0 } vshuffle ] [ { 1 1 2 3 } vshuffle ] bi* v*    ]
  15         [ [ { 2 1 2 3 } vshuffle ] [ { 2 0 0 0 } vshuffle ] bi* v* v+ ]
  16         [ [ { 3 2 3 1 } vshuffle ] [ { 3 3 1 2 } vshuffle ] bi* v* v+ ]
  17         [ [ { 0 3 1 2 } vshuffle ] [ { 0 2 3 1 } vshuffle ] bi* v* v- ]
  18     } 2cleave { -1 1 1 1 } v* ; inline
  19
  20 : qconjugate ( u -- u' )
  21     { 1 -1 -1 -1 } v* ; inline
  22
  23 : qrecip ( u -- 1/u )
  24     qconjugate dup norm-sq v/n ; inline
  25
  26 : q/ ( u v -- u/v )
  27     qrecip q* ; inline
  28
  29 : n*q ( q n -- q )
  30     v*n ; inline
  31
  32 : q*n ( q n -- q )
  33     v*n ; inline
  34
  35 : n>q ( n -- q )
  36     0 0 0 4array ; inline
  37
  38 : n>q-like ( c exemplar -- q )
  39     [ 0 0 0 ] dip 4sequence ; inline
  40
  41 : c>q ( c -- q )
  42     >rect 0 0 4array ; inline
  43
  44 : c>q-like ( c exemplar -- q )
  45     [ >rect 0 0 ] dip 4sequence ; inline
  46
  47 ! Euler angles
  48
  49 <PRIVATE
  50
  51 : (euler) ( theta exemplar shuffle -- q )
  52     swap
  53     [ 0.5 * [ fcos ] [ fsin ] bi 0.0 0.0 ] [ call ] [ 4sequence ] tri* ; inline
  54
  55 PRIVATE>
  56
  57 : euler-like ( phi theta psi exemplar -- q )
  58     [ [ ] (euler) ] [ [ swapd ] (euler) ] [ [ rot ] (euler) ] tri-curry tri* q* q* ; inline
  59
  60 : euler ( phi theta psi -- q )
  61     { } euler-like ; inline
  62