coolfactor/lightscript
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
Gavan Fantom
2007-04-22 23:23:15 +00:00
commit 163fea6b89
33 changed files with 5627 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
src/lsi/Makefile Normal file
View File

@@ -0,0 +1,31 @@
# Makefile
PREFIX?= /usr/local
OBJS= main.o vm.o dmx.o midi.o beatdetect.o fft.o map3d.o mouse.o
SRCS= main.c vm.c dmx.c midi.c beatdetect.c fft.c map3d.c mouse.c
COMMONOBJS= mem.o hash.o
COMMONDIR= ../common
INCDIR= ../include
CFLAGS+= -Wall -Werror
CPPFLAGS+= -I${INCDIR}
LDLIBS+= -lm
PROGOBJS= ${OBJS} ${COMMONOBJS:S/^/${COMMONDIR}\//}
lsi: ${OBJS}
${LINK.c} -o ${.TARGET} ${PROGOBJS} ${LDLIBS}
install: lsi
${INSTALL} -d ${PREFIX}/bin
${INSTALL} -c lsi ${PREFIX}/bin/lsi
depend:
mkdep -- ${CFLAGS} ${CPPFLAGS} ${SRCS}
clean:
rm -f ${OBJS} lsi

377
src/lsi/beatdetect.c Normal file
View File

@@ -0,0 +1,377 @@
/* beatdetect.c */
#include <sys/types.h>
#include <sys/ioctl.h>
#include <sys/audioio.h>
#include <fcntl.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <stdint.h>
#include <unistd.h>
#include <err.h>
#include <string.h>
#include <errno.h>
#include "fft.h"
#include "vm.h"
#define AUDIO_DEVICE "/dev/sound"
#define BUFSIZE 2048
#define CLIP 1
#define HISTSIZE 43
#define NOUTPUTS 10
#define BAR 1.5
#define MAXP 44
#define MINP 22
#define CHISTSIZE 512
double outputslist[NOUTPUTS] = {
1, 3, 7, 15, 31, 63, 127, 255, 511, 1023
};
#if 0
/*
double profile[NOUTPUTS] = {
1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
};
*/
double profile[NOUTPUTS] = {
1.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0
};
#else
double profile[NOUTPUTS] = {
1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0
};
#endif
int audiofd;
#if 0
double lavg[NOUTPUTS];
double var[NOUTPUTS];
#endif
int lastmi = 0;
int lastmj = 0;
int lastmp = 0;
double lastmax = 0;
int minp, maxp;
double lastsum[NOUTPUTS];
double history[NOUTPUTS][HISTSIZE];
double chistory[NOUTPUTS][CHISTSIZE];
int histptr;
int chistptr;
char buffer[BUFSIZE];
char *bufptr;
int bufleft;
double phase;
double confidence;
int audio_initialised = 0;
#define HISTORY(i, x) (history[i][((histptr+(x) >= HISTSIZE) ? histptr+(x)-HISTSIZE : histptr+(x))])
#define HISTWRITE(i, x) history[i][histptr] = (x)
#define HISTNEXT do { \
histptr++; \
if (histptr >= HISTSIZE) \
histptr = 0; \
} while (0)
#define CHISTORY(i, x) (chistory[i][((chistptr+(x) >= CHISTSIZE) ? chistptr+(x)-CHISTSIZE : chistptr+(x))])
#define CHISTWRITE(i, x) chistory[i][chistptr] = (x)
#define CHISTNEXT do { \
chistptr++; \
if (chistptr >= CHISTSIZE) \
chistptr = 0; \
} while (0)
#define SUMSQ(a, b) ((a) * (a) + (b) * (b))
#define MAGSQ(i) SUMSQ(freqs[2*(i)], freqs[2*(i)+1])
void beatdetect_close(void)
{
close(audiofd);
}
double beatdetect_getphase(void)
{
return phase;
}
double beatdetect_getconfidence(void)
{
return confidence;
}
void beatdetect_init(void)
{
audio_info_t info, oinfo;
audiofd = open(AUDIO_DEVICE, O_RDONLY | O_NONBLOCK, 0);
if (audiofd < 0)
err(1, "failed to open audio device");
if (ioctl(audiofd, AUDIO_GETINFO, &oinfo) < 0)
err(1, "failed to get audio info");
AUDIO_INITINFO(&info);
info.record.sample_rate = 44100;
info.record.channels = 1;
info.record.precision = 16;
info.record.encoding = AUDIO_ENCODING_SLINEAR;
info.record.gain = oinfo.record.gain;
info.record.port = oinfo.record.port;
info.record.balance = oinfo.record.balance;
info.monitor_gain = oinfo.monitor_gain;
info.mode = AUMODE_RECORD;
if (ioctl(audiofd, AUDIO_SETINFO, &info) < 0)
err(1, "failed to set audio info");
fft_init(BUFSIZE/2);
bzero(history, sizeof(history));
bzero(chistory, sizeof(chistory));
histptr = 0;
chistptr = 0;
bufptr = buffer;
bufleft = BUFSIZE;
audio_initialised = 1;
/* vm_register_blah */
vm_register_signal_fd(audiofd, VM_BEATQ);
}
int beatdetect_read(void)
{
#if 0
double elem;
#endif
int i, j, n, p;
int rv;
int clip;
double localsum[NOUTPUTS];
fft_type *freqs;
int count;
int nitems;
double mean, variance, sd;
int beat;
double r[NOUTPUTS][MAXP][MAXP-MINP];
double max;
double cmax;
int mi, mj, mp;
int cmi, cmj, cmp;
if (!audio_initialised)
return 0;
while (1) {
rv = read(audiofd, bufptr, bufleft);
if (rv == -1) {
if (errno == EAGAIN) {
return 0;
}
printf("audio read failed\n");
audio_initialised = 0;
return 0;
}
if (rv == 0)
return 0;
bufptr += rv;
bufleft -= rv;
if (bufleft != 0) {
// printf("audio: short read (read %d)\n", rv);
if (bufleft < 0) {
printf("audio: oversize read\n");
bufptr = buffer;
bufleft = BUFSIZE;
}
return 0;
}
bufptr = buffer;
bufleft = BUFSIZE;
fft_data_signed16((int16_t *)buffer);
clip = 0;
/* Check for clip and compute rms */
for (i = 0; i < BUFSIZE/2; i++) {
int sample = ((int16_t *)buffer)[i];
if ((sample == INT16_MAX) || (sample == (-1 - INT16_MAX)))
clip = 1;
}
fft_window();
fft_compute();
freqs = fft_getresult();
n = 0;
for (i = 0; i < NOUTPUTS; i++) {
double output = 0;
for (j = 0; i+j < outputslist[n]; j++)
output += MAGSQ(i+j);
output = output / j;
localsum[i] = output;
HISTWRITE(i, output);
n++;
}
HISTNEXT;
#if 0
for (i = 0; i < NOUTPUTS; i++) {
lavg[i] = 0;
for (j = 0; j < HISTSIZE; j++) {
lavg[i] += HISTORY(i, j);
}
lavg[i] = lavg[i] / (double)HISTSIZE;
}
for (i = 0; i < NOUTPUTS; i++) {
var[i] = 0;
for (j = 0; j < HISTSIZE; j++) {
elem = HISTORY(i, j) - lavg[i];
var[i] += elem * elem;
}
var[i] = var[i] / (double)HISTSIZE;
}
#endif
#if CLIP
if (clip)
printf("C");
else
printf(" ");
#endif
for (i = 0; i < NOUTPUTS; i++) {
CHISTWRITE(i, localsum[i] - lastsum[i]);
lastsum[i] = localsum[i];
}
CHISTNEXT;
max = 0;
mi = 0;
mp = 0;
mj = 0;
mean = 0;
nitems = 0;
for (i = 0; i < NOUTPUTS; i++) {
for (p = MINP; p < MAXP; p++) {
for (j = 0; j < p; j++) {
r[i][j][p-MINP] =
(CHISTORY(i, CHISTSIZE-j-1) +
CHISTORY(i, CHISTSIZE-(p+j)-1) +
CHISTORY(i, CHISTSIZE-(2*p+j)-1) +
CHISTORY(i, CHISTSIZE-(3*p+j)-1) +
CHISTORY(i, CHISTSIZE-(4*p+j)-1) +
CHISTORY(i, CHISTSIZE-(5*p+j)-1) +
CHISTORY(i, CHISTSIZE-(6*p+j)-1) +
CHISTORY(i, CHISTSIZE-(7*p+j)-1))
* profile[i];
// printf("i j p = %d %d %d\n", i, j, p);
if (r[i][j][p-MINP] < 0)
r[i][j][p-MINP] = 0;
if (r[i][j][p-MINP] > max) {
max = r[i][j][p-MINP];
mi = i;
mp = p;
mj = j;
}
mean += r[i][j][p-MINP];
nitems++;
}
}
}
mean /= nitems;
cmax = 0;
cmi = 0;
cmp = 0;
cmj = 0;
minp = lastmp - 1;
if (minp < MINP)
minp = MINP;
maxp = lastmp + 1;
if (maxp >= MAXP)
maxp = MAXP-1;
for (i = 0; i < NOUTPUTS; i++) {
for (p = minp; p <= maxp; p++) {
for (j = lastmj; j <= lastmj+2; j++) {
int nj = j;
if (nj >= p)
nj -= p;
if (r[i][nj][p-MINP] > cmax) {
cmax = r[i][nj][p-MINP];
cmi = i;
cmj = nj;
cmp = p;
}
}
}
}
count = 0;
variance = 0;
for (i = 0; i < NOUTPUTS; i++)
for (p = MINP; p < MAXP; p++)
for (j = 0; j < p; j++) {
double val;
if (r[i][j][p-MINP] > max / 1.2)
count++;
val = r[i][j][p-MINP] - mean;
variance += val * val;
}
variance /= nitems;
sd = sqrt(variance);
if (cmax > (max / 1.2)) {
max = cmax;
mi = cmi;
mj = cmj;
mp = cmp;
}
phase = (((double)mj) / ((double)mp));
confidence = sd;
beat = 0;
if (sd > /*200*/ 10)
if (mj < lastmj) {
beat = 1;
}
lastmax = max;
lastmj = mj;
lastmi = mi;
lastmp = mp;
if (beat)
return 1;
}
}

