From: jpreiss <jamesalanpreiss@gmail.com>
Date: Tue, 2 Jun 2020 02:13:15 +0000 (-0700)
Subject: quat axis/angle conversion tests
X-Git-Url: https://git.owens.tech/assets/static/gitweb.js/assets/static/gitweb.js/git?a=commitdiff_plain;h=61ede17307a61748798bf511a73491a58b1d35c3;p=forks%2Fcmath3d.git

quat axis/angle conversion tests
---

diff --git a/math3d.h b/math3d.h
index 73375a1..17b30ec 100644
--- a/math3d.h
+++ b/math3d.h
@@ -530,6 +530,7 @@ static inline struct quat qeye(void) {
 }
 // construct a quaternion from an axis and angle of rotation.
 // does not assume axis is normalized.
+// scaling the axis will NOT change the resulting quaternion.
 static inline struct quat qaxisangle(struct vec axis, float angle) {
 	float scale = sinf(angle / 2) / vmag(axis);
 	struct quat q;
diff --git a/test.c b/test.c
index 9a45d10..d6e13f0 100644
--- a/test.c
+++ b/test.c
@@ -96,10 +96,12 @@ void test_mat_axisangle()
 	printf("%s passed\n", __func__);
 }
 
-void test_quat_rpy_conversions()
+void test_quat_conversions()
 {
 	srand(0); // deterministic
 	int const N = 10000;
+
+	// rpy->quat->rpy
 	for (int i = 0; i < N; ++i) {
 		float yaw   = randu(-0.98f*M_PI_F, 0.98f*M_PI_F); // quat2rpy never outputs outside [-pi, pi]
 		float pitch = randu(-0.48f*M_PI_F, 0.48f*M_PI_F); // avoid singularity
@@ -108,21 +110,49 @@ void test_quat_rpy_conversions()
 		struct vec rpy1 = quat2rpy(rpy2quat(rpy0));
 		ASSERT_VEQ_EPSILON(rpy0, rpy1, 0.00001f); // must be fairly loose due to 32 bit trig, etc.
 	}
-	printf("%s passed\n", __func__);
-}
 
-void test_quat_mat_conversions()
-{
-	srand(0); // deterministic
-	int const N = 10000;
+	// quat->matrix->quat
 	for (int i = 0; i < N; ++i) {
 		struct quat const q = randquat();
 		struct mat33 const m = quat2rotmat(q);
 		struct quat const qq = mat2quat(m);
 		float const angle = qanglebetween(q, qq);
 		// TODO: seems like a lot of precision loss -- can we improve?
-		assert(fabs(angle) < radians(0.1f));
+		assert(fabsf(angle) < radians(0.1f));
+	}
+
+	// quat->axis/angle->quat
+	for (int i = 0; i < N; ++i) {
+		struct quat const q = randquat();
+		struct vec qaxis = quat2axis(q);
+		float qangle = quat2angle(q);
+		struct quat qq = qaxisangle(qaxis, qangle);
+		float const angle = qanglebetween(q, qq);
+		// TODO: seems like a lot of precision loss -- can we improve?
+		assert(fabsf(angle) < radians(0.1f));
 	}
+
+	// axis/angle->quat->axis/angle
+	for (int i = 0; i < N; ++i) {
+		struct vec axis = randcube();
+		float angle = randn();
+		if (fabsf(angle) < 1e-3) {
+			// conversion is not stable for small angles.
+			continue;
+		}
+		struct quat q = qaxisangle(axis, angle);
+		struct vec qaxis = quat2axis(q);
+		float qangle = quat2angle(q);
+		float anorm = vmag(axis);
+		float qanorm = vmag(qaxis);
+		float dot = vdot(vdiv(axis, anorm), vdiv(qaxis, qanorm));
+		assert(fabsf(dot) >= (1.0f - 1e-6f));
+		if (dot < 0) {
+			qangle *= -1.0f;
+		}
+		assert(fabsf(qangle - angle) < 1e-4);
+	}
+
 	printf("%s passed\n", __func__);
 }
 
@@ -190,8 +220,7 @@ typedef void (*voidvoid_fn)(void);
 voidvoid_fn test_fns[] = {
 	test_vec_basic,
 	test_mat_axisangle,
-	test_quat_rpy_conversions,
-	test_quat_mat_conversions,
+	test_quat_conversions,
 	test_qvectovec,
 	test_qslerp,
 };