altos: Update quaternion tests to check vectors_to_rotation

[fw/altos] / src / test / ao_quaternion_test.c

diff --git a/src/test/ao_quaternion_test.c b/src/test/ao_quaternion_test.c
index 0e4b5b071fd01e443e42166c3f4ac4ce9909ae37..b630f1d315a110ba75e45a25218b8a2cfb1be562 100644 (file)
--- 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;
 }