From: James Preiss <japreiss@usc.edu>
Date: Wed, 25 Jul 2018 19:32:26 +0000 (-0400)
Subject: more quat ctors/conversions
X-Git-Url: https://git.owens.tech/editable-focus.html/editable-focus.html/git?a=commitdiff_plain;h=6859420a381c23755f44e601210b8cd1da5bd19d;p=forks%2Fcmath3d.git

more quat ctors/conversions
---

diff --git a/Makefile b/Makefile
index 88db6cb..37059ff 100644
--- a/Makefile
+++ b/Makefile
@@ -1,5 +1,5 @@
 test_math3d: test.c math3d.h
-	$(CC) -std=c99 -Wall -Wpedantic -Wdouble-promotion -o test_math3d test.c -lm
+	$(CC) -std=c99 -Wall -Wpedantic -Wdouble-promotion -g -o test_math3d test.c -lm
 
 test:
 	./test_math3d
diff --git a/math3d.h b/math3d.h
index 279ece1..c798db7 100644
--- a/math3d.h
+++ b/math3d.h
@@ -29,7 +29,9 @@ SOFTWARE.
 #include <stdbool.h>
 
 #ifndef M_PI_F
-#define M_PI_F (3.14159265358979323846f)
+#define M_PI_F   (3.14159265358979323846f)
+#define M_1_PI_F (0.31830988618379067154f)
+#define M_PI_2_F (1.57079632679f)
 #endif
 
 
@@ -480,6 +482,10 @@ static inline struct quat qaxisangle(struct vec axis, float angle) {
 	q.w = cosf(angle/2);
 	return q;
 }
+
+// fwd declare, needed in some ctors
+static inline struct quat qnormalize(struct quat q);
+
 // construct a quaternion such that q * a = b,
 // and the rotation axis is orthogonal to the plane defined by a and b,
 // and the rotation is less than 180 degrees.
@@ -498,8 +504,8 @@ static inline struct quat qvectovec(struct vec a, struct vec b) {
 	}
 	float const halfcos = 0.5f * cosangle;
 	// since angle is < 180deg, the positive sqrt is always correct
-	float const sinhalfangle = sqrtf(0.5f - halfcos);
-	float const coshalfangle = sqrtf(0.5f + halfcos);
+	float const sinhalfangle = sqrtf(fmax(0.5f - halfcos, 0.0f));
+	float const coshalfangle = sqrtf(fmax(0.5f + halfcos, 0.0f));
 	struct vec const qimag = vscl(sinhalfangle / sinangle, cross);
 	float const qreal = coshalfangle;
 	return quatvw(qimag, qreal);
@@ -538,6 +544,17 @@ static inline struct quat rpy2quat_small(struct vec rpy) {
 		return quatvw(vscl(w/2, rpy), w);
 	}
 }
+// construct quaternion from orthonormal matrix.
+static inline struct quat mat2quat(struct mat33 m) {
+	float w = sqrtf(fmax(0.0f, 1.0f + m.m[0][0] + m.m[1][1] + m.m[2][2])) / 2.0f;
+	float x = sqrtf(fmax(0.0f, 1.0f + m.m[0][0] - m.m[1][1] - m.m[2][2])) / 2.0f;
+	float y = sqrtf(fmax(0.0f, 1.0f - m.m[0][0] + m.m[1][1] - m.m[2][2])) / 2.0f;
+	float z = sqrtf(fmax(0.0f, 1.0f - m.m[0][0] - m.m[1][1] + m.m[2][2])) / 2.0f;
+	x = copysign(x, m.m[2][1] - m.m[1][2]);
+	y = copysign(y, m.m[0][2] - m.m[2][0]);
+	z = copysign(z, m.m[1][0] - m.m[0][1]);
+	return mkquat(x, y, z, w);
+}
 
 //
 // conversions to other parameterizations of 3D rotations
@@ -560,8 +577,13 @@ static inline struct vec quat2axis(struct quat q) {
 	return vscl(s, mkvec(q.x, q.y, q.z));
 }
 // compute the angle of a quaternion's axis-angle decomposition.
+// result lies in domain (-pi, pi].
 static inline float quat2angle(struct quat q) {
-	return 2 * acosf(q.w);
+	float angle = 2 * acosf(q.w);
+	if (angle > M_PI_F) {
+		angle -= 2.0f * M_PI_F;
+	}
+	return angle;
 }
 // vector containing the imaginary part of the quaternion, i.e. (x, y, z)
 static inline struct vec quatimagpart(struct quat q) {
@@ -620,12 +642,19 @@ static inline struct quat qinv(struct quat q) {
 static inline struct quat qneg(struct quat q) {
 	return mkquat(-q.x, -q.y, -q.z, -q.w);
 }
+static inline struct quat qposreal(struct quat q) {
+	if (q.w < 0) return qneg(q);
+	return q;
+}
 // quaternion dot product. is cosine of angle between them.
 static inline float qdot(struct quat a, struct quat b) {
 	return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
 }
 static inline float qanglebetween(struct quat a, struct quat b) {
-	return acosf(qdot(a, b));
+	float const dot = qdot(qposreal(a), qposreal(b));
+	if (dot > 1.0f - 1e9f) return 0.0f;
+	if (dot < -1.0f + 1e9f) return M_PI_F;
+	return acosf(dot);
 }
 static inline bool qeq(struct quat a, struct quat b) {
 	return a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w;
diff --git a/test.c b/test.c
index 5da8e04..b870dcf 100644
--- a/test.c
+++ b/test.c
@@ -22,6 +22,13 @@ struct vec randcube() {
 	return mkvec(randu(-1.0f, 1.0f), randu(-1.0f, 1.0f), randu(-1.0f, 1.0f));
 }
 
+// returns a random quaternion not necessarily uniformly sampled
+struct quat randquat() {
+	struct quat q = mkquat(
+		randu(-1.0f, 1.0f), randu(-1.0f, 1.0f), randu(-1.0f, 1.0f), randu(-1.0f, 1.0f));
+	return qnormalize(q);
+}
+
 void printvec(struct vec v) {
 	printf("%f, %f, %f", (double)v.x, (double)v.y, (double)v.z);
 }
@@ -55,6 +62,20 @@ void test_quat_rpy_conversions()
 	printf("%s passed\n", __func__);
 }
 
+void test_quat_mat_conversions()
+{
+	srand(0); // deterministic
+	int const N = 10000;
+	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);
+		assert(fabs(angle) < radians(1e-1));
+	}
+	printf("%s passed\n", __func__);
+}
+
 void test_qvectovec()
 {
 	srand(0); // deterministic
@@ -76,6 +97,10 @@ void test_qvectovec()
 		struct quat const q = qvectovec(a, b);
 		struct vec const qa = qvrot(q, a);
 		ASSERT_VEQ_EPSILON(qa, b, 0.00001f); 
+
+		struct vec cross = vcross(a, b);
+		struct vec const qcross = qvrot(q, cross);
+		ASSERT_VEQ_EPSILON(qcross, cross, 0.00001f); 
 	}
 	printf("%s passed\n", __func__);
 }
@@ -84,6 +109,7 @@ void test_qvectovec()
 typedef void (*voidvoid_fn)(void);
 voidvoid_fn test_fns[] = {
 	test_quat_rpy_conversions,
+	test_quat_mat_conversions,
 	test_qvectovec,
 };