git.gag.com Git - fw/altos/blob - src/test/ao_quaternion_test.c

   1 /*
   2  * Copyright © 2013 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; version 2 of the License.
   7  *
   8  * This program is distributed in the hope that it will be useful, but
   9  * WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11  * General Public License for more details.
  12  *
  13  * You should have received a copy of the GNU General Public License along
  14  * with this program; if not, write to the Free Software Foundation, Inc.,
  15  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  16  */
  17
  18 #define _GNU_SOURCE
  19
  20 #include <stdint.h>
  21 #include <stdio.h>
  22 #include <stdlib.h>
  23 #include <stddef.h>
  24 #include <string.h>
  25 #include <getopt.h>
  26 #include <math.h>
  27
  28 #include "ao_quaternion.h"
  29
  30 static void
  31 print_q(char *name, struct ao_quaternion *q)
  32 {
  33         printf ("%8.8s: r%8.5f x%8.5f y%8.5f z%8.5f ", name,
  34                 q->r, q->x, q->y, q->z);
  35 }
  36
  37 int main(int argc, char **argv)
  38 {
  39         struct ao_quaternion    position;
  40         struct ao_quaternion    rotation;
  41         struct ao_quaternion    little_rotation;
  42         int                     i;
  43
  44         /* unit x vector */
  45         ao_quaternion_init_vector(&position, 1, 0, 0);
  46
  47         /* zero rotation */
  48         ao_quaternion_init_zero_rotation(&rotation);
  49
  50         /* π/16 rotation around Z axis */
  51         ao_quaternion_init_rotation(&little_rotation, 0, 0, 1,
  52                                     sin((M_PI/16)/2),
  53                                     cos((M_PI/16)/2));
  54         for (i = 0; i <= 16; i++) {
  55                 struct ao_quaternion    rotated;
  56
  57                 ao_quaternion_rotate(&rotated, &position, &rotation);
  58                 print_q("position", &position);
  59                 print_q("rotated", &rotated);
  60                 print_q("rotation", &rotation);
  61                 printf ("\n");
  62                 ao_quaternion_multiply(&rotation, &rotation, &little_rotation);
  63                 ao_quaternion_normalize(&rotation, &rotation);
  64         }
  65         return 0;
  66 }
  67