*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
#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;
}