AtlatestRepositorysigil-geometry
sigil-geometry / tree / testtest-quat.sgl
1
;;; Tests for (sigil geometry quat)3
(import (sigil test)4
(sigil geometry))6
(define pi 3.141592653589793)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 -> quaternion29
;; conversion: positive trace, then the largest diagonal element on x,30
;; y and z (rotations near pi have negative trace).31
(define rotation-cases32
(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 normalized50
(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 b132
(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 x146
(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 and179
;; 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))))