void bot_trans_set_from_velocities(BotTrans *dest, const double angular_rate[3], const double velocity[3], double dt) { //see Frazolli notes, Aircraft Stability and Control, lecture 2, page 15 if (dt == 0) { bot_trans_set_identity(dest); return; } double identity_quat[4] = { 1, 0, 0, 0 }; double norm_angular_rate = bot_vector_magnitude_3d(angular_rate); if (norm_angular_rate == 0) { double delta[3]; memcpy(delta, velocity, 3 * sizeof(double)); bot_vector_scale_3d(delta, dt); bot_trans_set_from_quat_trans(dest, identity_quat, delta); return; } //"exponential of a twist: R=exp(skew(omega*t)); //rescale vel, omega, t so ||omega||=1 //trans = (I-R)*(omega \cross v) + (omega \dot v) *omega* t // term2 term3 // R*(omega \cross v) // term1 //rescale double t = dt * norm_angular_rate; double omega[3], vel[3]; memcpy(omega, angular_rate, 3 * sizeof(double)); memcpy(vel, velocity, 3 * sizeof(double)); bot_vector_scale_3d(omega, 1.0/norm_angular_rate); bot_vector_scale_3d(vel, 1.0/norm_angular_rate); //compute R (quat in our case) bot_angle_axis_to_quat(t, omega, dest->rot_quat); //cross and dot products double omega_cross_vel[3]; bot_vector_cross_3d(omega, vel, omega_cross_vel); double omega_dot_vel = bot_vector_dot_3d(omega, vel); double term1[3]; double term2[3]; double term3[3]; //(I-R)*(omega \cross v) = term2 memcpy(term1, omega_cross_vel, 3 * sizeof(double)); bot_quat_rotate(dest->rot_quat, term1); bot_vector_subtract_3d(omega_cross_vel, term1, term2); //(omega \dot v) *omega* t memcpy(term3, omega, 3 * sizeof(double)); bot_vector_scale_3d(term3, omega_dot_vel * t); bot_vector_add_3d(term2, term3, dest->trans_vec); }
static void zoom_ratio(BotDefaultViewHandler *dvh, double ratio) { double le[3]; bot_vector_subtract_3d (dvh->eye, dvh->lookat, le); double eye_dist = bot_vector_magnitude_3d (le); eye_dist *= ratio; if (eye_dist < EYE_MIN_DIST) return; if (eye_dist > EYE_MAX_DIST) return; bot_vector_normalize_3d (le); bot_vector_scale_3d (le, eye_dist); bot_vector_add_3d (le, dvh->lookat, dvh->eye); look_at_changed(dvh); }
static void on_find_button(GtkWidget *button, RendererCar *self) { ViewHandler *vhandler = self->viewer->view_handler; double eye[3]; double lookat[3]; double up[3]; vhandler->get_eye_look(vhandler, eye, lookat, up); double diff[3]; bot_vector_subtract_3d(eye, lookat, diff); double pos[3] = { 0, 0, 0 }; atrans_vehicle_pos_local(self->atrans, pos); bot_vector_add_3d(pos, diff, eye); vhandler->set_look_at(vhandler, eye, pos, up); viewer_request_redraw(self->viewer); }
/** Given a coordinate in scene coordinates dq, modify the camera such that the screen projection of point dq moves (x,y) pixels. **/ static void window_space_pan(BotDefaultViewHandler *dvh, double dq[], double x, double y, int preserveZ) { double orig_eye[3], orig_lookat[3]; memcpy(orig_eye, dvh->eye, 3 * sizeof(double)); memcpy(orig_lookat, dvh->lookat, 3 * sizeof(double)); y *= -1; // y is upside down... double left[3], up[3]; // compute left and up vectors if (!preserveZ) { // constraint translation such that the distance between the // eye and the lookat does not change; i.e., that the motion // is upwards and leftwards only. double look_vector[3]; bot_vector_subtract_3d(dvh->lookat, dvh->eye, look_vector); bot_vector_normalize_3d(look_vector); bot_vector_cross_3d(dvh->up, look_vector, left); memcpy(up, dvh->up, 3 * sizeof(double)); } else { // abuse the left and up vectors: change them to xhat, yhat... this ensures // that pan does not affect the camera Z height. left[0] = 1; left[1] = 0; left[2] = 0; up[0] = 0; up[1] = 1; up[2] = 0; } double A[4]; double B[2] = { x, y }; build_pan_jacobian(dvh, dq, up, left, A); double detOriginal = A[0]*A[3] - A[1]*A[2]; // Ax = b, where x = how much we should move in the up and left directions. double Ainverse[4] = { 0, 0, 0, 0 }; bot_matrix_inverse_2x2d(A, Ainverse); double sol[2]; bot_matrix_vector_multiply_2x2_2d(Ainverse, B, sol); double motionup[3], motionleft[3], motion[3]; memcpy(motionup, up, 3 * sizeof(double)); bot_vector_scale_3d(motionup, sol[0]); memcpy(motionleft, left, 3 * sizeof(double)); bot_vector_scale_3d(motionleft, sol[1]); bot_vector_add_3d(motionup, motionleft, motion); double magnitude = bot_vector_magnitude_3d(motion); double new_magnitude = fmax(fmin(magnitude,MAX_MOTION_MAGNITUDE),MIN_MOTION_MAGNITUDE); //bot_vector_normalize_3d(motion); // if magnitude is zero it will return nan's bot_vector_scale_3d(motion,new_magnitude/fmax(magnitude,MIN_MOTION_MAGNITUDE)); double neweye[3], newlookat[3]; bot_vector_subtract_3d(dvh->eye, motion, neweye); bot_vector_subtract_3d(dvh->lookat, motion, newlookat); memcpy(dvh->eye, neweye, sizeof(double)*3); memcpy(dvh->lookat, newlookat, sizeof(double)*3); look_at_changed(dvh); build_pan_jacobian(dvh, dq, up, left, A); // if the projection is getting sketchy, and it's getting worse, // then just reject it. this is better than letting the // projection become singular! (This only happens with preserveZ?) double detNew = A[0]*A[3] - A[1]*A[2]; //printf(" %15f %15f\n", detOriginal, detNew); //if (fabs(detNew) < 0.01 && fabs(detNew) <= fabs(detOriginal)) { if ((fabs(detNew) < 25 )||(fabs(detOriginal) < 25 )) { memcpy(dvh->eye, orig_eye, 3 * sizeof(double)); memcpy(dvh->lookat, orig_lookat, 3 * sizeof(double)); look_at_changed(dvh); printf("skipping pan: %15f %15f\n", detOriginal, detNew); } }