quaternions

library/math/complex.factor

@@ -28,6 +28,8 @@ M: number = ( n n -- ? ) number= ;
 
 : conjugate ( z -- z* ) >rect neg rect> ; inline
 
+: ** ( u v -- u*v' ) conjugate * ; inline
+
 : arg ( z -- arg )
     #! Compute the complex argument.
     >rect swap fatan2 ; inline

library/math/matrices.factor

@@ -33,7 +33,7 @@ USING: arrays generic kernel sequences ;
     2dup v* >r >r drop dup r> v* v- r> v+ ;
 
 : v. ( v v -- x ) 0 [ * + ] 2reduce ;
-: c. ( v v -- x ) 0 [ conjugate * + ] 2reduce ;
+: c. ( v v -- x ) 0 [ ** + ] 2reduce ;
 
 : norm-sq ( v -- n ) 0 [ absq + ] reduce ;
 

library/math/quaternions.factor

@@ -0,0 +1,58 @@
+! Copyright (C) 2005 Slava Pestov.
+! See http://factor.sf.net/license.txt for BSD license.
+
+! Everybody's favorite non-commutative skew field, the
+! quaternions! Represented as pairs of complex numbers,
+! that is, (a+bi)+(c+di)j.
+USING: arrays kernel math sequences ;
+IN: math-internals
+
+: 2q [ first2 ] 2apply ;
+
+: q*a 2q swapd ** >r * r> - ;
+
+: q*b 2q >r ** swap r> * + ;
+
+IN: math
+
+: quaternion? ( seq -- ? )
+    dup length 2 = [
+        first2 [ number? ] 2apply and
+    ] [
+        2drop f
+    ] if ;
+
+: q* ( u v -- u*v ) [ q*a ] 2keep q*b 2array ;
+
+: qconjugate ( u -- u' ) first2 neg >r conjugate r> 2array ;
+
+: q/ ( u v -- u/v ) [ qconjugate q* ] keep norm-sq v/n ;
+
+: q*n ( q n -- q )
+    #! Note: you will get the wrong result if you try to
+    #! multiply a quaternion by a complex number on the right
+    #! using v*n. Use this word instead. Note that v*n with a
+    #! quaternion and a real is okay.
+    conjugate v*n ;
+
+: c>q ( c -- q )
+    #! Turn a complex number into a quaternion.
+    0 2array ;
+
+! Zero
+: q0 Q{ 0 0 0 0 }Q ;
+
+! Units
+: q1 Q{ 1 0 0 0 }Q ;
+: qi Q{ 0 1 0 0 }Q ;
+: qj Q{ 0 0 1 0 }Q ;
+: qk Q{ 0 0 0 1 }Q ;
+
+! Euler angles -- see
+! http://www.mathworks.com/access/helpdesk/help/toolbox/aeroblks/euleranglestoquaternions.html
+
+: (euler) ( theta unit -- q )
+    >r -0.5 * dup cos c>q swap sin r> n*q v- ;
+
+: euler ( phi theta psi -- q )
+    qk (euler) >r qj (euler) >r qi (euler) r> q* r> q* ;

library/syntax/parse-syntax.factor

@@ -153,3 +153,9 @@ SYMBOL: t
 : DEC: 10 (BASE) ; parsing
 : OCT: 8 (BASE) ; parsing
 : BIN: 2 (BASE) ; parsing
+
+: Q{ f ; parsing
+
+: }Q
+    reverse 2 swap cut
+    [ first2 rect> ] 2apply 2array swons ; parsing

library/syntax/prettyprint.factor

@@ -278,7 +278,15 @@ M: cons pprint* ( list -- )
    ] check-recursion ;
 
 M: array pprint* ( vector -- )
-    [ \ @{ \ }@ pprint-sequence ] check-recursion ;
+    #! This does a hack for printing quaternions.
+    [
+        dup quaternion? [
+            [ >rect 2array ] map concat
+            \ Q{ \ }Q
+        ] [
+            \ @{ \ }@
+        ] if pprint-sequence
+    ] check-recursion ;
 
 M: vector pprint* ( vector -- )
     [ \ { \ } pprint-sequence ] check-recursion ;

library/test/math/quaternions.factor

@@ -0,0 +1,20 @@
+IN: temporary
+USING: kernel math test ;
+
+[ 1 ] [ qi norm ] unit-test
+[ 1 ] [ qj norm ] unit-test
+[ 1 ] [ qk norm ] unit-test
+[ 1 ] [ q1 norm ] unit-test
+[ 0 ] [ q0 norm ] unit-test
+[ t ] [ qi qj q* qk = ] unit-test
+[ t ] [ qj qk q* qi = ] unit-test
+[ t ] [ qk qi q* qj = ] unit-test
+[ t ] [ qi qi q* q1 v+ q0 = ] unit-test
+[ t ] [ qj qj q* q1 v+ q0 = ] unit-test
+[ t ] [ qk qk q* q1 v+ q0 = ] unit-test
+[ t ] [ qi qj qk q* q* q1 v+ q0 = ] unit-test
+[ t ] [ i qj n*v qk = ] unit-test
+[ t ] [ qj i q*n qk v+ q0 = ] unit-test
+[ t ] [ qk qj q/ qi = ] unit-test
+[ t ] [ qi qk q/ qj = ] unit-test
+[ t ] [ qj qi q/ qk = ] unit-test

library/test/test.factor

@@ -87,8 +87,8 @@ SYMBOL: failures
         "words" "prettyprint" "random"
         "stream" "math/bitops"
         "math/math-combinators" "math/rational" "math/float"
-        "math/complex" "math/irrational" "math/integer"
+        "math/complex" "math/quaternions" "math/irrational"
-        "math/matrices"
+        "math/integer" "math/matrices"
         "httpd/url-encoding" "httpd/html" "httpd/httpd"
         "httpd/http-client" "threads" "parsing-word"
         "inference" "interpreter" "alien"