math: quaternion: Add rotation->quaternion procedure.

1 files changed, 17 insertions, 1 deletions

diff --git a/chickadee/math/quaternion.scm b/chickadee/math/quaternion.scm
index 1a5ce68..a55894a 100644
--- a/chickadee/math/quaternion.scm
+++ b/chickadee/math/quaternion.scm
@@ -23,6 +23,7 @@
 
 (define-module (chickadee math quaternion)
   #:use-module (chickadee math)
+  #:use-module (chickadee math vector)
   #:use-module (ice-9 format)
   #:use-module (srfi srfi-9)
   #:use-module (srfi srfi-9 gnu)
@@ -33,7 +34,8 @@
             quaternion-x
             quaternion-y
             quaternion-z
-            make-identity-quaternion))
+            make-identity-quaternion
+            rotation->quaternion))
 
 (define-record-type <quaternion>
   (wrap-quaternion bv pointer)
@@ -106,3 +108,17 @@
         (y (quaternion-y q))
         (z (quaternion-z q)))
     (sqrt (+ (* w w) (* x x) (* y y) (* z z)))))
+
+(define-inlinable (rotation->quaternion axis angle)
+  ;; Math taken from here:
+  ;; http://www.opengl-tutorial.org/intermediate-tutorials/tutorial-17-quaternions/
+  (let* ((a (/ angle 2.0))
+         (s (sin a))
+         (c (cos a))
+         (n (vec3-magnitude axis)))
+    (if (= n 0.0)
+        (make-identity-quaternion)
+        (quaternion (* (/ (vec3-x axis) n) s)
+                    (* (/ (vec3-y axis) n) s)
+                    (* (/ (vec3-z axis) n) s)
+                    c))))