From 6859420a381c23755f44e601210b8cd1da5bd19d Mon Sep 17 00:00:00 2001 From: James Preiss Date: Wed, 25 Jul 2018 15:32:26 -0400 Subject: [PATCH] more quat ctors/conversions --- Makefile | 2 +- math3d.h | 39 ++++++++++++++++++++++++++++++++++----- test.c | 26 ++++++++++++++++++++++++++ 3 files changed, 61 insertions(+), 6 deletions(-) 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 #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, }; -- 2.20.1