void vx_util_lookat(double * _eye, double * _lookat, double * _up, double * _out44)
{
zarray_t * fp = zarray_create(sizeof(matd_t*));
matd_t * eye = matd_create_data(3,1, _eye);
zarray_add(fp, &eye);
matd_t * lookat = matd_create_data(3,1, _lookat);
zarray_add(fp, &lookat);
matd_t * up = matd_create_data(3,1, _up);
zarray_add(fp, &up);
up = matd_vec_normalize(up);
zarray_add(fp, &up); // note different pointer than before!
matd_t * tmp1 = matd_subtract(lookat, eye); zarray_add(fp, &tmp1);
matd_t * f = matd_vec_normalize(tmp1); zarray_add(fp, &f);
matd_t * s = matd_crossproduct(f, up); zarray_add(fp, &s);
matd_t * u = matd_crossproduct(s, f); zarray_add(fp, &u);
matd_t * M = matd_create(4,4); // set the rows of M with s, u, -f
zarray_add(fp, &M);
memcpy(M->data,s->data,3*sizeof(double));
memcpy(M->data + 4,u->data,3*sizeof(double));
memcpy(M->data + 8,f->data,3*sizeof(double));
for (int i = 0; i < 3; i++)
M->data[2*4 +i] *= -1;
M->data[3*4 + 3] = 1.0;
matd_t * T = matd_create(4,4);
T->data[0*4 + 3] = -eye->data[0];
T->data[1*4 + 3] = -eye->data[1];
T->data[2*4 + 3] = -eye->data[2];
T->data[0*4 + 0] = 1;
T->data[1*4 + 1] = 1;
T->data[2*4 + 2] = 1;
T->data[3*4 + 3] = 1;
zarray_add(fp, &T);
matd_t * MT = matd_op("MM",M,T);
zarray_add(fp, &MT);
memcpy(_out44, MT->data, 16*sizeof(double));
// cleanup
zarray_vmap(fp, matd_destroy);
zarray_destroy(fp);
}
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;
}
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 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);
}