1 (in-package :cl-tuples
)
3 (def-tuple-type quaternion
4 :tuple-element-type fast-float
8 (export-tuple-operations quaternion
)
10 (def-tuple-type angle-axis
11 :tuple-element-type fast-float
12 :initial-element
0.0f0
15 (export-tuple-operations angle-axis
)
17 (def-tuple-op angle-axis-matrix33
*
18 ((aa angle-axis
(x y z a
)))
34 (def-tuple-op matrix33-trace
*
35 ((m matrix33
#.
(tuple-elements 'matrix33
)))
39 (def-tuple-op vector3d-angle-axis
*
40 ((axis vector3d
(x y z
))
43 (angle-axis-values* x y z angle
)))
45 (def-tuple-op matrix33-angle-axis
*
46 ((m matrix33
#.
(tuple-elements 'matrix33
)))
48 (let* ((trace (matrix33-trace* m
))
49 (angle (acos (/ (1- trace
) 2))))
52 (angle-axis-key-values))
53 ((< 0.0 angle
#1=#.
(coerce pi
'fast-float
))
64 (let* ((u0 (/ (sqrt (1+ (- e00 e11 e22
))) 2))
66 (vector3d-values* u0
(/ e01
2u0) (/ e02
2u0))))
68 (let* ((u1 (/ (sqrt (1+ (- e11 e00 e22
))) 2))
70 (vector3d-values* (/ e01
2u1) u1
(/ e12
2u1))))
72 (let* ((u2 (/ (sqrt (1+ (- e22 e00 e11
))) 2))
74 (vector3d-values* (/ e02
2u2) (/ e12
2u2) u2
))))
86 ;; need conjugate, angle-axis conversion, slerp
88 (def-tuple-op quaternion-conjugate
*
89 ((q quaternion
(x y z w
)))
90 (quaternion-values* (- x
) (- y
) (- z
) w
))
92 (def-tuple-op quaternion-scale
*
93 ((q quaternion
(x y z w
))
95 "Multiply a quat by a scalar"
98 (*) (quaternion-key-values :initial-element s
) q
)))
100 (def-tuple-op quaternion-dot
*
103 "Dot product of two quaternions."
106 (+) (quaternion-map* (*) q0 q1
))))
108 (def-tuple-op quaternion-mag-square
*
111 (quaternion-dot* q q
)))
113 (def-tuple-op quaternion-mag
*
116 (sqrt (quaternion-mag-square* q
))))
118 (def-tuple-op quaternion-normalize
*
120 "Ensure a quaternion is a unit"
123 q
(/ (quaternion-mag* q
)))))
125 (def-tuple-op quaternion-sum
*
128 "Sum the components of two quaternions"
130 (quaternion-map* (+) q0 q1
)))
132 (def-tuple-op quaternion-inverse
*
134 "Inverse of quaternion"
137 q
(/ (quaternion-mag-square* q
)))))
139 (def-tuple-op quaternion-power
*
140 ((q quaternion
(x y z w
))
143 (let* ((angle (acos w
))
145 (factor (* angle times
(/ sin
))))
150 (cos (* times angle
))))))
152 (def-tuple-op quaternion-product
*
153 ((q-lhs quaternion
(x1 y1 z1 w1
))
154 (q-rhs quaternion
(x2 y2 z2 w2
)))
155 "Multiple of two quaternions"
157 (quaternion-values* (- (+ (* w1 x2
) (* x1 w2
) (* y1 z2
)) (* z1 y2
))
158 (- (+ (* w1 y2
) (* y1 w2
) (* z1 x2
)) (* x1 z2
))
159 (- (+ (* w1 z2
) (* x1 y2
) (* z1 w2
)) (* y1 x2
))
160 (- (* w1 w2
) (* x1 x2
) (* y1 y2
) (* z1 z2
)))))
162 (def-tuple-op quaternion-matrix33
*
163 ((q quaternion
(x y z w
)))
164 "Convert a quaternion to a 3x3 rotation matrix."
167 (- 1 (* 2 y y
) (* 2 z z
)) (- (* 2 x y
) (* 2 w z
)) (+ (* 2 x z
) (* 2 w y
))
168 (+ (* 2 x y
) (* 2 w z
)) (- 1 (* 2 x x
) (* 2 z z
)) (- (* 2 y z
) (* 2 w x
))
169 (- (* 2 x z
) (* 2 w y
)) (+ (* 2 y z
) (* 2 w x
)) (- 1 (* 2 x x
) (* 2 y y
)))))
171 (def-tuple-op matrix33-quaternion
*
172 ((m matrix33
:default
))
174 (let ((trace (matrix33-trace* m
)))
176 (let* ((w (/ (sqrt (1+ trace
)) 2))
184 (let* ((x (/ (sqrt (1+ (- e00 e11 e22
))) 2))
190 (/ (- e21 e21
) 4x
))))
192 (let* ((y (/ (sqrt (1+ (- e11 e00 e22
))) 2))
198 (/ (- e20 e02
) 4y
))))
200 (let* ((z (/ (sqrt (1+ (- e22 e00 e11
))) 2))
206 (/ (- e01 e10
) 4z
)))))
217 (def-tuple-op angle-axis-quaternion
*
218 ((aa angle-axis
(x y z a
)))
219 "Convert an angle-axis tuple to a quaternion tuple"
221 (let* ((a/2 (* 0.5 a
))
222 (sin-angle (sin a
/2)))
229 (def-tuple-op quaternion-angle-axis
*
230 ((q quaternion
(x y z w
)))
231 "Convert an quaternion tuple to an angle-axis tuple. If the angle is
232 zero, the axis isn't defined, so the unit x vector is returned instead."
234 ;; either test for one, disable traps, or catch the exception
235 (if (or (= w
1.0) (= w -
1.0))
236 (angle-axis-key-values x
1.0)
237 (let ((angle (* 2 (acos w
))))
238 (vector3d-angle-axis*
240 (vector3d-values* x y z
)
241 (/ (sqrt (- 1 (* w w
)))))
244 (def-tuple-op quaternion-transform-vector3d
*
245 ((vector vector3d
(vx vy vz
))
246 (quat quaternion
(qx qy qz qw
)))
247 "Transform a 3d vector with a quaternion"
253 (vector3d-quaternion* vector
))
254 (quaternion-conjugate* quat
))
256 (vector3d-values* rx ry rz
))))
258 (def-tuple-op vector3d-quaternion
*
259 ((vector vector3d
(vx vy vz
)))
260 "Convert a 3d vector into q auqt for angular velocity purposes"
262 (quaternion-values* vx vy vz
0.0)))