git.mutantstargoat.com Git - smouse/blob - src/dos/main.c

   1 #include <stdio.h>
   2 #include <math.h>
   3 #include <conio.h>
   4 #include "serial.h"
   5 #include "device.h"
   6 #include "vmath.h"
   7
   8 void init_gfx(void);
   9 void destroy_gfx(void);
  10 int handle_dev_event(device_event *ev);
  11
  12 vec3_t pos = {0, 0, -6};
  13 quat_t rot = {0, 0, 0, 1};      /* that's 1 + 0i + 0j + 0k */
  14
  15 static struct device *dev;
  16
  17 int main(void)
  18 {
  19         int devfd;
  20
  21         register_all();
  22         set_port("COM1");
  23
  24         if(!(dev = dev_init(0))) {
  25                 fprintf(stderr, "failed to initialize 6dof serial device at COM1\n");
  26                 return 1;
  27         }
  28         if((devfd = dev->start()) == -1) {
  29                 return 1;
  30         }
  31
  32         init_gfx();
  33
  34         for(;;) {
  35                 device_event ev;
  36
  37                 if(kbhit() && getch() == 27) {
  38                         break;
  39                 }
  40
  41                 while(dev->getevent(&ev)) {
  42                         handle_dev_event(&ev);
  43                 }
  44
  45                 /* redisplay */
  46         }
  47
  48         dev->stop();
  49         dev_destroy();
  50         destroy_gfx();
  51         return 0;
  52 }
  53
  54 void init_gfx(void)
  55 {
  56 }
  57
  58 void destroy_gfx(void)
  59 {
  60 }
  61
  62 #define TX(ev)  ((ev)->motion.motion[0])
  63 #define TY(ev)  ((ev)->motion.motion[1])
  64 #define TZ(ev)  ((ev)->motion.motion[2])
  65 #define RX(ev)  ((ev)->motion.motion[3])
  66 #define RY(ev)  ((ev)->motion.motion[4])
  67 #define RZ(ev)  ((ev)->motion.motion[5])
  68
  69 int handle_dev_event(device_event *ev)
  70 {
  71         switch(ev->type) {
  72         case DEV_EV_MOTION:
  73                 printf("t: %5d %5d %5d - r: %5d %5d %5d\n", TX(ev), TY(ev), TZ(ev),
  74                                 RX(ev), RY(ev), RZ(ev));
  75                 if(RX(ev) | RY(ev) | RZ(ev)) {
  76                         float axis_len = sqrt(RX(ev) * RX(ev) + RY(ev) * RY(ev) + RZ(ev) * RZ(ev));
  77                         if(axis_len != 0.0) {
  78                                 rot = quat_rotate(rot, axis_len * 0.001, -RX(ev) / axis_len,
  79                                                 -RY(ev) / axis_len, -RZ(ev) / axis_len);
  80                         }
  81                 }
  82
  83                 pos.x += TX(ev) * 0.001;
  84                 pos.y += TY(ev) * 0.001;
  85                 pos.z += TZ(ev) * 0.001;
  86                 break;
  87
  88         case DEV_EV_BUTTON:
  89                 if(ev->button.pressed) {
  90                         pos = v3_cons(0, 0, -6);
  91                         rot = quat_cons(1, 0, 0, 0);
  92                 }
  93                 break;
  94         }
  95
  96         return 0;
  97 }