blob: 064a648e92f64ae76be5cf2fe4121b38dd785a28 (
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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
|
;;; 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 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-w
quaternion-x
quaternion-y
quaternion-z
make-identity-quaternion
rotation->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 (wrap-quaternion (f32vector 0.0 0.0 0.0 0.0) #f)))
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 (display-quaternion q port)
(format port "#<quaterion ~f ~f ~f ~f>"
(quaternion-x q)
(quaternion-y q)
(quaternion-z q)
(quaternion-w q)))
(set-record-type-printer! <quaternion> display-quaternion)
(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))))
|
