AtlatestRepositorysigil-geometry

sigil-geometry / tree / testtest-quat.sgl

1;;; Tests for (sigil geometry quat)
2
3(import (sigil test)
4 (sigil geometry))
5
6(define pi 3.141592653589793)
7
8(define (assert-vec3 expected actual)
9 (unless (vec3-approx=? expected actual)
10 (assert-equal (vec3->list expected) (vec3->list actual))))
12(define (assert-quat expected actual)
13 (unless (quat-approx=? expected actual)
14 (assert-equal (quat->list expected) (quat->list actual))))
16(define (assert-same-rotation expected actual)
17 (unless (quat-same-rotation? expected actual)
18 (assert-equal (quat->list expected) (quat->list actual))))
20(define (assert-mat4 expected actual)
21 (unless (mat4-approx=? expected actual)
22 (assert-equal (mat4->list expected) (mat4->list actual))))
24(define z-axis (vec3 0 0 1))
26(define (quat-scale-neg q) (quat (- (quat-x q)) (- (quat-y q)) (- (quat-z q)) (- (quat-w q))))
28;; Each case lands in a different branch of the matrix -> quaternion
29;; conversion: positive trace, then the largest diagonal element on x,
30;; y and z (rotations near pi have negative trace).
31(define rotation-cases
32 (list (cons "positive trace" (quat-from-axis-angle (vec3 1 2 3) 0.5))
33 (cons "x dominant" (quat-from-axis-angle (vec3 1 0 0) pi))
34 (cons "y dominant" (quat-from-axis-angle (vec3 0.1 1 0.2) 3.0))
35 (cons "z dominant" (quat-from-axis-angle (vec3 0 0.1 1) 3.1))
36 (cons "identity" (make-quat))))
38(define rot-0 (quat-from-axis-angle z-axis 0.0))
39(define rot-90 (quat-from-axis-angle z-axis (/ pi 2)))
41(test-group "quat - construction"
42 (test "identity"
43 (assert-equal '(0.0 0.0 0.0 1.0) (quat->list (make-quat)))
44 (assert-equal '(0.0 0.0 0.0 1.0) (quat->list (quat-identity))))
46 (test "axis-angle"
47 ;; 90 degrees about z: (0 0 sin 45 cos 45)
48 (assert-quat (quat 0 0 0.70710678 0.70710678) (quat-from-axis-angle z-axis (/ pi 2)))
49 ;; the axis is normalized
50 (assert-quat (quat-from-axis-angle z-axis 1.0) (quat-from-axis-angle (vec3 0 0 9) 1.0)))
52 (test "zero axis gives the identity"
53 (assert-quat (make-quat) (quat-from-axis-angle (make-vec3) 2.0)))
55 (test "angle"
56 (assert-true (float-approx=? 1.2 (quat-angle (quat-from-axis-angle (vec3 1 1 1) 1.2))))))
58(test-group "quat - rotation"
59 (test "rotate x by 90 degrees about z gives y"
60 (assert-vec3 (vec3 0 1 0) (quat-rotate-vec3 (quat-from-axis-angle z-axis (/ pi 2)) (vec3 1 0 0))))
62 (test "same direction as mat4-rotation"
63 (let ((axis (vec3 1 -2 0.5)) (v (vec3 0.3 4 -2)))
64 (assert-vec3 (mat4-transform-direction (mat4-rotation axis 2.1) v)
65 (quat-rotate-vec3 (quat-from-axis-angle axis 2.1) v))))
67 (test "mul applies the right-hand rotation first"
68 (let ((qa (quat-from-axis-angle (vec3 1 0 0) 0.7))
69 (qb (quat-from-axis-angle (vec3 0 1 0) -1.3))
70 (v (vec3 1 2 3)))
71 (assert-vec3 (quat-rotate-vec3 qa (quat-rotate-vec3 qb v))
72 (quat-rotate-vec3 (quat-mul qa qb) v))
73 (assert-mat4 (mat4-mul (mat4-from-quat qa) (mat4-from-quat qb))
74 (mat4-from-quat (quat-mul qa qb)))))
76 (test "conjugate undoes a unit rotation"
77 (let ((q (quat-from-axis-angle (vec3 2 1 -1) 0.9)))
78 (assert-quat (make-quat) (quat-mul q (quat-conjugate q)))))
80 (test "inverse works for non-unit quaternions"
81 (let ((q (quat 1 2 3 4)))
82 (assert-quat (make-quat) (quat-mul q (quat-inverse q)))
83 (assert-quat (make-quat) (quat-mul (quat-inverse q) q))))
85 (test "normalize"
86 (assert-true (float-approx=? 1.0 (quat-length (quat-normalize (quat 1 2 3 4)))))
87 (assert-quat (make-quat) (quat-normalize (quat 0 0 0 0))))
89 (test "mul! and rotate! may alias"
90 (let* ((qa (quat-from-axis-angle (vec3 1 0 0) 0.7))
91 (qb (quat-from-axis-angle (vec3 0 1 0) -1.3))
92 (expected (quat-mul qa qb))
93 (a (quat-copy qa)))
94 (quat-mul! a a qb)
95 (assert-quat expected a))
96 (let ((v (vec3 1 0 0)))
97 (quat-rotate-vec3! v (quat-from-axis-angle z-axis (/ pi 2)) v)
98 (assert-vec3 (vec3 0 1 0) v))))
100(test-group "quat - matrix round trips"
101 (test "quat -> mat4 -> quat"
102 (for-each (lambda (c)
103 (assert-same-rotation (cdr c) (quat-from-mat4 (mat4-from-quat (cdr c)))))
104 rotation-cases))
106 (test "quat -> mat3 -> quat"
107 (for-each (lambda (c)
108 (assert-same-rotation (cdr c) (quat-from-mat3 (mat3-from-quat (cdr c)))))
109 rotation-cases))
111 (test "mat4 -> quat -> mat4"
112 (for-each (lambda (m)
113 (assert-mat4 m (mat4-from-quat (quat-from-mat4 m))))
114 (list (mat4-rotation-x 0.3) (mat4-rotation-y 3.0) (mat4-rotation-z -2.9)
115 (mat4-rotation (vec3 1 1 1) (* 2 (/ pi 3))))))
117 (test "quat-from-mat4 ignores translation"
118 (let ((q (quat-from-axis-angle (vec3 0 1 0) 1.1)))
119 (assert-same-rotation q (quat-from-mat4 (mat4-from-trs (vec3 5 6 7) q (vec3 1 1 1)))))))
121(test-group "quat - interpolation"
122 (test "slerp endpoints"
123 (assert-quat rot-0 (quat-slerp rot-0 rot-90 0))
124 (assert-quat rot-90 (quat-slerp rot-0 rot-90 1)))
126 (test "slerp midpoint is half the angle"
127 (assert-quat (quat-from-axis-angle z-axis (/ pi 4)) (quat-slerp rot-0 rot-90 0.5))
128 (assert-quat (quat-from-axis-angle z-axis (/ pi 8)) (quat-slerp rot-0 rot-90 0.25)))
130 (test "slerp takes the shorter arc"
131 ;; -b is the same rotation as b
132 (assert-same-rotation (quat-slerp rot-0 rot-90 0.5)
133 (quat-slerp rot-0 (quat-scale-neg rot-90) 0.5)))
135 (test "slerp of nearly equal rotations falls back to nlerp"
136 (let ((c (quat-from-axis-angle z-axis 0.001)))
137 (assert-same-rotation (quat-from-axis-angle z-axis 0.0005) (quat-slerp rot-0 c 0.5))))
139 (test "nlerp stays unit length"
140 (assert-true (float-approx=? 1.0 (quat-length (quat-nlerp rot-0 rot-90 0.3))))))
143(test-group "quat - comparison is strict"
144 (test "same-rotation? rejects a different rotation of different length"
145 ;; (0.7 0 0 1.2) is not unit length and is a 60-degree turn about x
146 (assert-false (quat-same-rotation? (quat 0 0 0 1) (quat 0.7 0 0 1.2))))
148 (test "same-rotation? rejects a 0.5-degree difference"
149 (assert-false (quat-same-rotation? (quat-from-axis-angle z-axis 0.0)
150 (quat-from-axis-angle z-axis (degrees->radians 0.5)))))
152 (test "same-rotation? accepts q against -q"
153 (let ((q (quat-from-axis-angle (vec3 1 2 3) 0.8)))
154 (assert-true (quat-same-rotation? q (quat-scale-neg q)))))
156 (test "NaN is never the same rotation"
157 (let ((n (- (* 1e300 1e300) (* 1e300 1e300))))
158 (assert-false (quat-same-rotation? (make-quat) (quat 0 0 0 n)))
159 (assert-false (quat-approx=? (make-quat) (quat n 0 0 1))))))
161(test-group "quat - each matrix conversion branch, by value"
162 ;; The expected quaternions follow from the axis-angle definition:
163 ;; pi about x is (1 0 0 0), and so on. Up to sign.
164 (test "x dominant: pi about x"
165 (assert-same-rotation (quat 1 0 0 0) (quat-from-mat4 (mat4-rotation-x pi))))
167 (test "y dominant: pi about y"
168 (assert-same-rotation (quat 0 1 0 0) (quat-from-mat4 (mat4-rotation-y pi))))
170 (test "z dominant: pi about z"
171 (assert-same-rotation (quat 0 0 1 0) (quat-from-mat4 (mat4-rotation-z pi))))
173 (test "x dominant, not a half turn: 3.0 rad about x"
174 (assert-same-rotation (quat-from-axis-angle (vec3 1 0 0) 3.0)
175 (quat-from-mat3 (mat3-from-mat4 (mat4-rotation-x 3.0))))))
177(test-group "quat - nlerp by value"
178 ;; numpy (float64): normalize((1 - t) a + t b) with a the identity and
179 ;; b 90 degrees about z.
180 (test "nlerp at 0.25, 0.5 and 0.75"
181 (assert-quat (quat 0 0 0.18736555 0.982290258) (quat-nlerp rot-0 rot-90 0.25))
182 (assert-quat (quat 0 0 0.382683432 0.923879533) (quat-nlerp rot-0 rot-90 0.5))
183 (assert-quat (quat 0 0 0.562096651 0.827071554) (quat-nlerp rot-0 rot-90 0.75)))
185 (test "nlerp differs from slerp away from the midpoint"
186 (assert-false (quat-approx=? (quat-nlerp rot-0 rot-90 0.25) (quat-slerp rot-0 rot-90 0.25))))
188 (test "nlerp takes the shorter arc"
189 (assert-same-rotation (quat-nlerp rot-0 rot-90 0.25)
190 (quat-nlerp rot-0 (quat-scale-neg rot-90) 0.25))))