axis-angle rotation matrix

diff --git a/Makefile b/Makefile
index 37059ff..833525a 100644 (file)
--- a/Makefile
+++ b/Makefile
@@ -1,7 +1,7 @@
 test_math3d: test.c math3d.h
        $(CC) -std=c99 -Wall -Wpedantic -Wdouble-promotion -g -o test_math3d test.c -lm
 
-test:
+test: ./test_math3d
        ./test_math3d
 
 clean:

diff --git a/math3d.h b/math3d.h
index cac8b44..ef6bd94 100644 (file)
--- a/math3d.h
+++ b/math3d.h
@@ -179,7 +179,7 @@ static inline float vnorm1(struct vec v) {
 }
 
 //
-// comparisons
+// comparisons, including partial orderings
 //
 
 // compare two vectors for exact equality.
@@ -445,6 +445,46 @@ static inline void set_block33_rowmaj(float *block, int stride, struct mat33 con
        }
 }
 
+//
+// special functions to ease the pain of writing vector math in C.
+//
+
+// add three matrices.
+static inline struct mat33 madd3(struct mat33 a, struct mat33 b, struct mat33 c) {
+       return madd(madd(a, b), c);
+}
+
+//
+// 3D rotation constructors & operators
+//
+
+// construct equivalent rotation matrix for axis-angle rotation.
+// assumes input axis is normalized, angle in radians.
+static inline struct mat33 maxisangle(struct vec axis, float angle) {
+       // Rodrigues formula
+       struct mat33 const K = mcrossmat(axis);
+       return madd3(
+               meye(),
+               mscl(sinf(angle), K),
+               mscl(1.0f - cosf(angle), mmul(K, K))
+       );
+}
+// rotation about x axis by given angle (radians)
+static inline struct mat33 mrotx(float angle) {
+       return maxisangle(mkvec(1.0f, 0.0f, 0.0f), angle);
+}
+// rotation about y axis by given angle (radians)
+static inline struct mat33 mroty(float angle) {
+       return maxisangle(mkvec(0.0f, 1.0f, 0.0f), angle);
+}
+// rotation about z axis by given angle (radians)
+static inline struct mat33 mrotz(float angle) {
+       return maxisangle(mkvec(0.0f, 0.0f, 1.0f), angle);
+}
+// TODO: these might be faster if written by hand,
+// but these are correct and the trig is probably the slow part anyway
+
+
 // Matrix TODO: inv, solve, eig, 9 floats ctor, axis-aligned rotations
 
 

diff --git a/test.c b/test.c
index 64e608a..1565737 100644 (file)
--- a/test.c
+++ b/test.c
@@ -17,11 +17,23 @@ float randu(float a, float b) {
        return (float)x;
 }
 
+// APPROXIMATELY unit normal random number
+float randn() {
+       // central limit theorem at work
+       return randu(-1.0f, 1.0f) + randu(-1.0f, 1.0f) + randu(-1.0f, 1.0f);
+}
+
 // 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));
 }
 
+// returns a random vector approximately uniformly sampled from the unit sphere
+struct vec randsphere() {
+       struct vec v = mkvec(randn(), randn(), randn());
+       return vnormalize(v);
+}
+
 // returns a random quaternion not necessarily uniformly sampled
 struct quat randquat() {
        struct quat q = mkquat(
@@ -47,6 +59,33 @@ do { \
 // tests - when adding new tests, make sure to add to test_fns array
 //
 
+void test_mat_axisangle()
+{
+       srand(0); // deterministic
+       int const N = 10000;
+
+       for (int i = 0; i < N; ++i) {
+               // random rotation axis and point to rotate
+               struct vec const axis = randsphere();
+               struct vec const point = randsphere();
+
+               // rotation angle s.t. we will go a full circle
+               int const divisions = randu(0.0, 100.0);
+               float const theta = 2.0f * M_PI_F / divisions;
+
+               // rotate the point in a full circle
+               struct vec pointrot = point;
+               struct mat33 const R = maxisangle(axis, theta);
+               for (int j = 0; j < divisions; ++j) {
+                       pointrot = mvmul(R, pointrot);
+               }
+
+               // should be back where we started
+               ASSERT_VEQ_EPSILON(point, pointrot, 0.00001f); // must be fairly loose due to 32 bit trig, etc.
+       }
+       printf("%s passed\n", __func__);
+}
+
 void test_quat_rpy_conversions()
 {
        srand(0); // deterministic
@@ -108,6 +147,7 @@ void test_qvectovec()
 // micro test framework
 typedef void (*voidvoid_fn)(void);
 voidvoid_fn test_fns[] = {
+       test_mat_axisangle,
        test_quat_rpy_conversions,
        test_quat_mat_conversions,
        test_qvectovec,