;;; Chickadee Game Toolkit
;;; Copyright © 2017 David Thompson <dthompson2@worcester.edu>
;;;
;;; Licensed under the Apache License, Version 2.0 (the "License");
;;; you may not use this file except in compliance with the License.
;;; You may obtain a copy of the License at
;;;
;;;    http://www.apache.org/licenses/LICENSE-2.0
;;;
;;; Unless required by applicable law or agreed to in writing, software
;;; distributed under the License is distributed on an "AS IS" BASIS,
;;; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
;;; See the License for the specific language governing permissions and
;;; limitations under the License.

;;; Commentary:
;;
;; Useful representation of 3D rotations.
;;
;;; Code:

(define-module (chickadee math quaternion)
  #:use-module (chickadee data bytestruct)
  #: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)
  #:use-module (system foreign)
  #:export (quaternion
            quaternion?
            quaternion-x
            quaternion-y
            quaternion-z
            quaternion-w
            make-identity-quaternion
            rotation->quaternion))

;; Should this even be its own type?  Should probably just be a
;; generic 4D vector.
(define-byterecord-type <quaternion>
  (quaternion x y z w)
  quaternion?
  (x f32 quaternion-x set-quaternion-x!)
  (y f32 quaternion-y set-quaternion-y!)
  (z f32 quaternion-z set-quaternion-z!)
  (w f32 quaternion-w set-quaternion-w!))

(define-syntax-rule (with-new-quaternion name body ...)
  (let ((name (quaternion 0.0 0.0 0.0 0.0)))
    body ... name))

(define-inlinable (make-identity-quaternion)
  "Return the identity quaternion."
  (quaternion 0.0 0.0 0.0 1.0))

(define-inlinable (make-null-quaternion)
  (quaternion 0.0 0.0 0.0 0.0))

(define-inlinable (quaternion-magnitude q)
  "Return the magnitude of the quaternion Q."
  (let ((w (quaternion-w q))
        (x (quaternion-x q))
        (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))))