factor: trim using lists

[factor.git] / basis / math / quaternions / quaternions.factor

diff --git a/basis/math/quaternions/quaternions.factor b/basis/math/quaternions/quaternions.factor
old mode 100755 (executable)
new mode 100644 (file)
index bc6da9f..1674211
--- a/basis/math/quaternions/quaternions.factor
+++ b/basis/math/quaternions/quaternions.factor
@@ -1,66 +1,76 @@
-! Copyright (C) 2005, 2007 Slava Pestov.
+! Copyright (C) 2005, 2010 Joe Groff, Slava Pestov.
 ! See http://factorcode.org/license.txt for BSD license.
-USING: arrays kernel math math.functions math.vectors sequences ;
+USING: arrays combinators kernel math
+math.libm math.order math.vectors sequences ;
 IN: math.quaternions
 
-! Everybody's favorite non-commutative skew field, the quaternions!
+: q+ ( u v -- u+v )
+    v+ ; inline
 
-! Quaternions are represented as pairs of complex numbers, using the
-! identity: (a+bi)+(c+di)j = a+bi+cj+dk.
+: q- ( u v -- u-v )
+    v- ; inline
 
 <PRIVATE
 
-: ** ( x y -- z ) conjugate * ; inline
-
-: 2q ( u v -- u' u'' v' v'' ) [ first2 ] bi@ ; inline
-
-: q*a ( u v -- a ) 2q swapd ** [ * ] dip - ; inline
-
-: q*b ( u v -- b ) 2q [ ** swap ] dip * + ; inline
+GENERIC: (q*sign) ( q -- q' )
+M: object (q*sign) { -1 1 1 1 } v* ; inline
 
 PRIVATE>
 
 : q* ( u v -- u*v )
-    [ q*a ] [ q*b ] 2bi 2array ;
+    {
+        [ [ { 1 0 0 0 } vshuffle ] [ { 1 1 2 3 } vshuffle ] bi* v*    ]
+        [ [ { 2 1 2 3 } vshuffle ] [ { 2 0 0 0 } vshuffle ] bi* v* v+ ]
+        [ [ { 3 2 3 1 } vshuffle ] [ { 3 3 1 2 } vshuffle ] bi* v* v+ ]
+        [ [ { 0 3 1 2 } vshuffle ] [ { 0 2 3 1 } vshuffle ] bi* v* v- ]
+    } 2cleave (q*sign) ; inline
 
-: qconjugate ( u -- u' )
-    first2 [ conjugate ] [ neg  ] bi* 2array ;
+GENERIC: qconjugate ( u -- u' )
+M: object qconjugate
+    { 1 -1 -1 -1 } v* ; inline
 
 : qrecip ( u -- 1/u )
-    qconjugate dup norm-sq v/n ;
+    qconjugate dup norm-sq v/n ; inline
 
 : q/ ( u v -- u/v )
-    qrecip q* ;
+    qrecip q* ; inline
 
-: q*n ( q n -- q )
-    conjugate v*n ;
+: n*q ( n q -- r )
+    n*v ; inline
 
-: c>q ( c -- q )
-    0 2array ;
+: q*n ( q n -- r )
+    v*n ; inline
 
-: v>q ( v -- q )
-    first3 rect> [ 0 swap rect> ] dip 2array ;
+: n>q ( n -- q )
+    0 0 0 4array ; inline
 
-: q>v ( q -- v )
-    first2 [ imaginary-part ] dip >rect 3array ;
+: n>q-like ( c exemplar -- q )
+    [ 0 0 0 ] dip 4sequence ; inline
 
-! Zero
-: q0 { 0 0 } ;
+: c>q ( c -- q )
+    >rect 0 0 4array ; inline
 
-! Units
-: q1 { 1 0 } ;
-: qi { C{ 0 1 } 0 } ;
-: qj { 0 1 } ;
-: qk { 0 C{ 0 1 } } ;
+: c>q-like ( c exemplar -- q )
+    [ >rect 0 0 ] dip 4sequence ; inline
 
 ! Euler angles
 
 <PRIVATE
 
-: (euler) ( theta unit -- q )
-    [ -0.5 * [ cos c>q ] [ sin ] bi ] dip n*v v- ;
+: (euler) ( theta exemplar shuffle -- q )
+    swap
+    [ 0.5 * [ fcos ] [ fsin ] bi 0.0 0.0 ] [ call ] [ 4sequence ] tri* ; inline
 
 PRIVATE>
 
+: euler-like ( phi theta psi exemplar -- q )
+    [ [ ] (euler) ] [ [ swapd ] (euler) ] [ [ rot ] (euler) ] tri-curry tri* q* q* ; inline
+
 : euler ( phi theta psi -- q )
-  [ qi (euler) ] [ qj (euler) ] [ qk (euler) ] tri* q* q* ;
+    { } euler-like ; inline
+
+:: slerp ( q0 q1 t -- qt )
+    q0 q1 vdot -1.0 1.0 clamp :> dot
+    dot facos t * :> omega
+    q1 dot q0 n*v v- normalize :> qt'
+    omega fcos q0 n*v omega fsin qt' n*v v+ ; inline