static inline bool vneq(struct vec a, struct vec b) {
return !veq(a, b);
}
-// element-wise less-than
+// compare two vectors for near-equality with user-defined threshold.
+static inline bool veqepsilon(struct vec a, struct vec b, float epsilon) {
+ struct vec diffs = vabs(vsub(a, b));
+ return diffs.x < epsilon && diffs.y < epsilon && diffs.z < epsilon;
+}
+// all(element-wise less-than)
static inline bool vless(struct vec a, struct vec b) {
return (a.x < b.x) && (a.y < b.y) && (a.z < b.z);
}
-// element-wise less-than-or-equal
+// all(element-wise less-than-or-equal)
static inline bool vleq(struct vec a, struct vec b) {
return (a.x <= b.x) && (a.y <= b.y) && (a.z <= b.z);
}
-// element-wise greater-than
+// all(element-wise greater-than)
static inline bool vgreater(struct vec a, struct vec b) {
return (a.x > b.x) && (a.y > b.y) && (a.z > b.z);
}
-// element-wise greater-than-or-equal
+// all(element-wise greater-than-or-equal)
static inline bool vgeq(struct vec a, struct vec b) {
return (a.x >= b.x) && (a.y >= b.y) && (a.z >= b.z);
}
static inline void vstoref(struct vec v, float *f) {
f[0] = v.x; f[1] = v.y; f[2] = v.z;
}
-// Vector TODO: fuzzy comparison
// ---------------------------- 3x3 matrices ------------------------------
}
}
-// Matrix TODO: inv, solve, eig, raw floats, axis-aligned rotations
+// Matrix TODO: inv, solve, eig, 9 floats ctor, axis-aligned rotations
// ---------------------------- quaternions ------------------------------
q.w = cosf(angle/2);
return q;
}
-static inline struct vec quataxis(struct quat q) {
- // TODO this is not numerically stable for tiny rotations
- float s = 1.0f / sqrtf(1 - q.w * q.w);
- return vscl(s, mkvec(q.x, q.y, q.z));
-}
-static inline float quatangle(struct quat q) {
- return 2 * acosf(q.w);
-}
-// APPROXIMATE conversion of small (roll, pitch, yaw) Euler angles
-// into a quaternion without computing any trig functions.
-// only produces useful result for small angles.
+// 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) {
+ // from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+ float r = rpy.x;
+ float p = rpy.y;
+ float y = rpy.z;
+ float cr = cosf(r / 2.0f); float sr = sinf(r / 2.0f);
+ float cp = cosf(p / 2.0f); float sp = sinf(p / 2.0f);
+ float cy = cosf(y / 2.0f); float sy = sinf(y / 2.0f);
+
+ float qx = sr * cp * cy - cr * sp * sy;
+ float qy = cr * sp * cy + sr * cp * sy;
+ float qz = cr * cp * sy - sr * sp * cy;
+ float qw = cr * cp * cy + sr * sp * sy;
+
+ return mkquat(qx, qy, qz, qw);
+}
+// APPROXIMATE construction of a quaternion from small (roll, pitch, yaw) Euler angles
+// without computing any trig functions. only produces useful result for small angles.
// Example application is integrating a gyroscope when the angular velocity
// of the object is small compared to the sampling frequency.
static inline struct quat rpy2quat_small(struct vec rpy) {
+ // TODO: cite source, but can be derived from rpy2quat under first-order approximation:
+ // sin(epsilon) = epsilon, cos(epsilon) = 1, epsilon^2 = 0
float q2 = vmag2(rpy) / 4.0f;
if (q2 < 1) {
return quatvw(vdiv(rpy, 2), sqrtf(1.0f - q2));
return quatvw(vscl(w/2, rpy), w);
}
}
-static inline struct vec quatimagpart(struct quat q) {
- return mkvec(q.x, q.y, q.z);
-}
//
// conversions to other parameterizations of 3D rotations
//
-// convert quaternion to (roll, pitch, yaw) Euler angles
+// convert quaternion to (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 vec quat2rpy(struct quat q) {
+ // from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
struct vec v;
v.x = atan2f(2.0f * (q.w * q.x + q.y * q.z), 1 - 2 * (fsqr(q.x) + fsqr(q.y))); // roll
v.y = asinf(2.0f * (q.w * q.y - q.x * q.z)); // pitch
static inline float quat2angle(struct quat q) {
return 2 * acosf(q.w);
}
+// vector containing the imaginary part of the quaternion, i.e. (x, y, z)
+static inline struct vec quatimagpart(struct quat q) {
+ return mkvec(q.x, q.y, q.z);
+}
// convert a quaternion into a 3x3 rotation matrix.
static inline struct mat33 quat2rotmat(struct quat q) {
+ // from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
float x = q.x;
float y = q.y;
float z = q.z;
// spherical linear interpolation. s should be between 0 and 1.
static inline struct quat qslerp(struct quat a, struct quat b, float t)
{
+ // from "Animating Rotation with Quaternion Curves", Ken Shoemake, 1985
float dp = qdot(a, b);
if (dp < 0) {
dp = -dp;
f[0] = q.x; f[1] = q.y; f[2] = q.z; f[3] = q.w;
}
-// Quaternion TODO: rpy2quat
-
// Overall TODO: lines? segments? planes? axis-aligned boxes? spheres?
--- /dev/null
+#include <assert.h>
+#include <math.h>
+#include <signal.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "math3d.h"
+
+//
+// helper functions and special asserts
+// (use regular assert() when no special assert applies)
+//
+
+float randu(float a, float b) {
+ double s = rand() / ((double)RAND_MAX);
+ double x = (double)a + ((double)b - (double)a) * s;
+ return (float)x;
+}
+
+void printvec(struct vec v) {
+ printf("%f, %f, %f", (double)v.x, (double)v.y, (double)v.z);
+}
+
+#define ASSERT_VEQ_EPSILON(a, b, epsilon) \
+do { \
+ if (!veqepsilon(a, b, epsilon)) { \
+ printf("\t" #a " = "); printvec(a); printf("\n"); \
+ printf("\t" #b " = "); printvec(b); printf("\n"); \
+ assert(veqepsilon(a, b, epsilon)); \
+ } \
+} while(0) \
+
+
+//
+// tests - when adding new tests, make sure to add to test_fns array
+//
+
+void test_quat_rpy_conversions()
+{
+ srand(0); // deterministic
+ int const N = 10000;
+ 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
+ float roll = randu(-0.98f*M_PI_F, 0.98f*M_PI_F);
+ struct vec rpy0 = mkvec(roll, pitch, yaw);
+ struct vec rpy1 = quat2rpy(rpy2quat(rpy0));
+ ASSERT_VEQ_EPSILON(rpy0, rpy1, 0.00001f); // must be fairly loose due to 32 bit trig, etc.
+ }
+}
+
+// micro test framework
+typedef void (*voidvoid_fn)(void);
+voidvoid_fn test_fns[] = {
+ test_quat_rpy_conversions,
+};
+
+static int i_test = -1;
+static int recursion_level = 0;
+static int exit_code = 0;
+static int const n_tests = sizeof(test_fns) / sizeof(test_fns[0]);
+
+void sighandler(int sig)
+{
+ ++i_test;
+ if (i_test > recursion_level) {
+ exit_code = 1;
+ }
+ if (i_test < n_tests) {
+ (*test_fns[i_test])();
+ ++recursion_level;
+ sighandler(sig);
+ }
+ else {
+ if (exit_code == 0) {
+ puts("All tests passed.");
+ }
+ exit(exit_code);
+ }
+}
+
+int main()
+{
+ signal(SIGABRT, sighandler);
+ sighandler(SIGABRT);
+}
projects / forks / cmath3d.git / commitdiff