coolfactor
/
lightscript
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Scripting and control environment for stage lighting
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
2 Commits
1 Branch
0 Tags
230 KiB
 
 
 
 
 
 
Tree: bafa106f11
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'bafa106f11'
${ noResults }
lightscript/src/lsi/map3d.c

645 lines
16 KiB
Raw Blame History

/* 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");
}