blob: 17a9810a7b435f55e585f380acdcd106547328cc (
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
|
;;; 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))))
|
