From a694e52371820b8a5fc8e700d09c4479351579a5 Mon Sep 17 00:00:00 2001
From: Slava Pestov <slava@factorcode.org>
Date: Wed, 5 Oct 2005 01:33:02 +0000
Subject: [PATCH] quaternions

---
 library/math/complex.factor          |  2 +
 library/math/matrices.factor         |  2 +-
 library/math/quaternions.factor      | 58 ++++++++++++++++++++++++++++
 library/syntax/parse-syntax.factor   |  6 +++
 library/syntax/prettyprint.factor    | 10 ++++-
 library/test/math/quaternions.factor | 20 ++++++++++
 library/test/test.factor             |  4 +-
 7 files changed, 98 insertions(+), 4 deletions(-)
 create mode 100644 library/math/quaternions.factor
 create mode 100644 library/test/math/quaternions.factor

diff --git a/library/math/complex.factor b/library/math/complex.factor
index c6d46dbe07..273a9d465e 100644
--- a/library/math/complex.factor
+++ b/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
diff --git a/library/math/matrices.factor b/library/math/matrices.factor
index 0236cd89ce..4247464223 100644
--- a/library/math/matrices.factor
+++ b/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 ;
 
diff --git a/library/math/quaternions.factor b/library/math/quaternions.factor
new file mode 100644
index 0000000000..182427b548
--- /dev/null
+++ b/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* ;
diff --git a/library/syntax/parse-syntax.factor b/library/syntax/parse-syntax.factor
index 29ef2b568f..e7d73c6f5b 100644
--- a/library/syntax/parse-syntax.factor
+++ b/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
diff --git a/library/syntax/prettyprint.factor b/library/syntax/prettyprint.factor
index 8f15cb9129..ca26b9a097 100644
--- a/library/syntax/prettyprint.factor
+++ b/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 ;
diff --git a/library/test/math/quaternions.factor b/library/test/math/quaternions.factor
new file mode 100644
index 0000000000..c40cf8eea2
--- /dev/null
+++ b/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
diff --git a/library/test/test.factor b/library/test/test.factor
index 3ed4ddbd85..4dfdbb3da4 100644
--- a/library/test/test.factor
+++ b/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/matrices"
+        "math/complex" "math/quaternions" "math/irrational"
+        "math/integer" "math/matrices"
         "httpd/url-encoding" "httpd/html" "httpd/httpd"
         "httpd/http-client" "threads" "parsing-word"
         "inference" "interpreter" "alien"