7
src/lsi/beatdetect.h Normal file
View File

@@ -0,0 +1,7 @@
/* beatdetect.h */
void beatdetect_init(void);
void beatdetect_close(void);
double beatdetect_getphase(void);
double beatdetect_getconfidence(void);
int beatdetect_read(void);

129
src/lsi/dmx.c Normal file
View File

@@ -0,0 +1,129 @@
/* dmx.c */
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <err.h>
#include <termios.h>
#include <strings.h>
#include <assert.h>
#include "dmx.h"
#define PORT "/dev/ttyU0"
#define DMX_PACKETSIZE (DMX_UNIVERSESIZE + 6)
int dmxfd;
char dmxpacket[DMX_UNIVERSESIZE + DMX_PACKETSIZE];
char *dmxuniverse;
void dmx_open(void)
{
struct termios t;
int flags;
dmxfd = open(PORT, O_NONBLOCK | O_RDWR, 0);
if (dmxfd == -1) {
err(1, "failed to open DMX port");
}
flags = fcntl(dmxfd, F_GETFL);
fcntl(dmxfd, F_SETFL, flags & ~O_NONBLOCK);
tcgetattr(dmxfd, &t);
cfmakeraw(&t);
t.c_cflag = CLOCAL | CREAD | CS8;
tcsetattr(dmxfd, TCSANOW, &t);
}
void dmx_close(void)
{
close(dmxfd);
}
void dmx_dumpparams(void)
{
char buf[1024];
int more;
char *ptr;
int bytes;
int len;
buf[0] = 0x7e;
buf[1] = 3;
buf[2] = 2;
buf[3] = 0;
buf[4] = 0;
buf[5] = 0;
buf[6] = 0xe7;
if (write(dmxfd, buf, 7) != 7)
printf("didn't write 7 bytes\n");
more = 1;
ptr = buf;
bytes = 0;
len = 0;
while (more) {
if (read(dmxfd, ptr, 1) != 1)
goto out;
bytes++;
ptr++;
if (buf[0] != 0x7e) {
printf("Invalid packet received\n");
goto out;
}
if (bytes == 4)
len = buf[2] | (buf[3] << 8);
if (bytes == len + 5) {
if (buf[bytes-1] != (char)0xe7) {
printf("Invalid packet end\n");
goto out;
}
more = 0;
}
}
printf("Received packet:\n");
printf("%x %x %x %x %x %x %x %x %x %x %x %x\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7], buf[8], buf[9], buf[10], buf[11]);
out:
printf("buffer contents:\n");
printf("%x %x %x %x %x %x %x %x %x %x %x %x\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7], buf[8], buf[9], buf[10], buf[11]);
}
void dmx_init(void)
{
dmx_open();
dmxpacket[0] = 0x7e;
dmxpacket[1] = 6;
dmxpacket[2] = (DMX_UNIVERSESIZE+1) & 0xff;
dmxpacket[3] = ((DMX_UNIVERSESIZE+1) >> 8) & 0xff;
dmxpacket[4] = 0; /* start code */
bzero(dmxpacket+5, DMX_UNIVERSESIZE);
dmxpacket[DMX_UNIVERSESIZE+5] = 0xe7;
dmxuniverse = dmxpacket+5;
// dmx_dumpparams();
}
void dmx_setchannel(int channel, int value)
{
assert(channel < DMX_UNIVERSESIZE);
dmxuniverse[channel] = (char)value;
}
void dmx_output(void)
{
int off = 0;
// char *buf = dmxpacket;
// printf("%x %x %x %x %x %x %x %x %x %x %x %x\n", buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7], buf[8], buf[9], buf[10], buf[11]);
while (off < DMX_PACKETSIZE) {
int r;
r = write(dmxfd, dmxpacket + off, DMX_PACKETSIZE - off);
// printf("write returned %d\n", r);
if (r == -1)
err(1, "error writing packet");
off += r;
}
}

9
src/lsi/dmx.h Normal file
View File

@@ -0,0 +1,9 @@
/* dmx.h */
#define DMX_UNIVERSESIZE 512
void dmx_close(void);
void dmx_init(void);
void dmx_setchannel(int channel, int value);
void dmx_output(void);

138
src/lsi/fft.c Normal file
View File

@@ -0,0 +1,138 @@
/* fft.c */
#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <assert.h>
#include "mem.h"
#include "fft.h"
#define FFT_SIGN (-1)
int fft_size = 0;
fft_type *fft_buffer = NULL;
long fft_logN;
void fft_init(int size)
{
assert(fft_size == 0);
assert(fft_buffer == NULL);
/* We use complex pairs, so we need twice the number of elements */
fft_buffer = safe_malloc(sizeof(fft_type) * size * 2);
fft_size = size;
fft_logN = (long)(log(fft_size)/log(2.0)+0.5);
}
void fft_data_float(float *buf)
{
int i;
for (i = 0; i < fft_size; i++) {
fft_buffer[i*2] = buf[i];
fft_buffer[i*2+1] = 0.0;
}
}
void fft_data_signed16(int16_t *buf)
{
int i;
for (i = 0; i < fft_size; i++) {
fft_buffer[i*2] = (fft_type)buf[i] / (INT16_MAX+1);
fft_buffer[i*2+1] = 0.0;
// printf("input: %f, %f\n", fft_buffer[i*2], fft_buffer[i*2+1]);
}
}
void fft_data_unsigned16(uint16_t *buf)
{
int i;
for (i = 0; i < fft_size; i++) {
fft_buffer[i*2] = (fft_type)buf[i] / (INT16_MAX+1) -
(INT16_MAX+1);
fft_buffer[i*2+1] = 0.0;
}
}
fft_type *fft_getresult(void)
{
return fft_buffer;
}
void fft_window(void)
{
int i;
for (i = 0; i < fft_size; i++) {
fft_buffer[2*i] = fft_buffer[2*i] * (0.5 + 0.5 *
cos(M_PI * (i - fft_size/2)/(fft_size/2 + 1)));
}
}
/*
* FFT routine
* Based on routine (C) 1996 S.M.Bernsee.
*/
void fft_compute(void)
{
fft_type wr, wi, arg, *p1, *p2, temp;
fft_type tr, ti, ur, ui, *p1r, *p1i, *p2r, *p2i;
long i, bitm, j, le, le2, k;
for (i = 2; i < 2*fft_size-2; i += 2) {
for (bitm = 2, j = 0; bitm < 2*fft_size; bitm <<= 1) {
if (i & bitm) j++;
j <<= 1;
}
if (i < j) {
p1 = fft_buffer+i; p2 = fft_buffer+j;
temp = *p1; *(p1++) = *p2;
*(p2++) = temp; temp = *p1;
*p1 = *p2; *p2 = temp;
}
}
for (k = 0, le = 2; k < fft_logN; k++) {
le <<= 1;
le2 = le>>1;
ur = 1.0;
ui = 0.0;
arg = M_PI / (le2>>1);
wr = cos(arg);
wi = FFT_SIGN*sin(arg);
for (j = 0; j < le2; j += 2) {
p1r = fft_buffer+j; p1i = p1r+1;
p2r = p1r+le2; p2i = p2r+1;
for (i = j; i < 2*fft_size; i += le) {
tr = *p2r * ur - *p2i * ui;
ti = *p2r * ui + *p2i * ur;
*p2r = *p1r - tr; *p2i = *p1i - ti;
*p1r += tr; *p1i += ti;
p1r += le; p1i += le;
p2r += le; p2i += le;
}
tr = ur*wr - ui*wi;
ui = ur*wi + ui*wr;
ur = tr;
}
}
}

12
src/lsi/fft.h Normal file
View File

@@ -0,0 +1,12 @@
/* fft.h */
typedef float fft_type;
/* Initialise fft with the size of the data */
void fft_init(int);
void fft_data_float(float *);
void fft_data_signed16(int16_t *);
void fft_data_unsigned16(uint16_t *);
fft_type *fft_getresult(void);
void fft_window(void);
void fft_compute(void);

42
src/lsi/main.c Normal file
View File

@@ -0,0 +1,42 @@
/* main.c */
#include <signal.h>
#include <stdlib.h>
#include <err.h>
#include "vm.h"
#include "dmx.h"
#include "midi.h"
#include "beatdetect.h"
#include "mouse.h"
void finish(void)
{
dmx_close();
midi_close();
beatdetect_close();
exit(0);
}
void sigint_handler(int signal)
{
finish();
}
int main(int argc, char *argv[])
{
argv++;
argc--;
if (argc != 1)
errx(1, "Usage: lsi <filename>");
vm_init();
vm_load(argv[0]);
signal(SIGINT, sigint_handler);
midi_init();
dmx_init();
beatdetect_init();
mouse_init();
vm_spawn("main");
vm_run();
finish();
return 0;
}

645
src/lsi/map3d.c Normal file
View File

@@ -0,0 +1,645 @@
/* map3d.c */
#include <stdio.h>
#include <math.h>
#include <strings.h>
#include <fcntl.h>
#include <unistd.h>
#include "vm.h"
#define NLIGHTS 16
#define PANMAX 127
#define PANRANGE M_PI_2
#define PANOFFSET 128
#define TILTMAX 127
#define TILTRANGE M_PI_4
#define TILTOFFSET 128
#define MAP3D_FILENAME ".map3d.caldata"
struct light {
double M[4][3];
double cp[3][3];
double pan[3];
double tilt[3];
};
struct light map3d_cal[NLIGHTS];
#define MAG(x) (sqrt(SQUARE(x[0]) + SQUARE(x[1]) + SQUARE(x[2])))
#define SQUARE(x) ((x) * (x))
#define PYTHAG3(a, b) sqrt(SQUARE(a[0]-b[0]) + SQUARE(a[1]-b[1]) + SQUARE(a[2]-b[2]))
#define DEG(x) (180 * (x) / M_PI)
void normalise(double *v)
{
double mag;
int i;
mag = MAG(v);
for (i = 0; i < 3; i++)
v[i] /= mag;
}
#define MM map3d_cal[light].M
void multiply(int light, double *i, double *o) {
o[0] = i[0] * MM[0][0] + i[1] * MM[1][0] + i[2] * MM[2][0] + MM[3][0];
o[1] = i[0] * MM[0][1] + i[1] * MM[1][1] + i[2] * MM[2][1] + MM[3][1];
o[2] = i[0] * MM[0][2] + i[1] * MM[1][2] + i[2] * MM[2][2] + MM[3][2];
}
#undef MM
void map3d_init(void)
{
}
void map3d_close(void)
{
}
void map3d_save(void)
{
int fd, rv;
fd = open(MAP3D_FILENAME, O_WRONLY | O_CREAT, 0666);
rv = write(fd, map3d_cal, sizeof(map3d_cal));
if (rv != sizeof(map3d_cal))
printf("Warning: Calibration data not saved correctly\n");
close(fd);
}
int map3d_load(void)
{
int fd, rv;
fd = open(MAP3D_FILENAME, O_RDONLY, 0666);
if (!fd)
return 0;
rv = read(fd, map3d_cal, sizeof(map3d_cal));
if (rv != sizeof(map3d_cal))
printf("Warning: Calibration data not read correctly\n");
close(fd);
return 1;
}
void map3d_transform(int light, double x, double y, double z,
int *pan, int *tilt)
{
double pv[3];
double rv[3];
double p, t;
// printf("Transforming for light %d: (%f, %f, %f)\n", light, x, y, z);
fflush(stdout);
pv[0] = x;
pv[1] = y;
pv[2] = z;
multiply(light, pv, rv);
normalise(rv);
t = asin(rv[1]);
p = asin(rv[0]/cos(t));
*pan = (int)round((p * PANMAX)/PANRANGE) + PANOFFSET;
*tilt = (int)round((t * TILTMAX)/TILTRANGE) + TILTOFFSET;
if (*pan < 0)
*pan = 0;
if (*pan > 255)
*pan = 255;
if (*tilt < 0)
*tilt = 0;
if (*tilt > 255)
*tilt = 255;
// printf("pan = %d, tilt = %d\n", *pan, *tilt);
}
void map3d_setcal(int light, int n, double x, double y, double z,
int pan, int tilt)
{
printf("setcal(%d, %d, %f, %f, %f, %d, %d)\n", light, n, x, y, z, pan, tilt);
map3d_cal[light].cp[n][0] = x;
map3d_cal[light].cp[n][1] = y;
map3d_cal[light].cp[n][2] = z;
map3d_cal[light].pan[n] = PANRANGE * ((double)pan - PANOFFSET) / PANMAX;
map3d_cal[light].tilt[n] = TILTRANGE * ((double)tilt - TILTOFFSET)
/ TILTMAX;
printf("Setcal: pan = %f, tilt = %f\n", map3d_cal[light].pan[n], map3d_cal[light].tilt[n]);
}
void unitvector(double *vec, double pan, double tilt)
{
double tmp[3];
double cosp, sinp, cost, sint;
/* Precalculate some values */
cosp = cos(pan);
sinp = sin(pan);
cost = cos(tilt);
sint = sin(tilt);
/* Start with a unit vector */
vec[0] = 0;
vec[1] = 0;
vec[2] = 1;
/* Rotate around X axis (tilt) */
tmp[0] = vec[0];
tmp[1] = vec[1]*cost + vec[2]*sint;
tmp[2] = vec[2]*cost - vec[1]*sint;
/* Rotate around Y axis (pan) */
vec[0] = tmp[0]*cosp + tmp[2]*sinp;
vec[1] = tmp[1];
vec[2] = tmp[2]*cosp - tmp[0]*sinp;
}
double dotproduct(double *a, double *b)
{
return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}
void crossproduct(double *a, double *b, double *r)
{
r[0] = a[1]*b[2] - a[2]*b[1];
r[1] = a[2]*b[0] - a[0]*b[2];
r[2] = a[0]*b[1] - a[1]*b[0];
}
void dumpmatrix(double sim[12][13]) {
int i, j;
for (i = 0; i < 12; i++) {
for (j = 0; j < 13; j++)
printf("\t%f", sim[i][j]);
printf("\n");
}
printf("\n");
}
void eliminate(double sim[12][13], double *var)
{
int i, j, k, maxpos;
double max, val, tmp, x;
dumpmatrix(sim);
for (i = 0; i < 12; i++) {
/* Find the maximum element in the ith column */
max = 0.0;
maxpos = i;
for (j = i; j < 12; j++) {
val = fabs(sim[j][i]);
if (val > max) {
max = val;
maxpos = j;
}
}
if (maxpos != i)
for (j = 0; j < 13; j++) {
tmp = sim[maxpos][j];
sim[maxpos][j] = sim[i][j];
sim[i][j] = tmp;
}
if (fabs(sim[i][i]) < 0.0001)
printf("Warning: Lost accuracy at row %d\n", i);
for (j = i+1; j < 12; j++) {
if (fabs(sim[j][i]) > 0.0) {
/*
* Subtract x times row i from row j,
* where x is chosen such that
* sim[i][i] * x = sim[j][i]
*/
x = sim[j][i] / sim[i][i];
for (k = 0; k < 13; k++)
sim[j][k] -= x*sim[i][k];
}
}
}
dumpmatrix(sim);
/* Next, substitute in the unknowns. */
for (i = 12-1; i >= 0; i--) {
var[i] = sim[i][12];
for (j = i+1; j < 12; j++)
var[i] -= var[j] * sim[i][j];
var[i] /= sim[i][i];
}
}
/* In: Guess, length, angle, space for solutions */
void solvetriangle(double c, double a, double A, double *solp, double *soln)
{
double root;
double cosA;
double med;
cosA = cos(A);
med = 4*c*c*cosA*cosA - 4*(c*c-a*a);
root = sqrt(med);
if (isnan(root))
/* sqrt() of a negative number */
if (med > -0.000000001)
root = 0;
*solp = (2*c*cosA + root)/2;
*soln = (2*c*cosA - root)/2;
}
double try(int n, double dguess, double a, double b, double c, double A, double B, double C)
{
double s1, s2;
double e, f, d;
solvetriangle(dguess, a, A, &s1, &s2);
e = (n>=4?s2:s1);
n = n % 4;
solvetriangle(e, c, C, &s1, &s2);
f = (n>=2?s2:s1);
n = n % 2;
solvetriangle(f, b, B, &s1, &s2);
d = (n>=1?s2:s1);
return d;
}
void getsolution(int n, double dguess, double a, double b, double c, double A, double B, double C, double *e, double *f)
{
double s1, s2;
solvetriangle(dguess, a, A, &s1, &s2);
*e = (n>=4?s2:s1);
n = n % 4;
solvetriangle(*e, c, C, &s1, &s2);
*f = (n>=2?s2:s1);
}
#define DINCR 0.005
/*
* A is angle opposite a, between d and e
* B is angle opposite b, between d and f
* C is angle opposite c, between e and f
*/
int trysolvetetra(int n, double a, double b, double c,
double A, double B, double C,
double *dr, double *er, double *fr)
{
double dguess;
double thresh = 0.000000000001;
double d, lastd;
double mind, maxd;
double d1, d2, d3;
double e, f;
int found;
lastd = 0.0/0.0; /* NaN */
mind = maxd = 0; /* Shut up compiler */
/* Find a bracket */
found = 0;
for (dguess = 0.0; dguess < 100.0; dguess+=DINCR) {
d = try(n, dguess, a, b, c, A, B, C) - dguess;
if (((d >= 0) && (lastd < 0)) || ((d <= 0) && (lastd > 0))) {
mind = dguess - DINCR;
maxd = dguess;
printf("Found bracket: (%f, %f)\n", mind, maxd);
found = 1;
break;
}
lastd = d;
}
if (!found)
return 0;
while ((maxd-mind) > thresh) {
d1 = try(n, mind, a, b, c, A, B, C) - mind;
d2 = try(n, (mind+maxd)/2, a, b, c, A, B, C) - (mind+maxd)/2;
d3 = try(n, maxd, a, b, c, A, B, C) - maxd;
if (((d2 >= 0) && (d1 < 0)) || ((d2 < 0) && (d1 >= 0)))
maxd = (mind+maxd)/2;
else
mind = (mind+maxd)/2;
}
d = (mind+maxd)/2;
getsolution(n, d, a, b, c, A, B, C, &e, &f);
printf("Found solution: (%f, %f, %f)\n", d, e, f);
*dr = d;
*er = e;
*fr = f;
if (!isfinite(d))
return 0;
if (!isfinite(e))
return 0;
if (!isfinite(f))
return 0;
if (d < 0)
return 0;
if (e < 0)
return 0;
if (f < 0)
return 0;
return 1;
}
int solvetetra(double a, double b, double c,
double A, double B, double C,
double *dr, double *er, double *fr)
{
int i;
for (i = 0; i < 8; i++)
if (trysolvetetra(i, a, b, c, A, B, C, dr, er, fr))
return 1;
return 0;
}
int map3d_calibrate(int light)
{
double v1[3], v2[3], v3[3], p4[3];
double angle1, angle2, angle3;
double a, b, c, d, e, f;
double pAB[3], pBC[3];
double axis1[3], axis2[3], axis3[3];
double axm1, axm2, axm3;
double hypot, alpha, beta, gamma, delta;
double sim[12][13];
double var[12];
double pv[3], tv[3];
int success, i;
/*
* First, create unit vectors in the directions of the
* pan & tilt values given
*/
unitvector(v1, map3d_cal[light].pan[0], map3d_cal[light].tilt[0]);
unitvector(v2, map3d_cal[light].pan[1], map3d_cal[light].tilt[1]);
unitvector(v3, map3d_cal[light].pan[2], map3d_cal[light].tilt[2]);
/*
* Now, we need the angles between them
*/
angle1 = acos(dotproduct(v1, v2));
angle2 = acos(dotproduct(v2, v3));
angle3 = acos(dotproduct(v3, v1));
printf("angles (%f, %f, %f)\n", DEG(angle1), DEG(angle2), DEG(angle3));
/*
* And the lengths of the edges which we know
*/
a = PYTHAG3(map3d_cal[light].cp[0], map3d_cal[light].cp[1]);
b = PYTHAG3(map3d_cal[light].cp[1], map3d_cal[light].cp[2]);
c = PYTHAG3(map3d_cal[light].cp[2], map3d_cal[light].cp[0]);
/* Solve the tetrahedron */
success = solvetetra(a, b, c, angle1, angle2, angle3, &d, &e, &f);
if (!success) {
printf("failed to solve tetrahedron\n");
return 0;
}
/* Multiply the vectors by their magnitudes */
for (i = 0; i < 3; i++) {
v1[i] *= e;
v2[i] *= d;
v3[i] *= f;
}
/*
* Find two vectors to define the triangle between
* the calibration points
*/
for (i = 0; i < 3; i++) {
pAB[i] = map3d_cal[light].cp[1][i]-map3d_cal[light].cp[0][i];
pBC[i] = map3d_cal[light].cp[2][i]-map3d_cal[light].cp[1][i];
}
/*
* Create some perpendicular vectors in terms of which we can
* calculate a vector to the fourth point
*/
axis1[0] = pAB[0];
axis1[1] = pAB[1];
axis1[2] = pAB[2];
normalise(axis1);
crossproduct(axis1, pBC, axis2);
normalise(axis2);
crossproduct(axis1, axis2, axis3);
normalise(axis3);
/*
* Now we do some trigonometry to find out the distance to
* the fourth point in terms of the three axes
*/
beta = asin(d*sin(angle1)/a);
gamma = asin(f*sin(angle3)/c);
delta = acos(((a*a)+(c*c)-(b*b))/(2*a*c));
alpha = acos((cos(gamma)-cos(beta)*cos(delta))/(sin(beta)*sin(delta)));
hypot = e*sin(beta);
axm1 = e*cos(beta);
axm2 = hypot*sin(alpha);
axm3 = hypot*cos(alpha);
/* Now we have the magnitudes, let's get the vectors */
for (i = 0; i < 3; i++) {
axis1[i] *= axm1;
axis2[i] *= axm2;
axis3[i] *= axm3;
}
/*
* Now we can simply add these vectors to point A
* to get the fourth point
*/
for (i = 0; i < 3; i++)
p4[i] = map3d_cal[light].cp[0][i] + axis1[i] +
axis2[i] + axis3[i];
/*
* Now we can construct a matrix to represent 12 simultaneous
* equations, which we then solve to get our transformation matrix.
*/
bzero(sim, sizeof(sim));
/* First point */
sim[0][0] = map3d_cal[light].cp[0][0];
sim[0][1] = map3d_cal[light].cp[0][1];
sim[0][2] = map3d_cal[light].cp[0][2];
sim[0][9] = 1;
sim[0][12] = v1[0];
sim[1][3] = map3d_cal[light].cp[0][0];
sim[1][4] = map3d_cal[light].cp[0][1];
sim[1][5] = map3d_cal[light].cp[0][2];
sim[1][10] = 1;
sim[1][12] = v1[1];
sim[2][6] = map3d_cal[light].cp[0][0];
sim[2][7] = map3d_cal[light].cp[0][1];
sim[2][8] = map3d_cal[light].cp[0][2];
sim[2][11] = 1;
sim[2][12] = v1[2];
/* Second point */
sim[3][0] = map3d_cal[light].cp[1][0];
sim[3][1] = map3d_cal[light].cp[1][1];
sim[3][2] = map3d_cal[light].cp[1][2];
sim[3][9] = 1;
sim[3][12] = v2[0];
sim[4][3] = map3d_cal[light].cp[1][0];
sim[4][4] = map3d_cal[light].cp[1][1];
sim[4][5] = map3d_cal[light].cp[1][2];
sim[4][10] = 1;
sim[4][12] = v2[1];
sim[5][6] = map3d_cal[light].cp[1][0];
sim[5][7] = map3d_cal[light].cp[1][1];
sim[5][8] = map3d_cal[light].cp[1][2];
sim[5][11] = 1;
sim[5][12] = v2[2];
/* Third point */
sim[6][0] = map3d_cal[light].cp[2][0];
sim[6][1] = map3d_cal[light].cp[2][1];
sim[6][2] = map3d_cal[light].cp[2][2];
sim[6][9] = 1;
sim[6][12] = v3[0];
sim[7][3] = map3d_cal[light].cp[2][0];
sim[7][4] = map3d_cal[light].cp[2][1];
sim[7][5] = map3d_cal[light].cp[2][2];
sim[7][10] = 1;
sim[7][12] = v3[1];
sim[8][6] = map3d_cal[light].cp[2][0];
sim[8][7] = map3d_cal[light].cp[2][1];
sim[8][8] = map3d_cal[light].cp[2][2];
sim[8][11] = 1;
sim[8][12] = v3[2];
/* Fourth point */
sim[9][0] = p4[0];
sim[9][1] = p4[1];
sim[9][2] = p4[2];
sim[9][9] = 1;
sim[9][12] = 0;
sim[10][3] = p4[0];
sim[10][4] = p4[1];
sim[10][5] = p4[2];
sim[10][10] = 1;
sim[10][12] = 0;
sim[11][6] = p4[0];
sim[11][7] = p4[1];
sim[11][8] = p4[2];
sim[11][11] = 1;
sim[11][12] = 0;
eliminate(sim, var);
map3d_cal[light].M[0][0] = var[0];
map3d_cal[light].M[1][0] = var[1];
map3d_cal[light].M[2][0] = var[2];
map3d_cal[light].M[0][1] = var[3];
map3d_cal[light].M[1][1] = var[4];
map3d_cal[light].M[2][1] = var[5];
map3d_cal[light].M[0][2] = var[6];
map3d_cal[light].M[1][2] = var[7];
map3d_cal[light].M[2][2] = var[8];
map3d_cal[light].M[3][0] = var[9];
map3d_cal[light].M[3][1] = var[10];
map3d_cal[light].M[3][2] = var[11];
printf("%f\t%f\t%f\t\t%f\n", var[0], var[3], var[6], var[9]);
printf("%f\t%f\t%f\t\t%f\n", var[1], var[4], var[7], var[10]);
printf("%f\t%f\t%f\t\t%f\n", var[2], var[5], var[8], var[11]);
printf("Calibration points are:\n");
multiply(light, map3d_cal[light].cp[0], tv);
printf("(%f, %f, %f) => (%f, %f, %f)\n", map3d_cal[light].cp[0][0], map3d_cal[light].cp[0][1], map3d_cal[light].cp[0][2], tv[0], tv[1], tv[2]);
multiply(light, map3d_cal[light].cp[1], tv);
printf("(%f, %f, %f) => (%f, %f, %f)\n", map3d_cal[light].cp[1][0], map3d_cal[light].cp[1][1], map3d_cal[light].cp[1][2], tv[0], tv[1], tv[2]);
multiply(light, map3d_cal[light].cp[2], tv);
printf("(%f, %f, %f) => (%f, %f, %f)\n", map3d_cal[light].cp[2][0], map3d_cal[light].cp[2][1], map3d_cal[light].cp[2][2], tv[0], tv[1], tv[2]);
multiply(light, p4, tv);
printf("(%f, %f, %f) => (%f, %f, %f)\n", p4[0], p4[1], p4[2], tv[0], tv[1], tv[2]);
for (i = 0; i < 10; i++) {
pv[0] = 0;
pv[1] = (double)i/10.0;
pv[2] = 0;
multiply(light, pv, tv);
printf("(%f, %f, %f) => (%f, %f, %f)\n", pv[0], pv[1], pv[2], tv[0], tv[1], tv[2]);
}
return 1;
}
/* Set parameters based on position (x, y, z) and direction (tx, ty, tz) */
/* Temp: x, y = pan, tilt measured to (0, 0, 0)
tx, ty = pan, tilt of light fixture
tz = distance from light fixture to (0, 0, 0)
*/
void map3d_setparams(int light, int opan, int otilt, double lpan, double ltilt, double dist)
{
double n[3];
double up[3];
double right[3];
double v[3];
double op, ot;
op = PANRANGE * ((double)opan - PANOFFSET) / PANMAX;
ot = TILTRANGE * ((double)otilt - TILTOFFSET) / TILTMAX;
unitvector(v, op, ot);
unitvector(n, lpan, ltilt);
up[0] = 0;
up[1] = (n[1] + sqrt(n[1]*n[1]+4*n[2]*n[2])) /
(4*n[2]);
up[2] = -up[1]*n[1]/n[2];
normalise(up);
crossproduct(n, up, right);
normalise(right);
printf("n:\t%f\t%f\t%f\n", n[0], n[1], n[2]);
printf("up: \t%f\t%f\t%f\n", up[0], up[1], up[2]);
printf("right: \t%f\t%f\t%f\n", right[0], right[1], right[2]);
printf("\n");
/* Construct matrix */
map3d_cal[light].M[0][0] = right[0];
map3d_cal[light].M[0][1] = right[1];
map3d_cal[light].M[0][2] = right[2];
map3d_cal[light].M[1][0] = up[0];
map3d_cal[light].M[1][1] = up[1];
map3d_cal[light].M[1][2] = up[2];
map3d_cal[light].M[2][0] = n[0];
map3d_cal[light].M[2][1] = n[1];
map3d_cal[light].M[2][2] = n[2];
map3d_cal[light].M[3][0] = dist*v[0];
map3d_cal[light].M[3][1] = dist*v[1];
map3d_cal[light].M[3][2] = dist*v[2];
printf("\t%f\t%f\t%f\t\t%f\n", map3d_cal[light].M[0][0], map3d_cal[light].M[0][1], map3d_cal[light].M[0][2], map3d_cal[light].M[3][0]);
printf("\t%f\t%f\t%f\t\t%f\n", map3d_cal[light].M[1][0], map3d_cal[light].M[1][1], map3d_cal[light].M[1][2], map3d_cal[light].M[3][1]);
printf("\t%f\t%f\t%f\t\t%f\n", map3d_cal[light].M[2][0], map3d_cal[light].M[2][1], map3d_cal[light].M[2][2], map3d_cal[light].M[3][2]);
printf("\n");
}

11
src/lsi/map3d.h Normal file
View File

@@ -0,0 +1,11 @@
/* map3d.h */
void map3d_init(void);
void map3d_close(void);
int map3d_load(void);
void map3d_save(void);
void map3d_transform(int, double, double, double, int *, int *);
void map3d_setcal(int, int, double, double, double, int, int);
int map3d_calibrate(int);
void map3d_setparams(int, int, int, double, double, double);

101
src/lsi/midi.c Normal file
View File

@@ -0,0 +1,101 @@
/* midi.c */
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <err.h>
#include <sys/midiio.h>
#include <errno.h>
#include <sys/ioctl.h>
#include "vm.h"
#define PORT "/dev/music"
int midi_fd;
int midi_bytes;
seq_event_rec midi_buf;
int midi_initialised = 0;
void midi_init(void)
{
midi_fd = open(PORT, O_NONBLOCK | O_RDONLY, 0);
if (midi_fd == -1) {
err(1, "failed to open MIDI port");
}
midi_bytes = 0;
midi_initialised = 1;
vm_register_signal_fd(midi_fd, VM_MIDIQ);
}
void midi_close(void)
{
printf("Closing MIDI\n");
ioctl(midi_fd, SEQUENCER_SYNC, NULL);
ioctl(midi_fd, SEQUENCER_RESET, NULL);
printf("...\n");
close(midi_fd);
printf("X.\n");
midi_fd = 0;
midi_initialised = 0;
}
/*
* Returns non-zero if we're interested in the packet
* received. Zero otherwise.
*/
int midi_filter(void)
{
switch (midi_buf.arr[0]) {
case SEQ_CHN_COMMON:
/* We don't care about the unit number - yet */
/* We don't care about the channel - yet */
if (midi_buf.arr[2] == 0xb0) /* cmd */
return 1;
return 0;
case SEQ_TIMING:
return 0;
default:
printf("Unknown MIDI message received\n");
return 0;
}
}
int midi_read(void)
{
if (!midi_initialised)
return 0;
while (1) {
int rv;
int left = sizeof(midi_buf) - midi_bytes;
rv = read(midi_fd, midi_buf.arr + midi_bytes, left);
if (rv == -1) {
if (errno == EAGAIN)
return 0;
printf("Error reading from MIDI\n");
close(midi_fd);
midi_initialised = 0;
}
if (rv == 0)
return 0;
midi_bytes += rv;
if (midi_bytes == sizeof(midi_buf)) {
midi_bytes = 0;
if (midi_filter())
return 1;
}
}
}
/*
* Return the interesting bits of the MIDI command.
* The contents of this function is likely to change
* as we become interested in more types of MIDI device.
* Pre-condition: midi_filter() has returned 1
*/
void midi_getcmd(int *button, int *value)
{
*button = midi_buf.arr[4];
*value = midi_buf.arr[6];
}

7
src/lsi/midi.h Normal file
View File

@@ -0,0 +1,7 @@
/* midi.h */
void midi_init(void);
void midi_close(void);
int midi_filter(void);
int midi_read(void);
int midi_getcmd(int *, int *);

132
src/lsi/mouse.c Normal file
View File

@@ -0,0 +1,132 @@
/* mouse.c */
#include <sys/time.h>
#include <dev/wscons/wsconsio.h>
#include <fcntl.h>
#include <unistd.h>
#include <err.h>
#include <errno.h>
#include <stdio.h>
#include "vm.h"
#define MOUSEDEVICE "/dev/wsmouse"
char *events[] = {
"undefined",
"WSCONS_EVENT_KEY_UP",
"WSCONS_EVENT_KEY_DOWN",
"WSCONS_EVENT_ALL_KEYS_UP",
"WSCONS_EVENT_MOUSE_UP",
"WSCONS_EVENT_MOUSE_DOWN",
"WSCONS_EVENT_MOUSE_DELTA_X",
"WSCONS_EVENT_MOUSE_DELTA_Y",
"WSCONS_EVENT_MOUSE_ABSOLUTE_X",
"WSCONS_EVENT_MOUSE_ABSOLUTE_Y",
"WSCONS_EVENT_MOUSE_DELTA_Z",
"WSCONS_EVENT_MOUSE_ABSOLUTE_Z",
"WSCONS_EVENT_SCREEN_SWITCH",
"WSCONS_EVENT_ASCII",
"WSCONS_EVENT_MOUSE_DELTA_W",
"WSCONS_EVENT_MOUSE_ABSOLUTE_W"
};
#define WSCONS_EVENT_DELTA_X 6
#define WSCONS_EVENT_DELTA_Y 7
#define WSCONS_EVENT_DELTA_Z 10
#define MOUSE_SCALE 4
#define MOUSE_X_MIN (-127 * MOUSE_SCALE)
#define MOUSE_X_MAX (127 * MOUSE_SCALE)
#define MOUSE_Y_MIN (-127 * MOUSE_SCALE)
#define MOUSE_Y_MAX (127 * MOUSE_SCALE)
#define MOUSE_Z_MIN -127
#define MOUSE_Z_MAX 127
int mouse_fd;
int mouse_bytes;
struct wscons_event mouse_buf;
int mouse_initialised = 0;
int mouse_x, mouse_y, mouse_z;
void mouse_init(void)
{
mouse_fd = open(MOUSEDEVICE, O_RDONLY);
if (mouse_fd < 0)
err(1, "can't open mouse device");
vm_register_signal_fd(mouse_fd, VM_MOUSEQ);
mouse_initialised = 1;
mouse_x = 0;
mouse_y = 0;
mouse_z = 0;
}
void mouse_close(void)
{
close(mouse_fd);
mouse_initialised = 0;
}
int mouse_filter(void)
{
switch (mouse_buf.type) {
case WSCONS_EVENT_MOUSE_DELTA_X:
mouse_x += mouse_buf.value;
if (mouse_x > MOUSE_X_MAX)
mouse_x = MOUSE_X_MAX;
if (mouse_x < MOUSE_X_MIN)
mouse_x = MOUSE_X_MIN;
return 1;
case WSCONS_EVENT_MOUSE_DELTA_Y:
mouse_y += mouse_buf.value;
if (mouse_y > MOUSE_Y_MAX)
mouse_y = MOUSE_Y_MAX;
if (mouse_y < MOUSE_Y_MIN)
mouse_y = MOUSE_Y_MIN;
return 1;
case WSCONS_EVENT_MOUSE_DELTA_Z:
mouse_z += mouse_buf.value;
if (mouse_z > MOUSE_Z_MAX)
mouse_z = MOUSE_Z_MAX;
if (mouse_z < MOUSE_Z_MIN)
mouse_z = MOUSE_Z_MIN;
return 1;
}
return 0;
}
void mouse_getpos(int *x, int *y, int *z)
{
*x = mouse_x / MOUSE_SCALE;
*y = mouse_y / MOUSE_SCALE;
*z = mouse_z;
}
int mouse_read(void)
{
if (!mouse_initialised)
return 0;
while (1) {
int rv;
int left = sizeof(mouse_buf) - mouse_bytes;
rv = read(mouse_fd, ((char *)&mouse_buf) + mouse_bytes, left);
if (rv == -1) {
if (errno == EAGAIN)
return 0;
printf("Error reading from mouse\n");
close(mouse_fd);
mouse_initialised = 0;
}
if (rv == 0)
return 0;
mouse_bytes += rv;
if (mouse_bytes == sizeof(mouse_buf)) {
mouse_bytes = 0;
if (mouse_filter())
return 1;
}
}
}

7
src/lsi/mouse.h Normal file
View File

@@ -0,0 +1,7 @@
/* mouse.h */
void mouse_init(void);
void mouse_close(void);
int mouse_filter(void);
void mouse_getpos(int *, int *, int *);
int mouse_read(void);

1237
src/lsi/vm.c Normal file
View File

File diff suppressed because it is too large Load Diff

20
src/lsi/vm.h Normal file
View File

@@ -0,0 +1,20 @@
/* vm.h */
void vm_init(void);
void vm_load(char *);
int vm_spawn(char *);
void vm_run(void);
void vm_register_signal_fd(int /* fd */, int /* queue */);
#define VM_MAXQUEUES 512
#define VM_RUNQ 0
#define VM_TIMEQ 1
#define VM_MOUSEQ 6
#define VM_BEATQ 7
#define VM_MIDIQ 8
#define VM_SOCKQ 9
#define VM_USERQMIN 10
#define VM_NOQUEUE (-1)