summaryrefslogtreecommitdiff
path: root/chickadee/math/quaternion.scm
blob: 893ccd7b2bd0e0cc715d7b55b557df19feccc458 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
;;; Chickadee Game Toolkit
;;; Copyright © 2017 David Thompson <davet@gnu.org>
;;;
;;; Chickadee is free software: you can redistribute it and/or modify
;;; it under the terms of the GNU General Public License as published
;;; by the Free Software Foundation, either version 3 of the License,
;;; or (at your option) any later version.
;;;
;;; Chickadee is distributed in the hope that it will be useful, but
;;; WITHOUT ANY WARRANTY; without even the implied warranty of
;;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
;;; General Public License for more details.
;;;
;;; You should have received a copy of the GNU General Public License
;;; along with this program.  If not, see
;;; <http://www.gnu.org/licenses/>.

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

(define-module (chickadee math quaternion)
  #:use-module (chickadee math)
  #:use-module (srfi srfi-9)
  #:use-module (system foreign)
  #:export (quaternion
            quaternion?
            quaternion-w
            quaternion-x
            quaternion-y
            quaternion-z
            make-identity-quaternion))

(define-record-type <quaternion>
  (wrap-quaternion bv pointer)
  quaternion?
  (bv unwrap-quaternion)
  (pointer quaternion-pointer set-quaternion-pointer!))

(define (quaternion->pointer q)
  "Return a foreign pointer to Q."
  ;; Create foreign pointer lazily.
  (or (quaternion-pointer q)
      (let ((pointer (bytevector->pointer (unwrap-quaternion q))))
        (set-quaternion-pointer! q pointer)
        pointer)))

(define-inlinable (quaternion-ref q i)
  (f32vector-ref (unwrap-quaternion q) i))

(define-inlinable (quaternion-set! q i x)
  (f32vector-set! (unwrap-quaternion q) i x))

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

(define-inlinable (quaternion x y z w)
  "Return a new quaternion with values X, Y, Z, and W."
  (with-new-quaternion q
    (quaternion-set! q 0 x)
    (quaternion-set! q 1 y)
    (quaternion-set! q 2 z)
    (quaternion-set! q 3 w)))

(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-x q)
  "Return the X coordinate of the quaternion Q."
  (quaternion-ref q 0))

(define-inlinable (quaternion-y q)
  "Return the Y coordinate of the quaternion Q."
  (quaternion-ref q 1))

(define-inlinable (quaternion-z q)
  "Return the Z coordinate of the quaternion Q."
  (quaternion-ref q 2))

(define-inlinable (quaternion-w q)
  "Return the W coordinate of the quaternion Q."
  (quaternion-ref q 3))

(define-inlinable (quaternion-magnitude q)
  "Return the magnitude of the quaternion Q."
  (sqrt
   (+ (square (quaternion-w q))
      (square (quaternion-x q))
      (square (quaternion-y q))
      (square (quaternion-z q)))))