From: James Preiss <japreiss@usc.edu>
Date: Sat, 6 Apr 2019 04:32:58 +0000 (-0700)
Subject: axis-angle rotation matrix
X-Git-Url: https://git.owens.tech///git?a=commitdiff_plain;h=1ef37ec78bb5d6b3529386ff76022079b071bb0c;p=forks%2Fcmath3d.git

axis-angle rotation matrix
---

diff --git a/Makefile b/Makefile
index 37059ff..833525a 100644
--- 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
--- 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
--- 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,