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
index 0c94f4d1195e27c504de0189a9f28ada150c32ed..1674211aac28a8004595d9779af4375611ff971f 100644 (file)
--- a/basis/math/quaternions/quaternions.factor
+++ b/basis/math/quaternions/quaternions.factor
@@ -1,6 +1,7 @@
-! 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 combinators kernel math math.functions math.libm math.vectors sequences ;
+USING: arrays combinators kernel math
+math.libm math.order math.vectors sequences ;
 IN: math.quaternions
 
 : q+ ( u v -- u+v )
@@ -9,15 +10,23 @@ IN: math.quaternions
 : q- ( u v -- u-v )
     v- ; inline
 
+<PRIVATE
+
+GENERIC: (q*sign) ( q -- q' )
+M: object (q*sign) { -1 1 1 1 } v* ; inline
+
+PRIVATE>
+
 : q* ( u v -- u*v )
     {
         [ [ { 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 { -1 1 1 1 } v* ; inline
+    } 2cleave (q*sign) ; inline
 
-: qconjugate ( u -- u' )
+GENERIC: qconjugate ( u -- u' )
+M: object qconjugate
     { 1 -1 -1 -1 } v* ; inline
 
 : qrecip ( u -- 1/u )
@@ -26,10 +35,10 @@ IN: math.quaternions
 : q/ ( u v -- u/v )
     qrecip q* ; inline
 
-: n*q ( q n -- q )
-    v*n ; inline
+: n*q ( n q -- r )
+    n*v ; inline
 
-: q*n ( q n -- q )
+: q*n ( q n -- r )
     v*n ; inline
 
 : n>q ( n -- q )
@@ -60,3 +69,8 @@ PRIVATE>
 : euler ( phi theta psi -- 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