#include <stdio.h>
#include <stdlib.h>
+#include <math.h>
#include <string.h>
#include <limits.h>
#include "demo.h"
#include "mouse.h"
#include "timer.h"
#include "gfx.h"
+#include "vmath.h"
+#include "sball.h"
+#include "cfgopt.h"
#include "logger.h"
+static int handle_sball_event(sball_event *ev);
+static void recalc_sball_matrix(float *xform);
+
static int quit;
static int use_mouse;
static long fbsize;
+static int use_sball;
+static vec3_t pos = {0, 0, 0};
+static quat_t rot = {0, 0, 0, 1};
+
int main(int argc, char **argv)
{
fbsize = fb_width * fb_height * fb_bpp / CHAR_BIT;
set_text_mode();
return 1;
}
+
+ if(opt.sball && sball_init() == 0) {
+ use_sball = 1;
+ }
+
reset_timer();
while(!quit) {
if(use_mouse) {
mouse_bmask = read_mouse(&mouse_x, &mouse_y);
}
+ if(use_sball && sball_pending()) {
+ sball_event ev;
+ printf("got sball event\n");
+ while(sball_getevent(&ev)) {
+ handle_sball_event(&ev);
+ }
+ recalc_sball_matrix(sball_matrix);
+ }
time_msec = get_msec();
demo_draw();
set_text_mode();
demo_cleanup();
kb_shutdown();
+ if(use_sball) {
+ sball_shutdown();
+ }
return 0;
}
drawFps(vmem_back);
}
}
+
+
+#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])
+
+static int handle_sball_event(sball_event *ev)
+{
+ switch(ev->type) {
+ case SBALL_EV_MOTION:
+ if(RX(ev) | RY(ev) | RZ(ev)) {
+ float rx = (float)RX(ev);
+ float ry = (float)RY(ev);
+ float rz = (float)RZ(ev);
+ float axis_len = sqrt(rx * rx + ry * ry + rz * rz);
+ if(axis_len > 0.0) {
+ rot = quat_rotate(rot, axis_len * 0.001, -rx / axis_len,
+ -ry / axis_len, -rz / axis_len);
+ }
+ }
+
+ pos.x += TX(ev) * 0.001;
+ pos.y += TY(ev) * 0.001;
+ pos.z += TZ(ev) * 0.001;
+ break;
+
+ case SBALL_EV_BUTTON:
+ if(ev->button.pressed) {
+ pos = v3_cons(0, 0, 0);
+ rot = quat_cons(1, 0, 0, 0);
+ }
+ break;
+ }
+
+ return 0;
+}
+
+void recalc_sball_matrix(float *xform)
+{
+ quat_to_mat(xform, rot);
+ xform[12] = pos.x;
+ xform[13] = pos.y;
+ xform[14] = pos.z;
+}
--- /dev/null
+#include <stdio.h>
+#include <dos.h>
+#include <conio.h>
+#include <i86.h>
+#include "sball.h"
+
+struct motion {
+ int x, y, z;
+ int rx, ry, rz;
+};
+
+#define UART1_BASE 0x3f8
+#define UART2_BASE 0x2f8
+#define UART1_IRQ 4
+#define UART2_IRQ 3
+
+#define UART_DATA 0
+#define UART_INTR 1
+#define UART_DIVLO 0
+#define UART_DIVHI 1
+#define UART_FIFO 2
+#define UART_IID 2
+#define UART_LCTL 3
+#define UART_MCTL 4
+#define UART_LSTAT 5
+#define UART_MSTAT 6
+
+/* interrupt enable register bits */
+#define INTR_RECV 1
+#define INTR_SEND 2
+#define INTR_LSTAT 4
+#define INTR_DELTA 8
+
+/* fifo control register bits */
+#define FIFO_ENABLE 0x01
+#define FIFO_RECV_CLEAR 0x02
+#define FIFO_SEND_CLEAR 0x04
+#define FIFO_DMA 0x08
+#define FIFO_TRIG_4 0x40
+#define FIFO_TRIG_8 0x80
+#define FIFO_TRIG_14 0xc0
+
+/* interrupt id register bits */
+#define IID_PENDING 0x01
+#define IID_ID0 0x02
+#define IID_ID1 0x04
+#define IID_ID2 0x08
+#define IID_FIFO_EN 0xc0
+
+#define IID_SOURCE 0xe
+
+#define IID_DELTA 0
+#define IID_SEND 0x2
+#define IID_RECV 0x4
+#define IID_FIFO 0xc
+#define IID_STATUS 0x6
+
+/* line control register bits */
+#define LCTL_BITS_8 0x03
+#define LCTL_STOP_2 0x04
+#define LCTL_DLAB 0x80
+#define LCTL_8N1 LCTL_BITS_8
+#define LCTL_8N2 (LCTL_BITS_8 | LCTL_STOP_2)
+
+/* modem control register bits */
+#define MCTL_DTR 0x01
+#define MCTL_RTS 0x02
+#define MCTL_OUT1 0x04
+#define MCTL_OUT2 0x08
+#define MCTL_LOOP 0x10
+
+/* line status register bits */
+#define LST_DRDY 0x01
+#define LST_ERR_OVER 0x02
+#define LST_ERR_PARITY 0x04
+#define LST_ERR_FRAME 0x08
+#define LST_ERR_BRK 0x10
+#define LST_TREG_EMPTY 0x20
+#define LST_TIDLE 0x40
+#define LST_ERROR 0x80
+
+/* modem status register bits */
+#define MST_DELTA_CTS 0x01
+#define MST_DELTA_DSR 0x02
+#define MST_TERI 0x04
+#define MST_DELTA_DCD 0x08
+#define MST_CTS 0x10
+#define MST_DSR 0x20
+#define MST_RING 0x40
+#define MST_DCD 0x80
+
+/* interrupt controller stuff */
+#define PIC1_CMD_PORT 0x20
+#define PIC1_DATA_PORT 0x21
+#define PIC2_CMD_PORT 0xa0
+#define PIC2_DATA_PORT 0xa1
+#define OCW2_EOI 0x20
+
+static int init_smouse(void);
+static void read_motion(int *m, const char *s);
+static void read_keystate(unsigned int *stptr, const char *s);
+static void procpkt(struct packet *p);
+static void enqueue_event(sball_event *ev);
+
+#define COM_FMT_8N1 LCTL_8N1
+#define COM_FMT_8N2 LCTL_8N2
+static void com_setup(int port, int baud, unsigned int fmt);
+
+static void com_putc(char c);
+static void com_puts(const char *s);
+static int com_getc(void);
+static char *com_gets(char *buf, int sz);
+
+static int com_have_recv(void);
+static int com_can_send(void);
+
+static void __interrupt __far recv_intr(void);
+
+static int uart_base, uart_intr_num;
+static void (__interrupt __far *prev_recv_intr)(void);
+
+static struct packet {
+ int id;
+ char data[80];
+} pktbuf[16];
+static int pktbuf_ridx, pktbuf_widx;
+#define BNEXT(x) (((x) + 1) & 0xf)
+#define BEMPTY(b) (b##_ridx == b##_widx)
+
+static sball_event evbuf[16];
+static int evbuf_ridx, evbuf_widx;
+
+
+int sball_init(void)
+{
+ com_setup(0, 9600, COM_FMT_8N2);
+ init_smouse();
+ return 0;
+}
+
+void sball_shutdown(void)
+{
+ com_close();
+}
+
+int sball_getdev(void)
+{
+ return 0;
+}
+
+int sball_pending(void)
+{
+ _disable();
+ while(!BEMPTY(pktbuf)) {
+ procpkt(pktbuf + pktbuf_ridx);
+ pktbuf_ridx = BNEXT(pktbuf_ridx);
+ }
+ _enable();
+ return !BEMPTY(evbuf);
+}
+
+int sball_getevent(sball_event *ev)
+{
+ _disable();
+ while(!BEMPTY(pktbuf)) {
+ procpkt(pktbuf + pktbuf_ridx);
+ pktbuf_ridx = BNEXT(pktbuf_ridx);
+ }
+ _enable();
+
+ if(BEMPTY(evbuf)) {
+ return 0;
+ }
+ *ev = evbuf[evbuf_ridx];
+ evbuf_ridx = BNEXT(evbuf_ridx);
+ return 1;
+}
+
+static int init_smouse(void)
+{
+ /* try repeatedly zeroing the device until we get a response */
+ do {
+ delay(500);
+ com_puts("z\r");
+ } while(BEMPTY(pktbuf));
+
+ /* then ask for id string and request motion updates */
+ com_puts("vQ\r");
+ com_puts("m3\r");
+ return 0;
+}
+
+static void procpkt(struct packet *p)
+{
+ static unsigned int bnstate;
+ int i;
+ unsigned int st, delta, prev;
+ sball_event *ev;
+
+ switch(p->id) {
+ case 'd':
+ ev = evbuf + evbuf_widx;
+ read_motion(ev->motion.motion, p->data);
+ ev->type = SBALL_EV_MOTION;
+ enqueue_event(ev);
+ break;
+
+ case 'k':
+ read_keystate(&st, p->data);
+
+ delta = st ^ bnstate;
+ prev = bnstate;
+ bnstate = st;
+
+ for(i=0; i<32; i++) {
+ if(delta & 1) {
+ ev = evbuf + evbuf_widx;
+ ev->type = SBALL_EV_BUTTON;
+ ev->button.id = i;
+ ev->button.pressed = st & 1;
+ ev->button.state = prev ^ (1 << i);
+ enqueue_event(ev);
+ }
+ st >>= 1;
+ delta >>= 1;
+ }
+ break;
+
+ case 'v':
+ printf("Device: %s\n", p->data);
+ break;
+ /*
+ default:
+ printf("DBG %c -> %s\n", (char)p->id, p->data);
+ */
+ }
+}
+
+static void enqueue_event(sball_event *ev)
+{
+ if(ev != evbuf + evbuf_widx) {
+ evbuf[evbuf_widx] = *ev;
+ }
+
+ evbuf_widx = BNEXT(evbuf_widx);
+ if(evbuf_widx == evbuf_ridx) {
+ fprintf(stderr, "enqueue_event: overflow, dropping oldest\n");
+ evbuf_ridx = BNEXT(evbuf_ridx);
+ }
+}
+
+static void com_setup(int port, int baud, unsigned int fmt)
+{
+ unsigned char ctl;
+ unsigned short div = 115200 / baud;
+ static int base[] = {UART1_BASE, UART2_BASE};
+ static int irq[] = {UART1_IRQ, UART2_IRQ};
+
+ uart_base = base[port];
+ uart_intr_num = irq[port] | 8;
+
+ _disable();
+ prev_recv_intr = _dos_getvect(uart_intr_num);
+ _dos_setvect(uart_intr_num, recv_intr);
+ /* unmask the appropriate interrupt */
+ outp(PIC1_DATA_PORT, inp(PIC1_DATA_PORT) & ~(1 << irq[port]));
+
+ outp(uart_base + UART_LCTL, LCTL_DLAB);
+ outp(uart_base + UART_DIVLO, div & 0xff);
+ outp(uart_base + UART_DIVHI, (div >> 8) & 0xff);
+ outp(uart_base + UART_LCTL, fmt); /* fmt should be LCTL_8N1, LCTL_8N2 etc */
+ outp(uart_base + UART_FIFO, FIFO_ENABLE | FIFO_SEND_CLEAR | FIFO_RECV_CLEAR);
+ outp(uart_base + UART_MCTL, MCTL_DTR | MCTL_RTS | MCTL_OUT2);
+ outp(uart_base + UART_INTR, INTR_RECV);
+
+ _enable();
+}
+
+static void com_close(void)
+{
+ _disable();
+ outp(uart_base + UART_INTR, 0);
+ outp(uart_base + UART_MCTL, 0);
+ _dos_setvect(uart_intr_num, prev_recv_intr);
+ _enable();
+}
+
+static void com_putc(char c)
+{
+ while(!com_can_send());
+ while((inp(uart_base + UART_MSTAT) & MST_CTS) == 0);
+ outp(uart_base + UART_DATA, c);
+}
+
+static void com_puts(const char *s)
+{
+ while(*s) {
+ com_putc(*s++);
+ }
+}
+
+static int com_getc(void)
+{
+ int have;
+ while(!(have = com_have_recv()));
+ return inp(uart_base + UART_DATA);
+}
+
+static char *com_gets(char *buf, int sz)
+{
+ int c;
+ char *ptr = buf;
+
+ while(sz-- > 1 && (c = com_getc()) != -1) {
+ if(c == '\r') {
+ *ptr++ = '\n';
+ break;
+ }
+ *ptr++ = c;
+ }
+ if(c == -1) {
+ return 0;
+ }
+ *ptr = 0;
+ return buf;
+}
+
+static int com_have_recv(void)
+{
+ unsigned short stat = inp(uart_base + UART_LSTAT);
+ if(stat & LST_ERROR) {
+ fprintf(stderr, "receive error\n");
+ abort();
+ }
+ return stat & LST_DRDY;
+}
+
+static int com_can_send(void)
+{
+ return inp(uart_base + UART_LSTAT) & LST_TREG_EMPTY;
+}
+
+static void __interrupt __far recv_intr()
+{
+ static char buf[128];
+ static char *bptr = buf;
+ struct packet *pkt;
+ int idreg, c, datasz;
+
+ while(((idreg = inp(uart_base + UART_IID)) & IID_PENDING) == 0) {
+ while(com_have_recv()) {
+ if((c = inp(uart_base + UART_DATA)) == '\r') {
+ *bptr = 0;
+ datasz = bptr - buf;
+ bptr = buf;
+
+ pkt = pktbuf + pktbuf_widx;
+ pktbuf_widx = BNEXT(pktbuf_widx);
+
+ if(pktbuf_widx == pktbuf_ridx) {
+ /* we overflowed, drop the oldest packet */
+ pktbuf_ridx = BNEXT(pktbuf_ridx);
+ }
+
+ if(datasz > sizeof pkt->data) {
+ datasz = sizeof pkt->data; /* truncate */
+ }
+ pkt->id = buf[0];
+ memcpy(pkt->data, buf + 1, datasz);
+
+ } else if(bptr - buf < sizeof buf - 1) {
+ *bptr++ = c;
+ }
+ }
+ }
+
+ outp(PIC1_CMD_PORT, OCW2_EOI);
+}
+
+static void read_motion(int *m, const char *s)
+{
+ int i;
+
+ for(i=0; i<6; i++) {
+ long val = ((((long)s[0] & 0xf) << 12) |
+ (((long)s[1] & 0xf) << 8) |
+ (((long)s[2] & 0xf) << 4) |
+ ((long)s[3] & 0xf)) - 32768;
+ s += 4;
+ *m++ = (int)val;
+ }
+}
+
+static void read_keystate(unsigned int *stptr, const char *s)
+{
+ int i, bit = 0;
+ unsigned int st = 0;
+
+ for(i=0; i<3; i++) {
+ st |= ((unsigned int)*s++ & 0xf) << bit;
+ bit += 4;
+ }
+ *stptr = st;
+}
--- /dev/null
+#ifndef VMATH_H_
+#define VMATH_H_
+
+#ifdef __GNUC__
+#define INLINE __inline
+
+#elif defined(__WATCOMC__)
+#define INLINE __inline
+
+#else
+#define INLINE
+#endif
+
+typedef struct { float x, y, z; } vec3_t;
+typedef struct { float x, y, z, w; } vec4_t;
+
+typedef vec4_t quat_t;
+
+/* vector functions */
+static INLINE vec3_t v3_cons(float x, float y, float z)
+{
+ vec3_t res;
+ res.x = x;
+ res.y = y;
+ res.z = z;
+ return res;
+}
+
+static INLINE float v3_dot(vec3_t v1, vec3_t v2)
+{
+ return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
+}
+
+/* quaternion functions */
+static INLINE quat_t quat_cons(float s, float x, float y, float z)
+{
+ quat_t q;
+ q.x = x;
+ q.y = y;
+ q.z = z;
+ q.w = s;
+ return q;
+}
+
+static INLINE vec3_t quat_vec(quat_t q)
+{
+ vec3_t v;
+ v.x = q.x;
+ v.y = q.y;
+ v.z = q.z;
+ return v;
+}
+
+static INLINE quat_t quat_mul(quat_t q1, quat_t q2)
+{
+ quat_t res;
+ vec3_t v1 = quat_vec(q1);
+ vec3_t v2 = quat_vec(q2);
+
+ res.w = q1.w * q2.w - v3_dot(v1, v2);
+ res.x = v2.x * q1.w + v1.x * q2.w + (v1.y * v2.z - v1.z * v2.y);
+ res.y = v2.y * q1.w + v1.y * q2.w + (v1.z * v2.x - v1.x * v2.z);
+ res.z = v2.z * q1.w + v1.z * q2.w + (v1.x * v2.y - v1.y * v2.x);
+ return res;
+}
+
+static INLINE void quat_to_mat(float *res, quat_t q)
+{
+ res[0] = 1.0f - 2.0f * q.y*q.y - 2.0f * q.z*q.z;
+ res[1] = 2.0f * q.x * q.y - 2.0f * q.w * q.z;
+ res[2] = 2.0f * q.z * q.x + 2.0f * q.w * q.y;
+ res[3] = 0.0f;
+ res[4] = 2.0f * q.x * q.y + 2.0f * q.w * q.z;
+ res[5] = 1.0f - 2.0f * q.x*q.x - 2.0f * q.z*q.z;
+ res[6] = 2.0f * q.y * q.z - 2.0f * q.w * q.x;
+ res[7] = 0.0f;
+ res[8] = 2.0f * q.z * q.x - 2.0f * q.w * q.y;
+ res[9] = 2.0f * q.y * q.z + 2.0f * q.w * q.x;
+ res[10] = 1.0f - 2.0f * q.x*q.x - 2.0f * q.y*q.y;
+ res[11] = 0.0f;
+ res[12] = res[13] = res[14] = 0.0f;
+ res[15] = 1.0f;
+}
+
+static INLINE 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);
+}
+
+#endif /* VMATH_H_ */