--- /dev/null
+src = $(wildcard src/*.c) $(wildcard src/unix/*.c)
+obj = $(src:.c=.o)
+bin = smouse
+
+CFLAGS = -pedantic -Wall -g -Isrc
+LDFLAGS = -lGL -lGLU -lX11 -lm
+
+$(bin): $(obj)
+ $(CC) -o $@ $(obj) $(LDFLAGS)
+
+.PHONY: clean
+clean:
+ rm -f $(obj) $(bin)
--- /dev/null
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include "device.h"
+#include "serial.h"
+
+static int init(void);
+static void destroy(void);
+static int detect(void);
+static int start(void);
+static void stop(void);
+static int pending(void);
+static int getevent(device_event *ev);
+
+static int opendev(const char *dev);
+static int proc_packets(void);
+static void enqueue_event(device_event *ev);
+static int parse_motion(int *motv, const char *data);
+static int parse_keystate(unsigned int *st, const char *data);
+
+
+static int devfd = -1;
+static int bnstate;
+
+static device_event evq[16];
+static int evq_widx, evq_ridx;
+#define QNEXT(x) (((x) + 1) & 0xf)
+#define QEMPTY(b) (b##_ridx == b##_widx)
+
+
+struct device *device_magellan(void)
+{
+ static struct device dev = {
+ "magellan",
+ 0,
+ init,
+ destroy,
+ detect,
+ start,
+ stop,
+ pending,
+ getevent
+ };
+ return &dev;
+}
+
+
+static int init(void)
+{
+ return 0;
+}
+
+static void destroy(void)
+{
+}
+
+static int opendev(const char *dev)
+{
+ int fd, timeout;
+ char buf[128];
+
+ if((fd = ser_open(dev, 9600, SER_8N2 | SER_HWFLOW)) == -1) {
+ return -1;
+ }
+
+ /* try for about 5 sec */
+ timeout = 5000;
+ do {
+ ser_printf(fd, "z\r");
+ } while(!ser_wait(fd, 250) && (timeout -= 250) > 0);
+
+ if(timeout <= 0) {
+ fprintf(stderr, "magellan open(%s): device does not respond\n", dev);
+ ser_close(fd);
+ return -1; /* failed to get a response */
+ }
+
+ while(ser_getline(fd, buf, sizeof buf)) {
+ printf("magellan open(%s): %s\n", dev, buf);
+ }
+
+ ser_printf(fd, "vQ\r");
+ ser_wait(fd, 250);
+
+ while(ser_getline(fd, buf, sizeof buf)) {
+ if(buf[0] == 'v') {
+ break;
+ }
+ }
+ printf("magellan open(%s): got version string: \"%s\"\n", dev, buf + 1);
+ if(!strstr(buf, "v Magellan")) {
+ fprintf(stderr, "unknown device\n");
+ ser_close(fd);
+ return -1;
+ }
+ return fd;
+}
+
+static int detect(void)
+{
+ int fd;
+ if((fd = opendev(get_port())) >= 0) {
+ ser_close(fd);
+ return 1;
+ }
+ return 0;
+}
+
+static int start(void)
+{
+ if(devfd >= 0) {
+ fprintf(stderr, "magellan start: already started\n");
+ return -1;
+ }
+
+ if((devfd = opendev(get_port())) < 0) {
+ return -1;
+ }
+ ser_printf(devfd, "m3\r"); /* start motion packets */
+
+ return devfd;
+}
+
+static void stop(void)
+{
+ if(devfd < 0) {
+ fprintf(stderr, "magellan stop: not started\n");
+ return;
+ }
+
+ ser_close(devfd);
+ devfd = -1;
+}
+
+static int pending(void)
+{
+ if(devfd < 0) return 0;
+ return ser_pending(devfd);
+}
+
+static int getevent(device_event *ev)
+{
+ proc_packets();
+
+ if(!QEMPTY(evq)) {
+ *ev = evq[evq_ridx];
+ evq_ridx = QNEXT(evq_ridx);
+ return 1;
+ }
+ return 0;
+}
+
+static int proc_packets(void)
+{
+ int i, count = 0;
+ device_event *ev;
+ char buf[128];
+ unsigned int st, delta, prev;
+
+ if(devfd < 0) return -1;
+
+ while(ser_getline(devfd, buf, sizeof buf)) {
+
+ switch(buf[0]) {
+ case 'd':
+ ev = evq + evq_widx;
+ if(parse_motion(ev->motion.motion, buf + 1) == -1) {
+ break;
+ }
+ ev->type = DEV_EV_MOTION;
+ enqueue_event(ev);
+ ++count;
+ break;
+
+ case 'k':
+ if(parse_keystate(&st, buf + 1) == -1) {
+ break;
+ }
+
+ delta = st ^ bnstate;
+ prev = bnstate;
+ bnstate = st;
+
+ for(i=0; i<32; i++) {
+ if(delta & 1) {
+ ev = evq + evq_widx;
+ ev->type = DEV_EV_BUTTON;
+ ev->button.id = i;
+ ev->button.pressed = st & 1;
+ ev->button.state = prev ^ (1 << i);
+ enqueue_event(ev);
+ ++count;
+ }
+ st >>= 1;
+ }
+ break;
+ }
+ }
+ return count;
+}
+
+static void enqueue_event(device_event *ev)
+{
+ if(ev != evq + evq_widx) {
+ evq[evq_widx] = *ev;
+ }
+
+ evq_widx = QNEXT(evq_widx);
+ if(evq_widx == evq_ridx) {
+ /* overflowed, drop the oldest event */
+ evq_ridx = QNEXT(evq_ridx);
+ }
+}
+
+static int parse_motion(int *motv, const char *data)
+{
+ int i;
+ long val;
+
+ for(i=0; i<6; i++) {
+ if(!data[0] || !data[1] || !data[2] || !data[3]) {
+ return -1;
+ }
+ val = ((((long)data[0] & 0xf) << 12) |
+ (((long)data[1] & 0xf) << 8) |
+ (((long)data[2] & 0xf) << 4) |
+ ((long)data[3] & 0xf)) - 32768;
+ data += 4;
+ *motv++ = (int)val;
+ }
+ return 0;
+}
+
+static int parse_keystate(unsigned int *stptr, const char *data)
+{
+ int i, bit = 0;
+ unsigned int st = 0;
+
+ for(i=0; i<3; i++) {
+ if(!data) return -1;
+ st |= ((unsigned int)*data++ & 0xf) << bit;
+ bit += 4;
+ }
+ *stptr = st;
+ return 0;
+}
--- /dev/null
+#include <stdlib.h>
+#include <string.h>
+#include "device.h"
+
+struct device *device_magellan(void);
+
+static struct device *devlist;
+static char *port;
+
+void register_all(void)
+{
+ struct device *dev;
+
+ if((dev = device_magellan()) && dev->init() != -1) {
+ dev->next = devlist;
+ devlist = dev;
+ }
+}
+
+struct device *dev_init(const char *name)
+{
+ struct device *dev = devlist;
+
+ while(dev) {
+ if(name) {
+ if(strcmp(dev->name, name) == 0) {
+ return dev->detect() ? dev : 0;
+ }
+ } else {
+ if(dev->detect()) {
+ return dev;
+ }
+ }
+ dev = dev->next;
+ }
+ return 0;
+}
+
+void dev_destroy(void)
+{
+ while(devlist) {
+ struct device *dev = devlist;
+ devlist = devlist->next;
+ dev->destroy();
+ }
+ devlist = 0;
+}
+
+void set_port(const char *s)
+{
+ free(port);
+ port = strdup(s);
+}
+
+const char *get_port(void)
+{
+ return port;
+}
--- /dev/null
+#ifndef DEVICE_H_
+#define DEVICE_H_
+
+enum {
+ DEV_EV_NONE,
+ DEV_EV_MOTION,
+ DEV_EV_BUTTON
+};
+
+struct device_event_motion {
+ int type;
+ int motion[6];
+};
+
+struct device_event_button {
+ int type;
+ int id;
+ int pressed;
+ unsigned int state;
+};
+
+typedef union device_event {
+ int type;
+ struct device_event_motion motion;
+ struct device_event_button button;
+} device_event;
+
+struct device {
+ char *name;
+ struct device *next;
+
+ int (*init)(void);
+ void (*destroy)(void);
+
+ int (*detect)(void);
+
+ int (*start)(void);
+ void (*stop)(void);
+
+ int (*pending)(void);
+ int (*getevent)(device_event*);
+};
+
+void register_all(void);
+
+/* name 0 to auto-detect */
+struct device *dev_init(const char *name);
+void dev_destroy(void);
+
+void set_port(const char *s);
+const char *get_port(void);
+
+#endif /* DEVICE_H_ */
--- /dev/null
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <errno.h>
+#include <unistd.h>
+#include <sys/select.h>
+#include <X11/Xlib.h>
+#include <GL/gl.h>
+#include <GL/glu.h>
+#include <GL/glx.h>
+#include "vmath.h"
+#include "device.h"
+
+int create_gfx(int xsz, int ysz);
+void destroy_gfx(void);
+void set_window_title(const char *title);
+void redraw(void);
+void draw_cube(void);
+int handle_event(XEvent *xev);
+int handle_dev_event(device_event *ev);
+
+Display *dpy;
+Atom wm_prot, wm_del_win;
+GLXContext ctx;
+Window win;
+
+vec3_t pos = {0, 0, -6};
+quat_t rot = {0, 0, 0, 1}; /* that's 1 + 0i + 0j + 0k */
+
+int redisplay;
+
+/* serial 6dof device */
+struct device *dev;
+
+int main(void)
+{
+ int xsock, devfd, maxfd;
+
+ register_all();
+ set_port("/dev/ttyS0");
+
+ if(!(dev = dev_init(0))) {
+ fprintf(stderr, "failed to initialize 6dof serial device at /dev/ttyS0\n");
+ return 1;
+ }
+ if((devfd = dev->start()) == -1) {
+ return 1;
+ }
+
+ if(!(dpy = XOpenDisplay(0))) {
+ fprintf(stderr, "failed to connect to the X server\n");
+ return 1;
+ }
+ xsock = ConnectionNumber(dpy);
+
+ if(create_gfx(512, 512) == -1) {
+ return 1;
+ }
+
+ glEnable(GL_DEPTH_TEST);
+ glEnable(GL_CULL_FACE);
+
+ maxfd = xsock > devfd ? xsock : devfd;
+
+ for(;;) {
+ fd_set rdset;
+
+ FD_ZERO(&rdset);
+ FD_SET(xsock, &rdset);
+ FD_SET(devfd, &rdset);
+
+ while(select(maxfd + 1, &rdset, 0, 0, 0) == -1 && errno == EINTR);
+
+ if(FD_ISSET(xsock, &rdset)) {
+ while(XPending(dpy)) {
+ XEvent xev;
+ XNextEvent(dpy, &xev);
+
+ if(handle_event(&xev) != 0) {
+ goto end;
+ }
+ }
+ }
+
+ if(FD_ISSET(devfd, &rdset)) {
+ device_event ev;
+
+ while(dev->getevent(&ev)) {
+ handle_dev_event(&ev);
+ }
+ }
+
+ if(redisplay) {
+ redraw();
+ redisplay = 0;
+ }
+ }
+
+end:
+ destroy_gfx();
+ XCloseDisplay(dpy);
+ return 0;
+}
+
+int create_gfx(int xsz, int ysz)
+{
+ int scr;
+ Window root;
+ XVisualInfo *vis;
+ XSetWindowAttributes xattr;
+ unsigned int events;
+ XClassHint class_hint;
+
+ int attr[] = {
+ GLX_RGBA, GLX_DOUBLEBUFFER,
+ GLX_RED_SIZE, 8,
+ GLX_GREEN_SIZE, 8,
+ GLX_BLUE_SIZE, 8,
+ GLX_DEPTH_SIZE, 24,
+ None
+ };
+
+ wm_prot = XInternAtom(dpy, "WM_PROTOCOLS", False);
+ wm_del_win = XInternAtom(dpy, "WM_DELETE_WINDOW", False);
+
+ scr = DefaultScreen(dpy);
+ root = RootWindow(dpy, scr);
+
+ if(!(vis = glXChooseVisual(dpy, scr, attr))) {
+ fprintf(stderr, "requested GLX visual is not available\n");
+ return -1;
+ }
+
+ if(!(ctx = glXCreateContext(dpy, vis, 0, True))) {
+ fprintf(stderr, "failed to create GLX context\n");
+ XFree(vis);
+ return -1;
+ }
+
+ xattr.background_pixel = xattr.border_pixel = BlackPixel(dpy, scr);
+ xattr.colormap = XCreateColormap(dpy, root, vis->visual, AllocNone);
+
+ if(!(win = XCreateWindow(dpy, root, 0, 0, xsz, ysz, 0, vis->depth, InputOutput,
+ vis->visual, CWColormap | CWBackPixel | CWBorderPixel, &xattr))) {
+ fprintf(stderr, "failed to create X window\n");
+ return -1;
+ }
+ XFree(vis);
+
+ /* set the window event mask */
+ events = ExposureMask | StructureNotifyMask | KeyPressMask | KeyReleaseMask |
+ ButtonReleaseMask | ButtonPressMask | PointerMotionMask;
+ XSelectInput(dpy, win, events);
+
+ XSetWMProtocols(dpy, win, &wm_del_win, 1);
+
+ set_window_title("libspnav cube");
+
+ class_hint.res_name = "cube";
+ class_hint.res_class = "cube";
+ XSetClassHint(dpy, win, &class_hint);
+
+ if(glXMakeCurrent(dpy, win, ctx) == False) {
+ fprintf(stderr, "glXMakeCurrent failed\n");
+ glXDestroyContext(dpy, ctx);
+ XDestroyWindow(dpy, win);
+ return -1;
+ }
+
+ XMapWindow(dpy, win);
+ XFlush(dpy);
+
+ return 0;
+}
+
+void destroy_gfx(void)
+{
+ glXDestroyContext(dpy, ctx);
+ XDestroyWindow(dpy, win);
+ glXMakeCurrent(dpy, None, 0);
+}
+
+void set_window_title(const char *title)
+{
+ XTextProperty wm_name;
+
+ XStringListToTextProperty((char**)&title, 1, &wm_name);
+ XSetWMName(dpy, win, &wm_name);
+ XSetWMIconName(dpy, win, &wm_name);
+ XFree(wm_name.value);
+}
+
+void redraw(void)
+{
+ mat4_t xform;
+
+ quat_to_mat(xform, rot);
+
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+ glTranslatef(pos.x, pos.y, pos.z);
+ glMultTransposeMatrixf((float*)xform);
+
+ draw_cube();
+
+ glXSwapBuffers(dpy, win);
+}
+
+void draw_cube(void)
+{
+ glBegin(GL_QUADS);
+ /* face +Z */
+ glNormal3f(0, 0, 1);
+ glColor3f(1, 0, 0);
+ glVertex3f(-1, -1, 1);
+ glVertex3f(1, -1, 1);
+ glVertex3f(1, 1, 1);
+ glVertex3f(-1, 1, 1);
+ /* face +X */
+ glNormal3f(1, 0, 0);
+ glColor3f(0, 1, 0);
+ glVertex3f(1, -1, 1);
+ glVertex3f(1, -1, -1);
+ glVertex3f(1, 1, -1);
+ glVertex3f(1, 1, 1);
+ /* face -Z */
+ glNormal3f(0, 0, -1);
+ glColor3f(0, 0, 1);
+ glVertex3f(1, -1, -1);
+ glVertex3f(-1, -1, -1);
+ glVertex3f(-1, 1, -1);
+ glVertex3f(1, 1, -1);
+ /* face -X */
+ glNormal3f(-1, 0, 0);
+ glColor3f(1, 1, 0);
+ glVertex3f(-1, -1, -1);
+ glVertex3f(-1, -1, 1);
+ glVertex3f(-1, 1, 1);
+ glVertex3f(-1, 1, -1);
+ /* face +Y */
+ glNormal3f(0, 1, 0);
+ glColor3f(0, 1, 1);
+ glVertex3f(-1, 1, 1);
+ glVertex3f(1, 1, 1);
+ glVertex3f(1, 1, -1);
+ glVertex3f(-1, 1, -1);
+ /* face -Y */
+ glNormal3f(0, -1, 0);
+ glColor3f(1, 0, 1);
+ glVertex3f(-1, -1, -1);
+ glVertex3f(1, -1, -1);
+ glVertex3f(1, -1, 1);
+ glVertex3f(-1, -1, 1);
+ glEnd();
+}
+
+int handle_event(XEvent *xev)
+{
+ static int win_mapped;
+ KeySym sym;
+
+ switch(xev->type) {
+ case MapNotify:
+ win_mapped = 1;
+ break;
+
+ case UnmapNotify:
+ win_mapped = 0;
+ break;
+
+ case Expose:
+ if(win_mapped && xev->xexpose.count == 0) {
+ redraw();
+ }
+ break;
+
+ case KeyPress:
+ sym = XLookupKeysym((XKeyEvent*)&xev->xkey, 0);
+ if((sym & 0xff) == 27) {
+ return 1;
+ }
+ break;
+
+ case ConfigureNotify:
+ {
+ int x = xev->xconfigure.width;
+ int y = xev->xconfigure.height;
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ gluPerspective(45.0, (float)x / (float)y, 1.0, 1000.0);
+
+ glViewport(0, 0, x, y);
+ }
+ break;
+
+ default:
+ break;
+ }
+ return 0;
+}
+
+#define TX(ev) ((ev)->motion.motion[0])
+#define TY(ev) ((ev)->motion.motion[1])
+#define TZ(ev) ((ev)->motion.motion[2])
+#define RX(ev) ((ev)->motion.motion[3])
+#define RY(ev) ((ev)->motion.motion[4])
+#define RZ(ev) ((ev)->motion.motion[5])
+
+int handle_dev_event(device_event *ev)
+{
+ switch(ev->type) {
+ case DEV_EV_MOTION:
+ if(RX(ev) | RY(ev) | RZ(ev)) {
+ float axis_len = sqrt(RX(ev) * RX(ev) + RY(ev) * RY(ev) + RZ(ev) * RZ(ev));
+ if(axis_len != 0.0) {
+ rot = quat_rotate(rot, axis_len * 0.001, -RX(ev) / axis_len,
+ -RY(ev) / axis_len, RZ(ev) / axis_len);
+ }
+ }
+
+ pos.x += TX(ev) * 0.001;
+ pos.y += TY(ev) * 0.001;
+ pos.z -= TZ(ev) * 0.001;
+ redisplay = 1;
+ break;
+
+ case DEV_EV_BUTTON:
+ if(ev->button.pressed) {
+ pos = v3_cons(0, 0, -6);
+ rot = quat_cons(1, 0, 0, 0);
+ redisplay = 1;
+ }
+ break;
+ }
+
+ return 0;
+}
--- /dev/null
+#ifndef SERIAL_H_
+#define SERIAL_H_
+
+#define SER_8N1 0
+#define SER_8N2 1
+#define SER_HWFLOW 2
+
+int ser_open(const char *port, int baud, unsigned int mode);
+void ser_close(int fd);
+
+int ser_pending(int fd);
+/* if msec < 0: wait for ever */
+int ser_wait(int fd, long msec);
+
+int ser_write(int fd, const char *buf, int count);
+int ser_read(int fd, char *buf, int count);
+
+void ser_printf(int fd, const char *fmt, ...);
+char *ser_getline(int fd, char *buf, int bsz);
+
+#endif /* SERIAL_H_ */
--- /dev/null
+#include <stdio.h>
+#include <stdarg.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <sys/select.h>
+#include <sys/time.h>
+#include <fcntl.h>
+#include <termios.h>
+#include "serial.h"
+
+static int baud_id(int baud);
+
+int ser_open(const char *port, int baud, unsigned int mode)
+{
+ int fd;
+ struct termios term;
+
+ if((baud = baud_id(baud)) == -1) {
+ fprintf(stderr, "ser_open: invalid baud number: %d\n", baud);
+ return -1;
+ }
+
+ if((fd = open(port, O_RDWR | O_NONBLOCK | O_NOCTTY)) == -1) {
+ return -1;
+ }
+
+ /*memset(&term, 0, sizeof term);*/
+ tcgetattr(fd, &term);
+
+ term.c_oflag = 0;
+ term.c_lflag = 0;
+ term.c_cc[VMIN] = 0;
+ term.c_cc[VTIME] = 0;
+
+ term.c_cflag = CLOCAL | CREAD | CS8 | HUPCL | CRTSCTS;
+ if(mode & SER_8N2) {
+ term.c_cflag |= CSTOPB;
+ }
+
+ term.c_iflag = IGNBRK | IGNPAR;
+
+ cfsetispeed(&term, baud);
+ cfsetospeed(&term, baud);
+
+ if(tcsetattr(fd, TCSANOW, &term) < 0) {
+ fprintf(stderr, "ser_open: failed to set terminal attributes\n");
+ close(fd);
+ return -1;
+ }
+
+ if(mode & SER_HWFLOW) {
+ int st;
+ if(ioctl(fd, TIOCMGET, &st) == -1) {
+ perror("ser_open: failed to get modem status");
+ close(fd);
+ return -1;
+ }
+ st |= TIOCM_DTR | TIOCM_RTS;
+ if(ioctl(fd, TIOCMSET, &st) == -1) {
+ perror("ser_open: failed to set flow control");
+ close(fd);
+ return -1;
+ }
+ }
+
+ return fd;
+}
+
+void ser_close(int fd)
+{
+ close(fd);
+}
+
+int ser_pending(int fd)
+{
+ static struct timeval tv_zero;
+ fd_set rd;
+
+ FD_ZERO(&rd);
+ FD_SET(fd, &rd);
+
+ while(select(fd + 1, &rd, 0, 0, &tv_zero) == -1 && errno == EINTR);
+ return FD_ISSET(fd, &rd);
+}
+
+int ser_wait(int fd, long msec)
+{
+ struct timeval tv, tv0;
+ fd_set rd;
+
+ FD_ZERO(&rd);
+ FD_SET(fd, &rd);
+
+ tv.tv_sec = msec / 1000;
+ tv.tv_usec = msec * 1000;
+
+ gettimeofday(&tv0, 0);
+
+ while(select(fd + 1, &rd, 0, 0, msec >= 0 ? &tv : 0) == -1 && errno == EINTR) {
+ /* interrupted, recalc timeout and go back to sleep */
+ if(msec >= 0) {
+ gettimeofday(&tv, 0);
+ msec -= (tv.tv_sec - tv0.tv_sec) * 1000 + (tv.tv_usec - tv0.tv_usec) / 1000;
+ if(msec < 0) msec = 0;
+
+ tv.tv_sec = msec / 1000;
+ tv.tv_usec = msec * 1000;
+ }
+ }
+
+ return FD_ISSET(fd, &rd);
+}
+
+int ser_write(int fd, const char *buf, int count)
+{
+ return write(fd, buf, count);
+}
+
+int ser_read(int fd, char *buf, int count)
+{
+ return read(fd, buf, count);
+}
+
+void ser_printf(int fd, const char *fmt, ...)
+{
+ va_list ap;
+
+ va_start(ap, fmt);
+ vdprintf(fd, fmt, ap);
+ va_end(ap);
+}
+
+char *ser_getline(int fd, char *buf, int bsz)
+{
+ static char linebuf[512];
+ static int widx;
+ int i, rd, size;
+ char *ptr;
+
+ size = sizeof linebuf - widx;
+ while(size && (rd = read(fd, linebuf + widx, size)) > 0) {
+ widx += rd;
+ size -= rd;
+ }
+
+ ptr = linebuf;
+ for(i=0; i<widx; i++) {
+ if(*ptr == '\r' || *ptr == '\n') {
+ *ptr++ = '\n';
+ if(i < widx - 1 && ptr[1] == '\n') {
+ *ptr++ = 0;
+ }
+
+ size = widx >= bsz ? bsz - 1 : widx;
+ memcpy(buf, linebuf, size);
+ buf[size] = 0;
+
+ memmove(linebuf, linebuf + widx, sizeof linebuf - widx);
+ return buf;
+ } else {
+ ++ptr;
+ }
+ }
+ return 0;
+}
+
+static int baud_id(int baud)
+{
+ switch(baud) {
+ case 50: return B50;
+ case 75: return B75;
+ case 110: return B110;
+ case 134: return B134;
+ case 150: return B150;
+ case 200: return B200;
+ case 300: return B300;
+ case 600: return B600;
+ case 1200: return B1200;
+ case 1800: return B1800;
+ case 2400: return B2400;
+ case 4800: return B4800;
+ case 9600: return B9600;
+ case 19200: return B19200;
+ case 38400: return B38400;
+ case 57600: return B57600;
+ case 115200: return B115200;
+ default:
+ break;
+ }
+ return -1;
+}
--- /dev/null
+#include <math.h>
+#include "vmath.h"
+
+quat_t quat_rotate(quat_t q, float angle, float x, float y, float z)
+{
+ quat_t rq;
+ float half_angle = angle * 0.5;
+ float sin_half = sin(half_angle);
+
+ rq.w = cos(half_angle);
+ rq.x = x * sin_half;
+ rq.y = y * sin_half;
+ rq.z = z * sin_half;
+
+ return quat_mul(q, rq);
+}
--- /dev/null
+#ifndef VMATH_H_
+#define VMATH_H_
+
+typedef struct { float x, y, z; } vec3_t;
+typedef struct { float x, y, z, w; } vec4_t;
+
+typedef vec4_t quat_t;
+
+typedef float mat4_t[4][4];
+
+/* vector functions */
+static inline vec3_t v3_cons(float x, float y, float z);
+static inline float v3_dot(vec3_t v1, vec3_t v2);
+
+/* quaternion functions */
+static inline quat_t quat_cons(float s, float x, float y, float z);
+static inline vec3_t quat_vec(quat_t q);
+static inline quat_t quat_mul(quat_t q1, quat_t q2);
+static inline void quat_to_mat(mat4_t res, quat_t q);
+quat_t quat_rotate(quat_t q, float angle, float x, float y, float z);
+
+/* matrix functions */
+static inline void m4_cons(mat4_t m,
+ float m11, float m12, float m13, float m14,
+ float m21, float m22, float m23, float m24,
+ float m31, float m32, float m33, float m34,
+ float m41, float m42, float m43, float m44);
+
+#include "vmath.inl"
+
+#endif /* VMATH_H_ */
projects / smouse / commitdiff