quat for ortho rot from vec to vec

diff --git a/math3d.h b/math3d.h
index af937a9..279ece1 100644 (file)
--- 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 (file)
--- 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;