libs/math/quaternions.factor

   1 ! Copyright (C) 2005 Slava Pestov.
   2 ! See http://factor.sf.net/license.txt for BSD license.
   3
   4 ! Everybody's favorite non-commutative skew field, the
   5 ! quaternions!
   6
   7 ! Quaternions are represented as pairs of complex numbers,
   8 ! using the identity: (a+bi)+(c+di)j = a+bi+cj+dk.
   9 USING: arrays kernel math sequences math-contrib ;
  10 IN: quaternions-internals
  11
  12 : 2q [ first2 ] 2apply ; inline
  13
  14 : q*a 2q swapd ** >r * r> - ; inline
  15
  16 : q*b 2q >r ** swap r> * + ; inline
  17
  18 IN: math-contrib
  19
  20 : q* ( u v -- u*v )
  21     #! Multiply quaternions.
  22     [ q*a ] 2keep q*b 2array ;
  23
  24 : qconjugate ( u -- u' )
  25     #! Quaternion conjugate.
  26     first2 neg >r conjugate r> 2array ;
  27
  28 : qrecip ( u -- 1/u )
  29     #! Quaternion inverse.
  30     qconjugate dup norm-sq v/n ;
  31
  32 : q/ ( u v -- u/v )
  33     #! Divide quaternions.
  34     qrecip q* ;
  35
  36 : q*n ( q n -- q )
  37     #! Note: you will get the wrong result if you try to
  38     #! multiply a quaternion by a complex number on the right
  39     #! using v*n. Use this word instead. Note that v*n with a
  40     #! quaternion and a real is okay.
  41     conjugate v*n ;
  42
  43 : c>q ( c -- q )
  44     #! Turn a complex number into a quaternion.
  45     0 2array ;
  46
  47 : v>q ( v -- q )
  48     #! Turn a 3-vector into a quaternion with real part 0.
  49     first3 rect> >r 0 swap rect> r> 2array ;
  50
  51 : q>v ( q -- v )
  52     #! Get the vector part of a quaternion, discarding the real
  53     #! part.
  54     first2 >r imaginary r> >rect 3array ;
  55
  56 : cross ( u v -- u*v )
  57     #! Cross product of two 3-vectors can be computed using
  58     #! quaternion multiplication.
  59     [ v>q ] 2apply q* q>v ;
  60
  61 ! Zero
  62 : q0 { 0 0 } ;
  63
  64 ! Units
  65 : q1 { 1 0 } ;
  66 : qi { C{ 0 1 } 0 } ;
  67 : qj { 0 1 } ;
  68 : qk { 0 C{ 0 1 } } ;
  69
  70 ! Euler angles -- see
  71 ! http://www.mathworks.com/access/helpdesk/help/toolbox/aeroblks/euleranglestoquaternions.html
  72
  73 : (euler) ( theta unit -- q )
  74     >r -0.5 * dup cos c>q swap sin r> n*v v- ;
  75
  76 : euler ( phi theta psi -- q )
  77     qk (euler) >r qj (euler) >r qi (euler) r> q* r> q* ;
  78