void default_cam_mgr_follow_disable(default_cam_mgr_t * state)
{
if (state->follow_lastpos != NULL) {
matd_destroy(state->follow_lastpos);
matd_destroy(state->follow_lastquat);
}
state->follow_lastpos = NULL;
state->follow_lastquat = NULL;
}
line_t linear_regression(Blob<Gradient> &blob) {
line_t line;
//Perform linear regression
//Ax = B
//A = [1 X_0],[1 X_1]
//B = [Y_0] ,[Y_1]
//x = [ b, m]
matd_t *A = matd_create(blob.size(),2);
matd_t *B = matd_create(blob.size(),1);
int leftmost, rightmost;
leftmost = rightmost = blob.getPos(0).x;
//Convert to Matd
for(size_t i = 0; i < blob.size(); ++i) {
pos_t tmp = blob.getPos(i);
if(tmp.x < leftmost) {
leftmost = tmp.x;
}
if(tmp.x > rightmost) {
rightmost = tmp.x;
}
MATD_EL(A, i, 0) = 1;
MATD_EL(A, i, 1) = tmp.x;
MATD_EL(B, i, 0) = tmp.y;
}
matd_t *x = matd_op("(M' * M)^-1 * M' * M",
A,A,A,B);
line.b = MATD_EL(x,0,0);
line.m = MATD_EL(x,1,0);
line.ll.x = leftmost;
line.ll.y = line.m*leftmost + line.b;
line.ru.x = rightmost;
line.ru.y = line.m*rightmost + line.b;
line.num_pts = blob.size();
//Compute difference of hypothetical vs. real
matd_t *diff = matd_op("(M*M) - M",
A,x,B);
//Sum of squares
matd_t *var = matd_op("M' * M",diff,diff);
line.variance = MATD_EL(var,0,0);
//Clean up
matd_destroy(A);
matd_destroy(B);
matd_destroy(x);
matd_destroy(diff);
matd_destroy(var);
return line;
}
vx_camera_pos_t * default_cam_mgr_get_cam_pos(default_cam_mgr_t * state, int * viewport, uint64_t mtime)
{
vx_camera_pos_t * p = calloc(1, sizeof(vx_camera_pos_t));
memcpy(p->viewport, viewport, 4*sizeof(int));
p->perspective_fovy_degrees = state->perspective_fovy_degrees;
p->zclip_near = state->zclip_near;
p->zclip_far = state->zclip_far;
// process a fit command if necessary:
if (state->fit != NULL) {
fit_t * f = state->fit;
// consume the fit command
state->fit = NULL; // XXX minor race condition, could lose a fit cmd
// XXX We can probably do better than this using the viewport...
state->lookat1[0] = (f->xy0[0] + f->xy1[0]) / 2;
state->lookat1[1] = (f->xy0[1] + f->xy1[1]) / 2;
state->lookat1[2] = 0;
// dimensions of fit
double Fw = f->xy1[0] - f->xy0[0];
double Fh = f->xy1[1] - f->xy0[1];
// aspect ratios
double Far = Fw / Fh;
double Var = p->viewport[2] * 1.0 / p->viewport[3];
double tAngle = tan(p->perspective_fovy_degrees/2*M_PI/180.0);
double height = fabs(0.5 * (Var > Far ? Fh : Fw / Var) / tAngle);
state->eye1[0] = state->lookat1[0];
state->eye1[1] = state->lookat1[1];
state->eye1[2] = height;
state->up1[0] = 0;
state->up1[1] = 1;
state->up1[2] = 0;
state->mtime1 = f->mtime;
free(f);
}
if (mtime > state->mtime1) {
memcpy(p->eye, state->eye1, 3*sizeof(double));
memcpy(p->up, state->up1, 3*sizeof(double));
memcpy(p->lookat, state->lookat1, 3*sizeof(double));
p->perspectiveness = state->perspectiveness1;
} else if (mtime <= state->mtime0) {
memcpy(p->eye, state->eye0, 3*sizeof(double));
memcpy(p->up, state->up0, 3*sizeof(double));
memcpy(p->lookat, state->lookat0, 3*sizeof(double));
p->perspectiveness = state->perspectiveness0;
} else {
double alpha1 = ((double) mtime - state->mtime0) / (state->mtime1 - state->mtime0);
double alpha0 = 1.0 - alpha1;
scaled_combination(state->eye0, alpha0, state->eye1, alpha1, p->eye, 3);
scaled_combination(state->up0, alpha0, state->up1, alpha1, p->up, 3);
scaled_combination(state->lookat0, alpha0, state->lookat1, alpha1, p->lookat, 3);
p->perspectiveness = state->perspectiveness0*alpha0 + state->perspectiveness1*alpha1;
// Tweak so eye-to-lookat is the right distance
{
zarray_t * fp = zarray_create(sizeof(matd_t*));
matd_t * eye = matd_create_data(3,1, p->eye); zarray_add(fp, &eye);
matd_t * lookat = matd_create_data(3,1, p->lookat); zarray_add(fp, &lookat);
matd_t * up = matd_create_data(3,1, p->up); zarray_add(fp, &up);
matd_t * eye0 = matd_create_data(3,1, state->eye0); zarray_add(fp, &eye0);
matd_t * lookat0 = matd_create_data(3,1, state->lookat0); zarray_add(fp, &lookat0);
matd_t * up0 = matd_create_data(3,1, state->up0); zarray_add(fp, &up0);
matd_t * eye1 = matd_create_data(3,1, state->eye1); zarray_add(fp, &eye1);
matd_t * lookat1 = matd_create_data(3,1, state->lookat1); zarray_add(fp, &lookat1);
matd_t * up1 = matd_create_data(3,1, state->up1); zarray_add(fp, &up1);
double dist0 = matd_vec_dist(eye0, lookat0);
double dist1 = matd_vec_dist(eye1, lookat1);
matd_t * dist = matd_create_scalar(dist0*alpha0 + dist1*alpha1); zarray_add(fp, &dist);
matd_t * eye2p = matd_subtract(eye,lookat); zarray_add(fp, &eye2p);
eye2p = matd_vec_normalize(eye2p); zarray_add(fp, &eye2p);
eye = matd_op("M + (M*M)", lookat, eye2p, dist);
// Only modified eye
memcpy(p->eye, eye->data, 3*sizeof(double));
zarray_vmap(fp, matd_destroy);
zarray_destroy(fp);
}
}
// Need to do more fixup depending on interface mode!
{
if (state->interface_mode <= 2.0) {
// stack eye on lookat:
p->eye[0] = p->lookat[0];
p->eye[1] = p->lookat[1];
p->lookat[2] = 0;
// skip fabs() for ENU/NED compat
//p->eye[2] = fabs(p->eye[2]);
{
matd_t * up = matd_create_data(3,1, p->up);
up->data[2] = 0; // up should never point in Z
matd_t * up_norm = matd_vec_normalize(up);
memcpy(p->up, up_norm->data, sizeof(double)*3);
matd_destroy(up);
matd_destroy(up_norm);
}
} else if (state->interface_mode == 2.5) {
zarray_t * fp = zarray_create(sizeof(matd_t*));
matd_t * eye = matd_create_data(3,1, p->eye); zarray_add(fp, &eye);
matd_t * lookat = matd_create_data(3,1, p->lookat); zarray_add(fp, &lookat);
matd_t * up = matd_create_data(3,1, p->up); zarray_add(fp, &up);
lookat->data[2] = 0.0;
// Level horizon
matd_t * dir = matd_subtract(lookat, eye); zarray_add(fp, &dir);
matd_t * dir_norm = matd_vec_normalize(dir); zarray_add(fp, &dir_norm);
matd_t * left = matd_crossproduct(up, dir_norm); zarray_add(fp, &left);
left->data[2] = 0.0;
left = matd_vec_normalize(left); zarray_add(fp, &left);
// Don't allow upside down
//up->data[2] = fmax(0.0, up->data[2]); // XXX NED?
// Find an 'up' direction perpendicular to left
matd_t * dot_scalar = matd_create_scalar(matd_vec_dot_product(up, left)); zarray_add(fp, &dot_scalar);
up = matd_op("M - (M*M)", up, left, dot_scalar); zarray_add(fp, &up);
up = matd_vec_normalize(up); zarray_add(fp, &up);
// Now find eye position by computing new lookat dir
matd_t * eye_dir = matd_crossproduct(up, left); zarray_add(fp, &eye_dir);
matd_t *eye_dist_scalar = matd_create_scalar(matd_vec_dist(eye, lookat)); zarray_add(fp, &eye_dist_scalar);
eye = matd_op("M + (M*M)", lookat, eye_dir, eye_dist_scalar); zarray_add(fp, &eye);
// export results back to p:
memcpy(p->eye, eye->data, sizeof(double)*3);
memcpy(p->lookat, lookat->data, sizeof(double)*3);
memcpy(p->up, up->data, sizeof(double)*3);
zarray_vmap(fp, matd_destroy);
zarray_destroy(fp);
}
}
// Fix up for bad zoom
if (1) {
matd_t * eye = matd_create_data(3,1, p->eye);
matd_t * lookat = matd_create_data(3,1, p->lookat);
matd_t * up = matd_create_data(3,1, p->up);
matd_t * lookeye = matd_subtract(lookat, eye);
matd_t * lookdir = matd_vec_normalize(lookeye);
double dist = matd_vec_dist(eye, lookat);
dist = fmin(state->zclip_far / 3.0, dist);
dist = fmax(state->zclip_near * 3.0, dist);
matd_scale_inplace(lookdir, dist);
matd_t * eye_fixed = matd_subtract(lookat, lookdir);
memcpy(p->eye, eye_fixed->data, sizeof(double)*3);
matd_destroy(eye);
matd_destroy(lookat);
matd_destroy(up);
matd_destroy(lookeye);
matd_destroy(lookdir);
matd_destroy(eye_fixed);
}
// copy the result back into 'state'
{
memcpy(state->eye0, p->eye, 3*sizeof(double));
memcpy(state->up0, p->up, 3*sizeof(double));
memcpy(state->lookat0, p->lookat, 3*sizeof(double));
state->perspectiveness0 = p->perspectiveness;
state->mtime0 = mtime;
}
return p;
}
void default_cam_mgr_follow(default_cam_mgr_t * state, double * _pos3, double * _quat4, int followYaw, uint32_t animate_ms)
{
matd_t * pos3 = matd_create_data(3, 1, _pos3);
matd_t * quat4 = matd_create_data(4, 1, _quat4);
if (state->follow_lastpos != NULL) {
matd_t * eye1 = matd_create_data(3,1,state->eye1);
matd_t * lookat1 = matd_create_data(3,1, state->lookat1);
matd_t * up1 = matd_create_data(3,1, state->up1);
if (followYaw) {
matd_t * v2eye = matd_subtract(state->follow_lastpos, eye1);
matd_t * v2look = matd_subtract(state->follow_lastpos, lookat1);
matd_t * new_rpy = matd_create(3,1);
//matd_quat_to_rpy(quat4);
vx_util_quat_to_rpy(quat4->data, new_rpy->data);
matd_t * last_rpy = matd_create(3,1);
//matd_quat_to_rpy(state->follow_lastquat);
vx_util_quat_to_rpy(state->follow_lastquat->data, last_rpy->data);
double dtheta = new_rpy->data[2] - last_rpy->data[2];
matd_t * zaxis = matd_create(3,1);
zaxis->data[2] = 1;
matd_t * zq = matd_create(4,1);
//zq = matd_angle_axis_to_quat(dtheta, zaxis);
vx_util_angle_axis_to_quat(dtheta, zaxis->data, zq->data);
matd_t * v2look_rot = matd_create(3,1);
//matd_quat_rotate(zq, v2look);
vx_util_quat_rotate(zq->data, v2look->data, v2look_rot->data);
matd_t * new_lookat = matd_subtract(pos3, v2look_rot);
matd_t * v2eye_rot = matd_create(3,1);
//matd_quat_rotate(zq, v2eye);
vx_util_quat_rotate(zq->data, v2eye->data, v2eye_rot->data);
matd_t * new_eye = matd_subtract(pos3, v2eye_rot);
matd_t * new_up = matd_create(3,1);
//matd_quat_rotate(zq, up1);
vx_util_quat_rotate(zq->data, up1->data, new_up->data);
internal_lookat(state, new_eye->data, new_lookat->data, new_up->data, animate_ms);
// cleanup
matd_destroy(v2eye);
matd_destroy(v2look);
matd_destroy(new_rpy);
matd_destroy(last_rpy);
matd_destroy(zaxis);
matd_destroy(zq);
matd_destroy(v2look_rot);
matd_destroy(new_lookat);
matd_destroy(v2eye_rot);
matd_destroy(new_eye);
matd_destroy(new_up);
} else {
matd_t * dpos = matd_subtract(pos3, state->follow_lastpos);
matd_t * newEye = matd_add(eye1, dpos);
matd_t * newLookAt = matd_add(lookat1, dpos);
internal_lookat(state, newEye->data, newLookAt->data, up1->data, animate_ms);
matd_destroy(dpos);
matd_destroy(newEye);
matd_destroy(newLookAt);
}
matd_destroy(eye1);
matd_destroy(lookat1);
matd_destroy(up1);
// Cleanup
matd_destroy(state->follow_lastpos);
matd_destroy(state->follow_lastquat);
}
state->follow_lastpos = pos3;
state->follow_lastquat = quat4;
}
// correspondences is a list of float[4]s, consisting of the points x
// and y concatenated. We will compute a homography such that y = Hx
matd_t *homography_compute(zarray_t *correspondences, int flags)
{
// compute centroids of both sets of points (yields a better
// conditioned information matrix)
double x_cx = 0, x_cy = 0;
double y_cx = 0, y_cy = 0;
for (int i = 0; i < zarray_size(correspondences); i++) {
float *c;
zarray_get_volatile(correspondences, i, &c);
x_cx += c[0];
x_cy += c[1];
y_cx += c[2];
y_cy += c[3];
}
int sz = zarray_size(correspondences);
x_cx /= sz;
x_cy /= sz;
y_cx /= sz;
y_cy /= sz;
// NB We don't normalize scale; it seems implausible that it could
// possibly make any difference given the dynamic range of IEEE
// doubles.
matd_t *A = matd_create(9,9);
for (int i = 0; i < zarray_size(correspondences); i++) {
float *c;
zarray_get_volatile(correspondences, i, &c);
// (below world is "x", and image is "y")
double worldx = c[0] - x_cx;
double worldy = c[1] - x_cy;
double imagex = c[2] - y_cx;
double imagey = c[3] - y_cy;
double a03 = -worldx;
double a04 = -worldy;
double a05 = -1;
double a06 = worldx*imagey;
double a07 = worldy*imagey;
double a08 = imagey;
MATD_EL(A, 3, 3) += a03*a03;
MATD_EL(A, 3, 4) += a03*a04;
MATD_EL(A, 3, 5) += a03*a05;
MATD_EL(A, 3, 6) += a03*a06;
MATD_EL(A, 3, 7) += a03*a07;
MATD_EL(A, 3, 8) += a03*a08;
MATD_EL(A, 4, 4) += a04*a04;
MATD_EL(A, 4, 5) += a04*a05;
MATD_EL(A, 4, 6) += a04*a06;
MATD_EL(A, 4, 7) += a04*a07;
MATD_EL(A, 4, 8) += a04*a08;
MATD_EL(A, 5, 5) += a05*a05;
MATD_EL(A, 5, 6) += a05*a06;
MATD_EL(A, 5, 7) += a05*a07;
MATD_EL(A, 5, 8) += a05*a08;
MATD_EL(A, 6, 6) += a06*a06;
MATD_EL(A, 6, 7) += a06*a07;
MATD_EL(A, 6, 8) += a06*a08;
MATD_EL(A, 7, 7) += a07*a07;
MATD_EL(A, 7, 8) += a07*a08;
MATD_EL(A, 8, 8) += a08*a08;
double a10 = worldx;
double a11 = worldy;
double a12 = 1;
double a16 = -worldx*imagex;
double a17 = -worldy*imagex;
double a18 = -imagex;
MATD_EL(A, 0, 0) += a10*a10;
MATD_EL(A, 0, 1) += a10*a11;
MATD_EL(A, 0, 2) += a10*a12;
MATD_EL(A, 0, 6) += a10*a16;
MATD_EL(A, 0, 7) += a10*a17;
MATD_EL(A, 0, 8) += a10*a18;
MATD_EL(A, 1, 1) += a11*a11;
MATD_EL(A, 1, 2) += a11*a12;
MATD_EL(A, 1, 6) += a11*a16;
MATD_EL(A, 1, 7) += a11*a17;
MATD_EL(A, 1, 8) += a11*a18;
MATD_EL(A, 2, 2) += a12*a12;
MATD_EL(A, 2, 6) += a12*a16;
MATD_EL(A, 2, 7) += a12*a17;
MATD_EL(A, 2, 8) += a12*a18;
MATD_EL(A, 6, 6) += a16*a16;
MATD_EL(A, 6, 7) += a16*a17;
MATD_EL(A, 6, 8) += a16*a18;
MATD_EL(A, 7, 7) += a17*a17;
MATD_EL(A, 7, 8) += a17*a18;
MATD_EL(A, 8, 8) += a18*a18;
double a20 = -worldx*imagey;
double a21 = -worldy*imagey;
double a22 = -imagey;
double a23 = worldx*imagex;
double a24 = worldy*imagex;
double a25 = imagex;
MATD_EL(A, 0, 0) += a20*a20;
MATD_EL(A, 0, 1) += a20*a21;
MATD_EL(A, 0, 2) += a20*a22;
MATD_EL(A, 0, 3) += a20*a23;
MATD_EL(A, 0, 4) += a20*a24;
MATD_EL(A, 0, 5) += a20*a25;
MATD_EL(A, 1, 1) += a21*a21;
MATD_EL(A, 1, 2) += a21*a22;
MATD_EL(A, 1, 3) += a21*a23;
MATD_EL(A, 1, 4) += a21*a24;
MATD_EL(A, 1, 5) += a21*a25;
MATD_EL(A, 2, 2) += a22*a22;
MATD_EL(A, 2, 3) += a22*a23;
MATD_EL(A, 2, 4) += a22*a24;
MATD_EL(A, 2, 5) += a22*a25;
MATD_EL(A, 3, 3) += a23*a23;
MATD_EL(A, 3, 4) += a23*a24;
MATD_EL(A, 3, 5) += a23*a25;
MATD_EL(A, 4, 4) += a24*a24;
MATD_EL(A, 4, 5) += a24*a25;
MATD_EL(A, 5, 5) += a25*a25;
}
// make symmetric
for (int i = 0; i < 9; i++)
for (int j = i+1; j < 9; j++)
MATD_EL(A, j, i) = MATD_EL(A, i, j);
matd_t *H = matd_create(3,3);
if (flags & HOMOGRAPHY_COMPUTE_FLAG_INVERSE) {
// compute singular vector by (carefully) inverting the rank-deficient matrix.
if (1) {
matd_t *Ainv = matd_inverse(A);
double scale = 0;
for (int i = 0; i < 9; i++)
scale += sq(MATD_EL(Ainv, i, 0));
scale = sqrt(scale);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale;
matd_destroy(Ainv);
} else {
matd_t *b = matd_create_data(9, 1, (double[]) { 1, 0, 0, 0, 0, 0, 0, 0, 0 });
matd_t *Ainv = NULL;
if (0) {
matd_lu_t *lu = matd_lu(A);
Ainv = matd_lu_solve(lu, b);
matd_lu_destroy(lu);
} else {
matd_chol_t *chol = matd_chol(A);
Ainv = matd_chol_solve(chol, b);
matd_chol_destroy(chol);
}
double scale = 0;
for (int i = 0; i < 9; i++)
scale += sq(MATD_EL(Ainv, i, 0));
scale = sqrt(scale);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale;
matd_destroy(b);
matd_destroy(Ainv);
}
} else {
// correspondences is a list of float[4]s, consisting of the points x
// and y concatenated. We will compute a homography such that y = Hx
matd_t *homography_compute(zarray_t *correspondences)
{
// compute centroids of both sets of points (yields a better
// conditioned information matrix)
double x_cx = 0, x_cy = 0;
double y_cx = 0, y_cy = 0;
for (int i = 0; i < zarray_size(correspondences); i++) {
float *c;
zarray_get_volatile(correspondences, i, &c);
x_cx += c[0];
x_cy += c[1];
y_cx += c[2];
y_cy += c[3];
}
int sz = zarray_size(correspondences);
x_cx /= sz;
x_cy /= sz;
y_cx /= sz;
y_cy /= sz;
// NB We don't normalize scale; it seems implausible that it could
// possibly make any difference given the dynamic range of IEEE
// doubles.
matd_t *A = matd_create(9,9);
for (int i = 0; i < zarray_size(correspondences); i++) {
float *c;
zarray_get_volatile(correspondences, i, &c);
// (below world is "x", and image is "y")
double worldx = c[0] - x_cx;
double worldy = c[1] - x_cy;
double imagex = c[2] - y_cx;
double imagey = c[3] - y_cy;
double a03 = -worldx;
double a04 = -worldy;
double a05 = -1;
double a06 = worldx*imagey;
double a07 = worldy*imagey;
double a08 = imagey;
MATD_EL(A, 3, 3) += a03*a03;
MATD_EL(A, 3, 4) += a03*a04;
MATD_EL(A, 3, 5) += a03*a05;
MATD_EL(A, 3, 6) += a03*a06;
MATD_EL(A, 3, 7) += a03*a07;
MATD_EL(A, 3, 8) += a03*a08;
MATD_EL(A, 4, 4) += a04*a04;
MATD_EL(A, 4, 5) += a04*a05;
MATD_EL(A, 4, 6) += a04*a06;
MATD_EL(A, 4, 7) += a04*a07;
MATD_EL(A, 4, 8) += a04*a08;
MATD_EL(A, 5, 5) += a05*a05;
MATD_EL(A, 5, 6) += a05*a06;
MATD_EL(A, 5, 7) += a05*a07;
MATD_EL(A, 5, 8) += a05*a08;
MATD_EL(A, 6, 6) += a06*a06;
MATD_EL(A, 6, 7) += a06*a07;
MATD_EL(A, 6, 8) += a06*a08;
MATD_EL(A, 7, 7) += a07*a07;
MATD_EL(A, 7, 8) += a07*a08;
MATD_EL(A, 8, 8) += a08*a08;
double a10 = worldx;
double a11 = worldy;
double a12 = 1;
double a16 = -worldx*imagex;
double a17 = -worldy*imagex;
double a18 = -imagex;
MATD_EL(A, 0, 0) += a10*a10;
MATD_EL(A, 0, 1) += a10*a11;
MATD_EL(A, 0, 2) += a10*a12;
MATD_EL(A, 0, 6) += a10*a16;
MATD_EL(A, 0, 7) += a10*a17;
MATD_EL(A, 0, 8) += a10*a18;
MATD_EL(A, 1, 1) += a11*a11;
MATD_EL(A, 1, 2) += a11*a12;
MATD_EL(A, 1, 6) += a11*a16;
MATD_EL(A, 1, 7) += a11*a17;
MATD_EL(A, 1, 8) += a11*a18;
MATD_EL(A, 2, 2) += a12*a12;
MATD_EL(A, 2, 6) += a12*a16;
MATD_EL(A, 2, 7) += a12*a17;
MATD_EL(A, 2, 8) += a12*a18;
MATD_EL(A, 6, 6) += a16*a16;
MATD_EL(A, 6, 7) += a16*a17;
MATD_EL(A, 6, 8) += a16*a18;
MATD_EL(A, 7, 7) += a17*a17;
MATD_EL(A, 7, 8) += a17*a18;
MATD_EL(A, 8, 8) += a18*a18;
double a20 = -worldx*imagey;
double a21 = -worldy*imagey;
double a22 = -imagey;
double a23 = worldx*imagex;
double a24 = worldy*imagex;
double a25 = imagex;
MATD_EL(A, 0, 0) += a20*a20;
MATD_EL(A, 0, 1) += a20*a21;
MATD_EL(A, 0, 2) += a20*a22;
MATD_EL(A, 0, 3) += a20*a23;
MATD_EL(A, 0, 4) += a20*a24;
MATD_EL(A, 0, 5) += a20*a25;
MATD_EL(A, 1, 1) += a21*a21;
MATD_EL(A, 1, 2) += a21*a22;
MATD_EL(A, 1, 3) += a21*a23;
MATD_EL(A, 1, 4) += a21*a24;
MATD_EL(A, 1, 5) += a21*a25;
MATD_EL(A, 2, 2) += a22*a22;
MATD_EL(A, 2, 3) += a22*a23;
MATD_EL(A, 2, 4) += a22*a24;
MATD_EL(A, 2, 5) += a22*a25;
MATD_EL(A, 3, 3) += a23*a23;
MATD_EL(A, 3, 4) += a23*a24;
MATD_EL(A, 3, 5) += a23*a25;
MATD_EL(A, 4, 4) += a24*a24;
MATD_EL(A, 4, 5) += a24*a25;
MATD_EL(A, 5, 5) += a25*a25;
}
// make symmetric
for (int i = 0; i < 9; i++)
for (int j = i+1; j < 9; j++)
MATD_EL(A, j, i) = MATD_EL(A, i, j);
matd_svd_t svd = matd_svd(A);
matd_t *Ainv = matd_inverse(A);
double scale = 0;
for (int i = 0; i < 9; i++)
scale += sq(MATD_EL(Ainv, i, 0));
scale = sqrt(scale);
if (1) {
// compute singular vector using SVD. A bit slower, but more accurate.
matd_svd_t svd = matd_svd(A);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
// MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0)/ scale;
MATD_EL(H, i, j) = MATD_EL(svd.U, 3*i+j, 8);
matd_destroy(svd.U);
matd_destroy(svd.S);
matd_destroy(svd.V);
} else {
// compute singular vector by (carefully) inverting the rank-deficient matrix.
matd_t *Ainv = matd_inverse(A);
double scale = 0;
for (int i = 0; i < 9; i++)
scale += sq(MATD_EL(Ainv, i, 0));
scale = sqrt(scale);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0)/ scale;
matd_destroy(Ainv);
}
matd_t *Tx = matd_identity(3);
MATD_EL(Tx,0,2) = -x_cx;
MATD_EL(Tx,1,2) = -x_cy;
matd_t *Ty = matd_identity(3);
MATD_EL(Ty,0,2) = y_cx;
MATD_EL(Ty,1,2) = y_cy;
matd_t *H2 = matd_op("M*M*M", Ty, H, Tx);
matd_destroy(A);
matd_destroy(Tx);
matd_destroy(Ty);
matd_destroy(H);
matd_destroy(svd.U);
matd_destroy(svd.S);
matd_destroy(svd.V);
return H2;
}
BoundingBox::~BoundingBox() {
matd_destroy(this->pos);
matd_destroy(this->ux);
matd_destroy(this->uy);
matd_destroy(this->uz);
}
bool BoundingBox::intersect(BoundingBox *b) {
double checkL, sum;
BoundingBox *a = this;
matd_t *cross;
matd_t *T = matd_subtract(b->pos, a->pos);
// Case 1
checkL = fabs(matd_vec_dot_product(T, a->ux));
sum = a->hW;
sum += fabs(b->hW*matd_vec_dot_product(a->ux, b->ux));
sum += fabs(b->hH*matd_vec_dot_product(a->ux, b->uy));
sum += fabs(b->hD*matd_vec_dot_product(a->ux, b->uz));
if (checkL > sum) {
//printf("Axis found, case 1\n");
return false;
}
//printf("1 - checkL %f, sum %f\n", checkL, sum);
// Case 2
checkL = fabs(matd_vec_dot_product(T, a->uy));
sum = a->hH;
sum += fabs(b->hW*matd_vec_dot_product(a->uy, b->ux));
sum += fabs(b->hH*matd_vec_dot_product(a->uy, b->uy));
sum += fabs(b->hD*matd_vec_dot_product(a->uy, b->uz));
if (checkL > sum) {
//printf("Axis found, case 2\n");
return false;
}
//printf("2 - checkL %f, sum %f\n", checkL, sum);
// Case 3
checkL = fabs(matd_vec_dot_product(T, a->uz));
sum = a->hD;
sum += fabs(b->hW*matd_vec_dot_product(a->uz, b->ux));
sum += fabs(b->hH*matd_vec_dot_product(a->uz, b->uy));
sum += fabs(b->hD*matd_vec_dot_product(a->uz, b->uz));
if (checkL > sum) {
//printf("Axis found, case 3\n");
return false;
}
//printf("3 - checkL %f, sum %f\n", checkL, sum);
// Case 4
checkL = fabs(matd_vec_dot_product(T, b->ux));
sum = fabs(a->hW*matd_vec_dot_product(a->ux, b->ux));
sum += fabs(a->hH*matd_vec_dot_product(a->uy, b->ux));
sum += fabs(a->hD*matd_vec_dot_product(a->uz, b->ux));
sum += b->hW;
if (checkL > sum) {
//printf("Axis found, case 4\n");
return false;
}
//printf("4 - checkL %f, sum %f\n", checkL, sum);
// Case 5
checkL = fabs(matd_vec_dot_product(T, b->uy));
sum = fabs(a->hW*matd_vec_dot_product(a->ux, b->uy));
sum += fabs(a->hH*matd_vec_dot_product(a->uy, b->uy));
sum += fabs(a->hD*matd_vec_dot_product(a->uz, b->uy));
sum += b->hH;
if (checkL > sum) {
//printf("Axis found, case 5\n");
return false;
}
//printf("5 - checkL %f, sum %f\n", checkL, sum);
// Case 6
checkL = fabs(matd_vec_dot_product(T, b->uz));
sum = fabs(a->hW*matd_vec_dot_product(a->ux, b->uz));
sum += fabs(a->hH*matd_vec_dot_product(a->uy, b->uz));
sum += fabs(a->hD*matd_vec_dot_product(a->uz, b->uz));
sum += b->hD;
if (checkL > sum) {
//printf("Axis found, case 6\n");
return false;
}
//printf("6 - checkL %f, sum %f\n", checkL, sum);
// Case 7
cross = matd_crossproduct(a->ux, b->ux);
checkL = fabs(matd_vec_dot_product(T, cross));
sum = fabs(a->hH*matd_vec_dot_product(a->uz, b->ux));
sum += fabs(a->hD*matd_vec_dot_product(a->uy, b->ux));
sum += fabs(b->hH*matd_vec_dot_product(a->ux, b->uz));
sum += fabs(b->hD*matd_vec_dot_product(a->ux, b->uy));
matd_destroy(cross);
if (checkL > sum) {
//printf("Axis found, case 7\n");
return false;
}
//printf("7 - checkL %f, sum %f\n", checkL, sum);
// Case 8
cross = matd_crossproduct(a->ux, b->uy);
checkL = fabs(matd_vec_dot_product(T, cross));
sum = fabs(a->hH*matd_vec_dot_product(a->uz, b->uy));
sum += fabs(a->hD*matd_vec_dot_product(a->uy, b->uy));
sum += fabs(b->hW*matd_vec_dot_product(a->ux, b->uz));
sum += fabs(b->hD*matd_vec_dot_product(a->ux, b->ux));
matd_destroy(cross);
if (checkL > sum) {
//printf("Axis found, case 8\n");
return false;
}
//printf("8 - checkL %f, sum %f\n", checkL, sum);
// Case 9
cross = matd_crossproduct(a->ux, b->uz);
checkL = fabs(matd_vec_dot_product(T, cross));
sum = fabs(a->hH*matd_vec_dot_product(a->uz, b->uz));
sum += fabs(a->hD*matd_vec_dot_product(a->uy, b->uz));
sum += fabs(b->hW*matd_vec_dot_product(a->ux, b->uy));
sum += fabs(b->hH*matd_vec_dot_product(a->ux, b->ux));
matd_destroy(cross);
if (checkL > sum) {
//printf("Axis found, case 9\n");
return false;
}
//printf("9 - checkL %f, sum %f\n", checkL, sum);
// Case 10
cross = matd_crossproduct(a->uy, b->ux);
checkL = fabs(matd_vec_dot_product(T, cross));
sum = fabs(a->hW*matd_vec_dot_product(a->uz, b->ux));
sum += fabs(a->hD*matd_vec_dot_product(a->ux, b->ux));
sum += fabs(b->hH*matd_vec_dot_product(a->uy, b->uz));
sum += fabs(b->hD*matd_vec_dot_product(a->uy, b->uy));
matd_destroy(cross);
if (checkL > sum) {
//printf("Axis found, case 10\n");
return false;
}
//printf("10 - checkL %f, sum %f\n", checkL, sum);
// Case 11
cross = matd_crossproduct(a->uy, b->uy);
checkL = fabs(matd_vec_dot_product(T, cross));
sum = fabs(a->hW*matd_vec_dot_product(a->uz, b->uy));
sum += fabs(a->hD*matd_vec_dot_product(a->ux, b->uy));
sum += fabs(b->hW*matd_vec_dot_product(a->uy, b->uz));
sum += fabs(b->hD*matd_vec_dot_product(a->uy, b->ux));
matd_destroy(cross);
if (checkL > sum) {
//printf("Axis found, case 11\n");
return false;
}
//printf("11 - checkL %f, sum %f\n", checkL, sum);
// Case 12
cross = matd_crossproduct(a->uy, b->uz);
checkL = fabs(matd_vec_dot_product(T, cross));
sum = fabs(a->hW*matd_vec_dot_product(a->uz, b->uz));
sum += fabs(a->hD*matd_vec_dot_product(a->ux, b->uz));
sum += fabs(b->hW*matd_vec_dot_product(a->uy, b->uy));
sum += fabs(b->hH*matd_vec_dot_product(a->uy, b->ux));
matd_destroy(cross);
if (checkL > sum) {
//printf("Axis found, case 12\n");
return false;
}
//printf("12 - checkL %f, sum %f\n", checkL, sum);
// Case 13
cross = matd_crossproduct(a->uz, b->ux);
checkL = fabs(matd_vec_dot_product(T, cross));
sum = fabs(a->hW*matd_vec_dot_product(a->uy, b->ux));
sum += fabs(a->hH*matd_vec_dot_product(a->ux, b->ux));
sum += fabs(b->hH*matd_vec_dot_product(a->uz, b->uz));
sum += fabs(b->hD*matd_vec_dot_product(a->uz, b->uy));
matd_destroy(cross);
if (checkL > sum) {
//printf("Axis found, case 13\n");
return false;
}
//printf("13 - checkL %f, sum %f\n", checkL, sum);
// Case 14
cross = matd_crossproduct(a->uz, b->uy);
checkL = fabs(matd_vec_dot_product(T, cross));
sum = fabs(a->hW*matd_vec_dot_product(a->uy, b->uy));
sum += fabs(a->hH*matd_vec_dot_product(a->ux, b->uy));
sum += fabs(b->hW*matd_vec_dot_product(a->uz, b->uz));
sum += fabs(b->hD*matd_vec_dot_product(a->uz, b->ux));
matd_destroy(cross);
if (checkL > sum) {
//printf("Axis found, case 14\n");
return false;
}
//printf("14 - checkL %f, sum %f\n", checkL, sum);
// Case 15
cross = matd_crossproduct(a->uz, b->uz);
checkL = fabs(matd_vec_dot_product(T, cross));
sum = fabs(a->hW*matd_vec_dot_product(a->uy, b->uz));
sum += fabs(a->hH*matd_vec_dot_product(a->ux, b->uz));
sum += fabs(b->hW*matd_vec_dot_product(a->uz, b->uy));
sum += fabs(b->hH*matd_vec_dot_product(a->uz, b->ux));
matd_destroy(cross);
if (checkL > sum) {
//printf("Axis found, case 15\n");
return false;
}
//printf("15 - checkL %f, sum %f\n", checkL, sum);
matd_destroy(T);
return true;
}
void * camera_loop(void * data)
{
state_t * state = data;
sleep(2); // wait for 2 seconds before starting the animation
matd_t * zaxis = matd_create(3,1);
zaxis->data[2] = 1;
vx_buffer_add_back(vx_world_get_buffer(state->world, "cam-circle"), vxo_chain(vxo_mat_scale(CAM_RADIUS),
vxo_circle(vxo_lines_style(vx_green, 3))));
vx_buffer_swap(vx_world_get_buffer(state->world, "cam-circle"));
int64_t start_mtime = vx_util_mtime();
// tell each layer to follow
pthread_mutex_lock(&state->mutex);
{
zhash_iterator_t itr;
zhash_iterator_init(state->layers, &itr);
vx_display_t * key;
vx_layer_t * vl;
while(zhash_iterator_next(&itr, &key, &vl)) {
if (1) {
float eye3[] = {CAM_RADIUS,-CAM_RADIUS,45.0f};
float lookat3[] = {CAM_RADIUS,0,0.0f};
float up3[] = {0,1,0};
vx_layer_camera_lookat(vl, eye3, lookat3, up3, 0);
}
}
}
pthread_mutex_unlock(&state->mutex);
while (state->running) {
// 5 seconds revolutions
double rad = ( (vx_util_mtime() - start_mtime) % 5000) * 2* M_PI / 5e3;
// compute the current position and orientation of the "robot"
matd_t * orientation = matd_angle_axis_to_quat(rad, zaxis);
matd_t * pos = matd_create(3,1);
pos->data[0] = cos(rad) * CAM_RADIUS;
pos->data[1] = sin(rad) * CAM_RADIUS;
// tell each layer to follow
pthread_mutex_lock(&state->mutex);
{
zhash_iterator_t itr;
zhash_iterator_init(state->layers, &itr);
vx_display_t * key;
vx_layer_t * vl;
while(zhash_iterator_next(&itr, &key, &vl)) {
vx_layer_camera_follow(vl, pos->data, orientation->data, 1);
}
}
pthread_mutex_unlock(&state->mutex);
vx_buffer_add_back(vx_world_get_buffer(state->world, "robot-proxy"),
vxo_chain(vxo_mat_quat_pos(orientation->data, pos->data),
vxo_box(vxo_lines_style(vx_purple, 3))));
vx_buffer_swap(vx_world_get_buffer(state->world, "robot-proxy"));
matd_destroy(orientation);
matd_destroy(pos);
usleep(100000);
}
matd_destroy(zaxis);
return NULL;
}
void body_getServoAngles(Body *body, double servoAngles[], bool right_side) {
matd_t* floor_shoulder;
matd_t* shoulder_elbow;
matd_t* shoulder_elbow0;
matd_t* shoulder_elbow1;
matd_t* elbow_wrist;
joint_t shoulder, elbow, wrist;
if(right_side){
//use right side of body
shoulder = body->getJoint(RSHOULDER);
elbow = body->getJoint(RELBOW);
wrist = body->getJoint(RWRIST);
}else{
//use left side of body
shoulder = body->getJoint(LSHOULDER);
elbow = body->getJoint(LELBOW);
wrist = body->getJoint(LWRIST);
}
double floor_shoulder_data[3] = {
0, shoulder.y, 0};
double shoulder_elbow_data[3] = {
elbow.x - shoulder.x,
elbow.y - shoulder.y,
elbow.z - shoulder.z};
//Shoulder rotation in the yz plane (forward/backward)
double shoulder_elbow_data0[3] = {
0,
elbow.y - shoulder.y,
elbow.z - shoulder.z};
//Shoulder rotation in the xy plane (left/right)
double shoulder_elbow_data1[3] = {
elbow.x - shoulder.x,
elbow.y - shoulder.y,
0};
double elbow_wrist_data[3] = {
wrist.x - elbow.x,
wrist.y - elbow.y,
wrist.z - elbow.z};
floor_shoulder = matd_create_data(3, 1, floor_shoulder_data);
shoulder_elbow = matd_create_data(3, 1, shoulder_elbow_data);
//shoulder_elbow0 = matd_create_data(3, 1, shoulder_elbow_data0);
shoulder_elbow1 = matd_create_data(3, 1, shoulder_elbow_data1);
elbow_wrist = matd_create_data(3, 1, elbow_wrist_data);
double magfs = matd_vec_mag(floor_shoulder);
double magse = matd_vec_mag(shoulder_elbow);
//double magse0 = matd_vec_mag(shoulder_elbow0);
double magse1 = matd_vec_mag(shoulder_elbow1);
double magew = matd_vec_mag(elbow_wrist);
//double shoulderValue0 = matd_vec_dot_product(floor_shoulder, shoulder_elbow0) / (magfs * magse0);
double shoulderValue1 = matd_vec_dot_product(floor_shoulder, shoulder_elbow1) / (magfs * magse1);
double elbowValue = matd_vec_dot_product(shoulder_elbow, elbow_wrist) / (magse * magew);
//printf("Elbow: %g\n", elbowValue);
//double shoulderAngle0 = (acos(shoulderValue0));
double shoulderAngle1 = sgn(elbow.y - shoulder.y)*(acos(shoulderValue1) - M_PI/2);
double elbowAngle = acos(elbowValue);
/*if (elbow.z - shoulder.z < 0) {
shoulderAngle0 = M_PI - shoulderAngle0;
} else {
shoulderAngle0 = shoulderAngle0 - M_PI;
}*/
if (elbow.y < shoulder.y) {
shoulderAngle1 = -shoulderAngle1;
}
double unitZ_data[3] = {0, 0, 1};
matd_t *unitZ = matd_create_data(3, 1, unitZ_data);
matd_t *elbowCheck = matd_crossproduct(elbow_wrist, shoulder_elbow);
double elbowSign = sgn(matd_vec_dot_product(elbowCheck, unitZ));
matd_destroy(unitZ);
matd_destroy(elbowCheck);
servoAngles[1] = shoulderAngle1;
servoAngles[2] = elbowSign*elbowAngle;
//printf("shoulder - %f, elbow - %f\n", servoAngles[1], servoAngles[2]);
matd_destroy(floor_shoulder);
matd_destroy(shoulder_elbow);
//matd_destroy(shoulder_elbow0);
matd_destroy(shoulder_elbow1);
matd_destroy(elbow_wrist);
}
