void default_cam_mgr_rotate(default_cam_mgr_t * state, double *q, uint32_t animate_ms)
{
zarray_t * fp = zarray_create(sizeof(matd_t*));
matd_t * eye = matd_create_data(3,1,state->eye1); zarray_add(fp, &eye);
matd_t *lookat = matd_create_data(3,1,state->lookat1); zarray_add(fp, &lookat);
matd_t *up = matd_create_data(3,1,state->up1); zarray_add(fp, &up);
matd_t * toEye = matd_subtract(eye, lookat); zarray_add(fp, &toEye);
matd_t * nextToEye = matd_create(3,1); zarray_add(fp, &nextToEye);
vx_util_quat_rotate(q, toEye->data, nextToEye->data);
matd_t * nextEye = matd_add(lookat, nextToEye); zarray_add(fp, &nextEye);
matd_t * nextUp = matd_copy(up); zarray_add(fp, &nextUp);
vx_util_quat_rotate(q, up->data, nextUp->data);
// copy back results
memcpy(state->eye1, nextEye->data, sizeof(double)*3);
memcpy(state->up1, nextUp->data, sizeof(double)*3);
state->mtime1 = vx_util_mtime() + animate_ms;
// Disable any prior fit command
default_destroy_fit(state);
// cleanup
zarray_vmap(fp, matd_destroy);
zarray_destroy(fp);
default_cam_mgr_follow_disable(state);
}
void vx_util_unproject(double * winxyz, double * model_matrix, double * projection_matrix, int * viewport, double * vec3_out)
{
zarray_t * fp = zarray_create(sizeof(matd_t*));
matd_t * mm = matd_create_data(4, 4, model_matrix);
zarray_add(fp, &mm);
matd_t * pm = matd_create_data(4, 4, projection_matrix);
zarray_add(fp, &pm);
matd_t *invpm = matd_op("(MM)^-1", pm, mm);
zarray_add(fp, &invpm);
double v[4] = { 2*(winxyz[0]-viewport[0]) / viewport[2] - 1,
2*(winxyz[1]-viewport[1]) / viewport[3] - 1,
2*winxyz[2] - 1,
1 };
matd_t * vm = matd_create_data(4, 1, v);
zarray_add(fp, &vm);
matd_t * objxyzh = matd_op("MM", invpm, vm);
zarray_add(fp, &objxyzh);
vec3_out[0] = objxyzh->data[0] / objxyzh->data[3];
vec3_out[1] = objxyzh->data[1] / objxyzh->data[3];
vec3_out[2] = objxyzh->data[2] / objxyzh->data[3];
// cleanup
zarray_vmap(fp, matd_destroy);
zarray_destroy(fp);
}
void vx_util_project(double * xyz, double * M44, double * P44, int * viewport, double * win_out3)
{
zarray_t * fp = zarray_create(sizeof(matd_t*));
matd_t * M = matd_create_data(4,4, M44); zarray_add(fp, &M);
matd_t * P = matd_create_data(4,4, P44); zarray_add(fp, &P);
matd_t * xyzp = matd_create(4,1); zarray_add(fp, &xyzp);
memcpy(xyzp->data, xyz, 3*sizeof(double));
xyzp->data[3] = 1.0;
matd_t * p = matd_op("MMM", P, M, xyzp); zarray_add(fp, &p);
p->data[0] = p->data[0] / p->data[3];
p->data[1] = p->data[1] / p->data[3];
p->data[2] = p->data[2] / p->data[3];
double res[] = { viewport[0] + viewport[2]*(p->data[0]+1)/2.0,
viewport[1] + viewport[3]*(p->data[1]+1)/2.0,
(viewport[2] + 1)/2.0 };
memcpy(win_out3, res, 3*sizeof(double));
// cleanup
zarray_vmap(fp, matd_destroy);
zarray_destroy(fp);
}
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);
}
BoundingBox::BoundingBox() {
double vec[3] = {0, 0, 0};
this->pos = matd_create_data(3, 1, vec);
vec[0] = 1;
this->ux = matd_create_data(3, 1, vec);
vec[0] = 0;
vec[1] = 1;
this->uy = matd_create_data(3, 1, vec);
vec[1] = 0;
vec[2] = 1;
this->uz = matd_create_data(3, 1, vec);
this->tx = 0;
this->ty = 0;
this->tz = 0;
}
void vx_util_angle_axis_to_quat(double theta, double * axis3, double * qout)
{
zarray_t * fp = zarray_create(sizeof(matd_t*));
matd_t * axis = matd_create_data(3,1, axis3); zarray_add(fp, &axis);
matd_t * axis_norm = matd_vec_normalize(axis); zarray_add(fp, &axis_norm);
qout[0] = cos(theta/2);
double s = sin(theta/2);
qout[1] = axis_norm->data[0] * s;
qout[2] = axis_norm->data[1] * s;
qout[3] = axis_norm->data[2] * s;
// cleanup
zarray_vmap(fp, matd_destroy);
zarray_destroy(fp);
}
void project_measurements_through_homography(matd_t* H, vx_buffer_t* buf,
zarray_t* pix_found, int size)
{
int npoints = NUM_CHART_BLOBS * 2; // line per chart blob
float points[npoints*3];
float* real_world_coords;
if(size == NUM_TARGETS) real_world_coords = target_coords;
else if(size == NUM_CHART_BLOBS) real_world_coords = chart_coords;
else assert(0);
for(int i = 0; i < size; i++) {
// run each real world point through homography and add to buf
double tmp[3] = {real_world_coords[i*2], real_world_coords[i*2+1], 1};
matd_t* xy_matrix = matd_create_data(3,1,tmp);
matd_t* pix_estimated = matd_op("(M)*M",H, xy_matrix);
MATD_EL(pix_estimated,0,0) /= MATD_EL(pix_estimated,2, 0);
MATD_EL(pix_estimated,1,0) /= MATD_EL(pix_estimated,2, 0);
vx_buffer_add_back(buf,
vxo_pix_coords(VX_ORIGIN_BOTTOM_LEFT,
vxo_chain(vxo_mat_translate3(MATD_EL(pix_estimated,0,0), MATD_EL(pix_estimated,1,0), 0),
vxo_mat_scale(2.0),
vxo_circle(vxo_mesh_style(vx_green)))));
// create endpoints for lines
loc_t pos;
zarray_get(pix_found, i, &pos); //
points[6*i + 0] = pos.x;
points[6*i + 1] = pos.y;
points[6*i + 2] = 0;
points[6*i + 3] = MATD_EL(pix_estimated,0,0);
points[6*i + 4] = MATD_EL(pix_estimated,1,0);
points[6*i + 5] = 0;
}
// make lines
vx_resc_t *verts = vx_resc_copyf(points, npoints*3);
vx_buffer_add_back(buf, vxo_pix_coords(VX_ORIGIN_BOTTOM_LEFT,
vxo_lines(verts, npoints, GL_LINES,
vxo_points_style(vx_blue, 2.0f))));
}
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;
}
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);
}
