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 kernel math math.functions math.vectors sequences ;
   4 IN: math.quaternions
   5
   6 ! Everybody's favorite non-commutative skew field, the quaternions!
   7
   8 ! Quaternions are represented as pairs of complex numbers, using the
   9 ! identity: (a+bi)+(c+di)j = a+bi+cj+dk.
  10
  11 <PRIVATE
  12
  13 : ** ( x y -- z ) conjugate * ; inline
  14
  15 : 2q ( u v -- u' u'' v' v'' ) [ first2 ] bi@ ; inline
  16
  17 : q*a ( u v -- a ) 2q swapd ** [ * ] dip - ; inline
  18
  19 : q*b ( u v -- b ) 2q [ ** swap ] dip * + ; inline
  20
  21 PRIVATE>
  22
  23 : q+ ( u v -- u+v )
  24     v+ ;
  25
  26 : q- ( u v -- u-v )
  27     v- ;
  28
  29 : q* ( u v -- u*v )
  30     [ q*a ] [ q*b ] 2bi 2array ;
  31
  32 : qconjugate ( u -- u' )
  33     first2 [ conjugate ] [ neg  ] bi* 2array ;
  34
  35 : qrecip ( u -- 1/u )
  36     qconjugate dup norm-sq v/n ;
  37
  38 : q/ ( u v -- u/v )
  39     qrecip q* ;
  40
  41 : q*n ( q n -- q )
  42     conjugate v*n ;
  43
  44 : c>q ( c -- q )
  45     0 2array ;
  46
  47 : v>q ( v -- q )
  48     first3 rect> [ 0 swap rect> ] dip 2array ;
  49
  50 : q>v ( q -- v )
  51     first2 [ imaginary-part ] dip >rect 3array ;
  52
  53 ! Zero
  54 CONSTANT: q0 { 0 0 }
  55
  56 ! Units
  57 CONSTANT: q1 { 1 0 }
  58 CONSTANT: qi { C{ 0 1 } 0 }
  59 CONSTANT: qj { 0 1 }
  60 CONSTANT: qk { 0 C{ 0 1 } }
  61
  62 ! Euler angles
  63
  64 <PRIVATE
  65
  66 : (euler) ( theta unit -- q )
  67     [ -0.5 * [ cos c>q ] [ sin ] bi ] dip n*v v- ;
  68
  69 PRIVATE>
  70
  71 : euler ( phi theta psi -- q )
  72   [ qi (euler) ] [ qj (euler) ] [ qk (euler) ] tri* q* q* ;