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