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_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); }
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; }
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; }