VECTOR3D vector3d_rotation(VECTOR3D v, const QUATERNION *q)
{
QUATERNION qv, qr;
VECTOR3D vr;
qv.w = 0.0;
qv.x = v.x;
qv.y = v.y;
qv.z = v.z;
qr = quaternion_mult(quaternion_mult(*q, qv), quaternion_conjugate(*q));
vr.x = qr.x;
vr.y = qr.y;
vr.z = qr.z;
return (vr);
}
static int turtle_bank(TURTLE_STATE *ts, double angle)
{
VECTOR3D axis = { 1.0, 0.0, 0.0 };
if (ts == NULL)
return (-1);
ts->orientation = quaternion_normalized(quaternion_mult(rotation_quaternion(-angle * M_PI / 180.0, &axis), ts->orientation));
return (0);
}
static void ltgl_motion(int x, int y)
{
int dx, dy;
VECTOR3D axis = { 0.0, 0.0, 0.0 };
if (mouse_block.state == GLUT_DOWN)
{
dx = x - mouse_block.x;
dy = y - mouse_block.y;
mouse_block.x = x;
mouse_block.y = y;
if (mouse_block.button == GLUT_LEFT_BUTTON)
{
if ((mouse_block.modifiers & GLUT_ACTIVE_CTRL))
render_parameters.view_position.z -= dy * 0.1;
else if ((mouse_block.modifiers & GLUT_ACTIVE_SHIFT))
render_parameters.view_position.y += dy * 0.1;
else
render_parameters.view_position.x -= dx * 0.1;
}
if (mouse_block.button == GLUT_RIGHT_BUTTON)
{
if ((mouse_block.modifiers & GLUT_ACTIVE_CTRL))
{
axis.z = 1.0;
render_parameters.model_orientation = quaternion_normalized(quaternion_mult(rotation_quaternion(-dx / 180.0 * M_PI, &axis), render_parameters.model_orientation));
}
else if ((mouse_block.modifiers & GLUT_ACTIVE_SHIFT))
{
axis.y = 1.0;
render_parameters.model_orientation = quaternion_normalized(quaternion_mult(rotation_quaternion(dx / 180.0 * M_PI, &axis), render_parameters.model_orientation));
}
else
{
axis.x = 1.0;
render_parameters.model_orientation = quaternion_normalized(quaternion_mult(rotation_quaternion(-dy / 180.0 * M_PI, &axis), render_parameters.model_orientation));
}
}
glutPostRedisplay();
}
/* fprintf(stderr, "motion: %d %d\n", x, y); */
}
static int rotate_model(RENDER_PARAMETERS *rp, int axis_no, double angle)
{
VECTOR3D axis = { 0.0, 0.0, 0.0 };
switch(axis_no)
{
case 1:
axis.x = 1.0;
break;
case 2:
axis.y = 1.0;
break;
case 3:
axis.z = 1.0;
break;
}
rp->model_orientation = quaternion_normalized(quaternion_mult(rotation_quaternion(angle / 180.0 * M_PI, &axis), render_parameters.model_orientation));
return (0);
}
void test_bingham_mixture_sample(int argc, char *argv[])
{
if (argc < 3) {
printf("usage: %s <bmx_file> <n> <outfile>\n", argv[0]);
return;
}
int i, j, k, d = 5;//4th column for component
bingham_mix_t *bmx = load_bmx(argv[1], &k);
int n = atoi(argv[2]);
double **X = new_matrix2(n, d);
bingham_mixture_sample(X, &bmx[0], n);
// Fit a BMM to sampled points from a BMM => BinghamCeption!?
// This is just printed in terminal. Doesn't affect anything
bingham_mix_t BM;
bingham_cluster(&BM, X, n, d);
for (i = 0; i < BM.n; i++) {
print_bingham(&BM.B[i]);
printf("---------------------------\n");
}
for (i = 0; i < n; i++) {
for (j = 0; j < d; j++)
printf("%.4f, ", X[i][j]);
printf("\n");
FILE *fp = fopen(argv[3], "w");
for (i = 0; i < n; i++) {
for (j = 0; j < d; j++)//d+1 is for the component index
fprintf(fp, "%.4f, ", X[i][j]);
fprintf(fp, "\n");
}
}
void test_bingham_sample(int argc, char *argv[])
{
if (argc < 5) {
printf("usage: %s <z1> <z2> <z3> <num_samples>\n", argv[0]);
exit(1);
}
double z1 = atof(argv[1]);
double z2 = atof(argv[2]);
double z3 = atof(argv[3]);
int nsamples = atoi(argv[4]);
double Z[3] = {z1, z2, z3};
double V[3][4] = {{1,0,0,0}, {0,1,0,0}, {0,0,1,0}};
double *Vp[3] = {&V[0][0], &V[1][0], &V[2][0]};
bingham_t B;
bingham_new(&B, 4, Vp, Z);
print_bingham(&B);
printf("---------------------------\n");
bingham_stats(&B);
printf("Original scatter matrix:\n");
int i, j, d=4;
for (i = 0; i < d; i++) {
for (j = 0; j < d; j++)
printf("%.4f, ", B.stats->scatter[i][j]);
printf("\n");
}
double t0 = get_time_ms();
double **X = new_matrix2(nsamples, 4);
bingham_sample(X, &B, nsamples);
printf("Sampled %d points in %.0f ms\n", nsamples, get_time_ms() - t0);
bingham_fit(&B, X, nsamples, 4);
print_bingham(&B);
int n = nsamples;
double **Xt = new_matrix2(d, n);
transpose(Xt, X, n, d);
double **S = new_matrix2(d, d);
matrix_mult(S, Xt, X, d, n, d);
mult(S[0], S[0], 1/(double)n, d*d);
printf("Sample scatter matrix:\n");
for (i = 0; i < d; i++) {
for (j = 0; j < d; j++)
printf("%.4f, ", S[i][j]);
printf("\n");
}
}
void test_bingham_sample_pmf(int argc, char *argv[])
{
if (argc < 6) {
printf("usage: %s <z1> <z2> <z3> <num_cells> <num_samples>\n", argv[0]);
exit(1);
}
double z1 = atof(argv[1]);
double z2 = atof(argv[2]);
double z3 = atof(argv[3]);
int ncells = atoi(argv[4]);
int nsamples = atoi(argv[5]);
double Z[3] = {z1, z2, z3};
double V[3][4] = {{1,0,0,0}, {0,1,0,0}, {0,0,1,0}};
double *Vp[3] = {&V[0][0], &V[1][0], &V[2][0]};
bingham_t B;
bingham_new(&B, 4, Vp, Z);
bingham_pmf_t pmf;
double t0 = get_time_ms();
bingham_discretize(&pmf, &B, ncells);
printf("Created PMF with %d cells in %.0f ms\n", pmf.n, get_time_ms() - t0);
t0 = get_time_ms();
double **X = new_matrix2(nsamples, 4);
bingham_sample_pmf(X, &pmf, nsamples);
printf("Sampled %d points in %.0f ms\n", nsamples, get_time_ms() - t0);
bingham_fit(&B, X, nsamples, 4);
print_bingham(&B);
}
void test_bingham_compose(int argc, char *argv[])
{
int i, j, d = 4;
if (argc < 8) {
printf("usage: %s <z11> <z12> <z13> <z21> <z22> <z23> <num_samples>\n", argv[0]);
exit(1);
}
double z11 = atof(argv[1]);
double z12 = atof(argv[2]);
double z13 = atof(argv[3]);
double z21 = atof(argv[4]);
double z22 = atof(argv[5]);
double z23 = atof(argv[6]);
int nsamples = atoi(argv[7]);
bingham_t B1;
double Z1[3] = {z11, z12, z13};
double V1[3][4] = {{0,1,0,0}, {0,0,1,0}, {0,0,0,1}};
double *Vp1[3] = {&V1[0][0], &V1[1][0], &V1[2][0]};
bingham_new(&B1, 4, Vp1, Z1);
bingham_stats(&B1);
print_bingham(&B1);
printf("S1:\n");
for (i = 0; i < d; i++) {
for (j = 0; j < d; j++)
printf("%.4f, ", B1.stats->scatter[i][j]);
printf("\n");
}
printf("---------------------------\n");
bingham_t B2;
double Z2[3] = {z21, z22, z23};
double V2[3][4] = {{0,1,0,0}, {0,0,1,0}, {0,0,0,1}}; //{{1,0,0,0}, {0,1,0,0}, {0,0,1,0}};
double *Vp2[3] = {&V2[0][0], &V2[1][0], &V2[2][0]};
bingham_new(&B2, 4, Vp2, Z2);
bingham_stats(&B2);
print_bingham(&B2);
printf("S2:\n");
for (i = 0; i < d; i++) {
for (j = 0; j < d; j++)
printf("%.4f, ", B2.stats->scatter[i][j]);
printf("\n");
}
printf("---------------------------\n");
// compose with method of moments
//double **S_mom = new_matrix2(4,4);
bingham_t B_mom;
double t0 = get_time_ms();
for (i = 0; i < nsamples; i++)
bingham_compose(&B_mom, &B1, &B2);
printf("Composed %d Bingham pairs with Method-of-Moments in %.0f ms\n", nsamples, get_time_ms() - t0);
// compose with sampling
t0 = get_time_ms();
double **X1 = new_matrix2(nsamples, 4);
bingham_sample(X1, &B1, nsamples);
double **X2 = new_matrix2(nsamples, 4);
bingham_sample(X2, &B2, nsamples);
double **Y = new_matrix2(nsamples, 4);
for (i = 0; i < nsamples; i++)
quaternion_mult(Y[i], X1[i], X2[i]);
int n = nsamples;
double **Yt = new_matrix2(d, n);
transpose(Yt, Y, n, d);
double **S_sam = new_matrix2(d, d);
matrix_mult(S_sam, Yt, Y, d, n, d);
mult(S_sam[0], S_sam[0], 1/(double)n, d*d);
printf("Composed 1 Bingham pair with %d samples in %.0f ms\n", nsamples, get_time_ms() - t0);
printf("---------------------------\n");
printf("Method-of-Moments scatter matrix:\n");
bingham_stats(&B_mom);
for (i = 0; i < d; i++) {
for (j = 0; j < d; j++)
printf("%.4f, ", B_mom.stats->scatter[i][j]);
printf("\n");
}
printf("---------------------------\n");
printf("Sample scatter matrix:\n");
for (i = 0; i < d; i++) {
for (j = 0; j < d; j++)
printf("%.4f, ", S_sam[i][j]);
printf("\n");
}
printf("\n---------------------------\n\n");
t0 = get_time_ms();
for (i = 0; i < nsamples; i++)
bingham_compose_true_pdf(X1[i], &B1, &B2);
printf("Computed %d true composition pdf's in %.0f ms\n", nsamples, get_time_ms() - t0);
// compute sample mean error
double tot_err = 0;
for (i = 0; i < nsamples; i++) {
double f_true = bingham_compose_true_pdf(X1[i], &B1, &B2);
double f_approx = bingham_pdf(X1[i], &B_mom);
//printf("f_true = %f, f_approx = %f\n", f_true, f_approx);
tot_err += 2*fabs((f_true - f_approx) / (f_true + f_approx));
}
printf("mean sample err = %.2f%%\n", 100*tot_err/nsamples);
// compute KL divergence
printf("KL divergence = %f\n", bingham_compose_error(&B1, &B2));
}
void test_bingham_KL_divergence(int argc, char *argv[])
{
if (argc < 8) {
printf("usage: %s <z11> <z12> <z13> <z21> <z22> <z23> <theta>\n", argv[0]);
exit(1);
}
double z11 = atof(argv[1]);
double z12 = atof(argv[2]);
double z13 = atof(argv[3]);
double z21 = atof(argv[4]);
double z22 = atof(argv[5]);
double z23 = atof(argv[6]);
double theta = atof(argv[7]);
double Z1[3] = {z11, z12, z13};
double V1[3][4] = {{1,0,0,0}, {0,1,0,0}, {0,0,1,0}};
double *Vp1[3] = {&V1[0][0], &V1[1][0], &V1[2][0]};
bingham_t B1;
B1.d = 4;
B1.Z = Z1;
B1.V = Vp1;
B1.F = bingham_F_lookup_3d(Z1);
//bingham_new(&B1, 4, Vp1, Z1);
double Z2[3] = {z21, z22, z23};
//double V2[3][4] = {{0,1,0,0}, {0,0,1,0}, {0,0,0,1}};
double V2[3][4] = {{1,0,0,0}, {0,1,0,0}, {0,0,1,0}};
double *Vp2[3] = {&V2[0][0], &V2[1][0], &V2[2][0]};
bingham_t B2;
B2.d = 4;
B2.Z = Z2;
B2.V = Vp2;
B2.F = bingham_F_lookup_3d(Z2);
//bingham_new(&B2, 4, Vp2, Z2);
// rotate B2 by theta about (1,0,0,0)
double q[4] = {cos(theta/2.0), sin(theta/2.0), 0, 0};
quaternion_mult(B2.V[0], q, B2.V[0]);
quaternion_mult(B2.V[1], q, B2.V[1]);
quaternion_mult(B2.V[2], q, B2.V[2]);
printf("B2:\n");
print_bingham(&B2);
printf("\n");
bingham_stats(&B1);
bingham_stats(&B2);
int i, j;
printf("\n");
printf("B1.stats->mode = [ %f %f %f %f ]\n", B1.stats->mode[0], B1.stats->mode[1], B1.stats->mode[2], B1.stats->mode[3]);
printf("B1.stats->dF = [ %f %f %f ]\n", B1.stats->dF[0], B1.stats->dF[1], B1.stats->dF[2]);
printf("B1.stats->entropy = %f\n", B1.stats->entropy);
for (i = 0; i < B1.d; i++) {
printf("B1.stats->scatter[%d] = [ ", i);
for (j = 0; j < B1.d; j++)
printf("%f ", B1.stats->scatter[i][j]);
printf("]\n");
}
printf("\n");
printf("B2.stats->mode = [ %f %f %f %f ]\n", B2.stats->mode[0], B2.stats->mode[1], B2.stats->mode[2], B2.stats->mode[3]);
printf("B2.stats->dF = [ %f %f %f ]\n", B2.stats->dF[0], B2.stats->dF[1], B2.stats->dF[2]);
printf("B2.stats->entropy = %f\n", B2.stats->entropy);
for (i = 0; i < B2.d; i++) {
printf("B2.stats->scatter[%d] = [ ", i);
for (j = 0; j < B2.d; j++)
printf("%f ", B2.stats->scatter[i][j]);
printf("]\n");
}
int n = 1000;
double t0 = get_time_ms();
double d_KL;
for (i = 0; i < n; i++)
d_KL = bingham_KL_divergence(&B1, &B2);
printf("Computed KL divergence %d times in %.0f ms\n", n, get_time_ms() - t0);
printf("d_KL = %f\n", d_KL);
bingham_t B;
bingham_merge(&B, &B1, &B2, .5);
bingham_stats(&B);
printf("\nMerged binghams:\n");
print_bingham(&B);
printf("\n");
printf("B.stats->mode = [ %f %f %f %f ]\n", B.stats->mode[0], B.stats->mode[1], B.stats->mode[2], B.stats->mode[3]);
printf("B.stats->dF = [ %f %f %f ]\n", B.stats->dF[0], B.stats->dF[1], B.stats->dF[2]);
printf("B.stats->entropy = %f\n", B.stats->entropy);
for (i = 0; i < B.d; i++) {
printf("B.stats->scatter[%d] = [ ", i);
for (j = 0; j < B.d; j++)
printf("%f ", B.stats->scatter[i][j]);
printf("]\n");
}
}
void test_bingham_compose(int argc, char *argv[])
{
int i, j, d = 4;
if (argc < 8) {
printf("usage: %s <z11> <z12> <z13> <z21> <z22> <z23> <num_samples>\n", argv[0]);
exit(1);
}
double z11 = atof(argv[1]);
double z12 = atof(argv[2]);
double z13 = atof(argv[3]);
double z21 = atof(argv[4]);
double z22 = atof(argv[5]);
double z23 = atof(argv[6]);
int nsamples = atoi(argv[7]);
bingham_t B1;
double Z1[3] = {z11, z12, z13};
double V1[3][4] = {{0,1,0,0}, {0,0,1,0}, {0,0,0,1}};
double *Vp1[3] = {&V1[0][0], &V1[1][0], &V1[2][0]};
bingham_new(&B1, 4, Vp1, Z1);
bingham_stats(&B1);
print_bingham(&B1);
printf("S1:\n");
for (i = 0; i < d; i++) {
for (j = 0; j < d; j++)
printf("%.4f, ", B1.stats->scatter[i][j]);
printf("\n");
}
printf("---------------------------\n");
bingham_t B2;
double Z2[3] = {z21, z22, z23};
double V2[3][4] = {{0,1,0,0}, {0,0,1,0}, {0,0,0,1}}; //{{1,0,0,0}, {0,1,0,0}, {0,0,1,0}};
double *Vp2[3] = {&V2[0][0], &V2[1][0], &V2[2][0]};
bingham_new(&B2, 4, Vp2, Z2);
bingham_stats(&B2);
print_bingham(&B2);
printf("S2:\n");
for (i = 0; i < d; i++) {
for (j = 0; j < d; j++)
printf("%.4f, ", B2.stats->scatter[i][j]);
printf("\n");
}
printf("---------------------------\n");
// compose with method of moments
//double **S_mom = new_matrix2(4,4);
bingham_t B_mom;
double t0 = get_time_ms();
for (i = 0; i < nsamples; i++)
bingham_compose(&B_mom, &B1, &B2);
printf("Composed %d Bingham pairs with Method-of-Moments in %.0f ms\n", nsamples, get_time_ms() - t0);
// compose with sampling
t0 = get_time_ms();
double **X1 = new_matrix2(nsamples, 4);
bingham_sample(X1, &B1, nsamples);
double **X2 = new_matrix2(nsamples, 4);
bingham_sample(X2, &B2, nsamples);
double **Y = new_matrix2(nsamples, 4);
for (i = 0; i < nsamples; i++)
quaternion_mult(Y[i], X1[i], X2[i]);
int n = nsamples;
double **Yt = new_matrix2(d, n);
transpose(Yt, Y, n, d);
double **S_sam = new_matrix2(d, d);
matrix_mult(S_sam, Yt, Y, d, n, d);
mult(S_sam[0], S_sam[0], 1/(double)n, d*d);
printf("Composed 1 Bingham pair with %d samples in %.0f ms\n", nsamples, get_time_ms() - t0);
printf("---------------------------\n");
printf("Method-of-Moments scatter matrix:\n");
bingham_stats(&B_mom);
for (i = 0; i < d; i++) {
for (j = 0; j < d; j++)
printf("%.4f, ", B_mom.stats->scatter[i][j]);
printf("\n");
}
printf("---------------------------\n");
printf("Sample scatter matrix:\n");
for (i = 0; i < d; i++) {
for (j = 0; j < d; j++)
printf("%.4f, ", S_sam[i][j]);
printf("\n");
}
printf("\n---------------------------\n\n");
t0 = get_time_ms();
for (i = 0; i < nsamples; i++)
bingham_compose_true_pdf(X1[i], &B1, &B2);
printf("Computed %d true composition pdf's in %.0f ms\n", nsamples, get_time_ms() - t0);
// compute sample mean error
double tot_err = 0;
for (i = 0; i < nsamples; i++) {
double f_true = bingham_compose_true_pdf(X1[i], &B1, &B2);
double f_approx = bingham_pdf(X1[i], &B_mom);
//printf("f_true = %f, f_approx = %f\n", f_true, f_approx);
tot_err += 2*fabs((f_true - f_approx) / (f_true + f_approx));
}
printf("mean sample err = %.2f%%\n", 100*tot_err/nsamples);
// compute KL divergence
printf("KL divergence = %f\n", bingham_compose_error(&B1, &B2));
}
