From e8926b3aa2c42cd3a6e0e10fea395b2b7f6ac0b7 Mon Sep 17 00:00:00 2001
From: David Thompson <dthompson2@worcester.edu>
Date: Mon, 10 May 2021 17:11:13 -0400
Subject: math: quaternion: Add rotation->quaternion procedure.

---
 chickadee/math/quaternion.scm | 18 +++++++++++++++++-
 1 file changed, 17 insertions(+), 1 deletion(-)

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))))
-- 
cgit v1.2.3