altos: Update quaternion tests to check vectors_to_rotation

Signed-off-by: Keith Packard <keithp@keithp.com>

1 files changed, 104 insertions, 17 deletions

diff --git a/src/test/ao_quaternion_test.c b/src/test/ao_quaternion_test.c
index 0e4b5b07..b630f1d3 100644
--- a/src/test/ao_quaternion_test.c
+++ b/src/test/ao_quaternion_test.c
@@ -27,41 +27,128 @@
 
 #include "ao_quaternion.h"
 
+#if 0
 static void
 print_q(char *name, struct ao_quaternion *q)
 {
 	printf ("%8.8s: r%8.5f x%8.5f y%8.5f z%8.5f ", name,
 		q->r, q->x, q->y, q->z);
 }
+#endif
+
+#define STEPS	16
+
+#define DEG	(1.0f * 3.1415926535f / 180.0f)
+
+struct ao_rotation {
+	int	steps;
+	float	x, y, z;
+};
+
+static struct ao_rotation ao_path[] = {
+	{ .steps = 45, .x =  2*DEG, .y = 0, .z = 0 },
+
+	{ .steps = 45, .x = 0, .y = 2*DEG, .z = 0 },
+	
+	{ .steps = 45, .x = -2*DEG, .y = 0, .z = 0 },
+
+	{ .steps = 45, .x = 0, .y = -2*DEG, .z = 0 },
+};
+
+#define NUM_PATH	(sizeof ao_path / sizeof ao_path[0])
+
+static int close(float a, float b) {
+	return fabsf (a - b) < 1e-5;
+}
+
+static int check_quaternion(char *where, struct ao_quaternion *got, struct ao_quaternion *expect) {
+	if (!close (got->r, expect->r) ||
+	    !close (got->x, expect->x) ||
+	    !close (got->y, expect->y) ||
+	    !close (got->z, expect->z))
+	{
+		printf ("%s: got r%8.5f x%8.5f y%8.5f z%8.5f expect r%8.5f x%8.5f y%8.5f z%8.5f\n",
+			where,
+			got->r, got->x, got->y, got->z,
+			expect->r, expect->x, expect->y, expect->z);
+		return 1;
+	}
+	return 0;
+}
 
 int main(int argc, char **argv)
 {
 	struct ao_quaternion	position;
+	struct ao_quaternion 	position_expect;
 	struct ao_quaternion	rotation;
+	struct ao_quaternion	rotated;
 	struct ao_quaternion	little_rotation;
 	int			i;
+	int			p;
+	int			ret = 0;
 
-	/* unit x vector */
-	ao_quaternion_init_vector(&position, 1, 0, 0);
+	/* vector */
+	ao_quaternion_init_vector(&position, 1, 1, 1);
+	ao_quaternion_init_vector(&position_expect, -1, -1, 1);
 
 	/* zero rotation */
 	ao_quaternion_init_zero_rotation(&rotation);
 
-	/* π/16 rotation around Z axis */
-	ao_quaternion_init_rotation(&little_rotation, 0, 0, 1,
-				    sin((M_PI/16)/2),
-				    cos((M_PI/16)/2));
-	for (i = 0; i <= 16; i++) {
-		struct ao_quaternion	rotated;
-
-		ao_quaternion_rotate(&rotated, &position, &rotation);
-		print_q("position", &position);
-		print_q("rotated", &rotated);
-		print_q("rotation", &rotation);
-		printf ("\n");
-		ao_quaternion_multiply(&rotation, &rotation, &little_rotation);
-		ao_quaternion_normalize(&rotation, &rotation);
+#define dump() do {							\
+									\
+		ao_quaternion_rotate(&rotated, &position, &rotation);	\
+		print_q("rotated", &rotated);				\
+		print_q("rotation", &rotation);				\
+		printf ("\n");						\
+	} while (0)
+
+//	dump();
+
+	for (p = 0; p < NUM_PATH; p++) {
+		ao_quaternion_init_half_euler(&little_rotation,
+					      ao_path[p].x / 2.0f,
+					      ao_path[p].y / 2.0f,
+					      ao_path[p].z / 2.0f);
+//		printf ("\t\tx: %8.4f, y: %8.4f, z: %8.4f ", ao_path[p].x, ao_path[p].y, ao_path[p].z);
+//		print_q("step", &little_rotation);
+//		printf("\n");
+		for (i = 0; i < ao_path[p].steps; i++) {
+			ao_quaternion_multiply(&rotation, &little_rotation, &rotation);
+
+			ao_quaternion_normalize(&rotation, &rotation);
+
+//			dump();
+		}
 	}
-	return 0;
+
+	ao_quaternion_rotate(&rotated, &position, &rotation);
+
+	ret += check_quaternion("rotation", &rotated, &position_expect);
+
+	struct ao_quaternion	vertical;
+	struct ao_quaternion	angle;
+	struct ao_quaternion	rot;
+
+	ao_quaternion_init_vector(&vertical, 0, 0, 1);
+	ao_quaternion_init_vector(&angle, 0, 0, 1);
+
+	ao_quaternion_init_half_euler(&rot,
+				      M_PI * 3.0 / 8.0 , 0, 0);
+
+	ao_quaternion_rotate(&angle, &angle, &rot);
+
+	struct ao_quaternion	rot_compute;
+
+	ao_quaternion_vectors_to_rotation(&rot_compute, &vertical, &angle);
+
+	ret += check_quaternion("vector rotation", &rot_compute, &rot);
+
+	struct ao_quaternion	rotd;
+
+	ao_quaternion_rotate(&rotd, &vertical, &rot_compute);
+
+	ret += check_quaternion("vector rotated", &rotd, &angle);
+
+	return ret;
 }