From 769ad3f402ad9ea4e020b73937ebe7d75d58ae2a Mon Sep 17 00:00:00 2001
From: James Preiss <japreiss@usc.edu>
Date: Sun, 22 Jul 2018 13:22:19 -0700
Subject: [PATCH] quat for ortho rot from vec to vec

---
 math3d.h | 28 ++++++++++++++++++++++++++++
 test.c   | 32 ++++++++++++++++++++++++++++++++
 2 files changed, 60 insertions(+)

diff --git a/math3d.h b/math3d.h
index af937a9..279ece1 100644
--- a/math3d.h
+++ b/math3d.h
@@ -470,6 +470,7 @@ static inline struct quat qeye(void) {
 	return mkquat(0, 0, 0, 1);
 }
 // construct a quaternion from an axis and angle of rotation.
+// does not assume axis is normalized.
 static inline struct quat qaxisangle(struct vec axis, float angle) {
 	float scale = sinf(angle / 2) / vmag(axis);
 	struct quat q;
@@ -479,6 +480,30 @@ static inline struct quat qaxisangle(struct vec axis, float angle) {
 	q.w = cosf(angle/2);
 	return 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.
+// assumes a and b are unit vectors.
+// does not handle degenerate case where a = -b. returns all-zero quat
+static inline struct quat qvectovec(struct vec a, struct vec b) {
+	struct vec const cross = vcross(a, b);
+	float const sinangle = vmag(cross);
+	float const cosangle = vdot(a, b);
+	// avoid taking sqrt of negative number due to floating point error.
+	// TODO: find tighter exact bound
+	float const EPS_ANGLE = 1e-6;
+	if (sinangle < EPS_ANGLE) {
+		if (cosangle > 0.0f) return qeye();
+		else return mkquat(0.0f, 0.0f, 0.0f, 0.0f); // degenerate
+	}
+	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);
+	struct vec const qimag = vscl(sinhalfangle / sinangle, cross);
+	float const qreal = coshalfangle;
+	return quatvw(qimag, qreal);
+}
 // construct from (roll, pitch, yaw) Euler angles using Tait-Bryan convention
 // (yaw, then pitch about new pitch axis, then roll about new roll axis)
 static inline struct quat rpy2quat(struct vec rpy) {
@@ -602,6 +627,9 @@ static inline float qdot(struct quat a, struct quat b) {
 static inline float qanglebetween(struct quat a, struct quat b) {
 	return acosf(qdot(a, b));
 }
+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;
+}
 // normalize a quaternion.
 // typically used to mitigate precision errors.
 static inline struct quat qnormalize(struct quat q) {
diff --git a/test.c b/test.c
index be4a841..5da8e04 100644
--- a/test.c
+++ b/test.c
@@ -17,6 +17,11 @@ float randu(float a, float b) {
 	return (float)x;
 }
 
+// returns a random vector uniformly sampled from the unit cube
+struct vec randcube() {
+	return mkvec(randu(-1.0f, 1.0f), randu(-1.0f, 1.0f), randu(-1.0f, 1.0f));
+}
+
 void printvec(struct vec v) {
 	printf("%f, %f, %f", (double)v.x, (double)v.y, (double)v.z);
 }
@@ -47,12 +52,39 @@ 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_qvectovec()
+{
+	srand(0); // deterministic
+	int const N = 10000;
+	struct quat const qzero = mkquat(0.0f, 0.0f, 0.0f, 0.0f);
+
+	for (int i = 0; i < N; ++i) {
+		struct vec a = randcube(), b = randcube();
+		// do not try to normalize tiny vectors.
+		if (vmag2(a) < 1e-8 || vmag2(b) < 1e-8) continue;
+		a = vnormalize(a);
+		b = vnormalize(b);
+		// degenerate case - test explicitly, not accidentally.
+		if (vdot(a, b) < -0.99f) continue;
+		// should return zero vector for degenerate case.
+		assert(qeq(qvectovec(a, vneg(a)), qzero));
+
+		// non-degenerate case.
+		struct quat const q = qvectovec(a, b);
+		struct vec const qa = qvrot(q, a);
+		ASSERT_VEQ_EPSILON(qa, b, 0.00001f); 
+	}
+	printf("%s passed\n", __func__);
 }
 
 // micro test framework
 typedef void (*voidvoid_fn)(void);
 voidvoid_fn test_fns[] = {
 	test_quat_rpy_conversions,
+	test_qvectovec,
 };
 
 static int i_test = -1;
-- 
2.20.1