static double pick_query(Viewer *viewer, EventHandler *ehandler, const double ray_start[3], const double ray_dir[3])
{
RendererCar *self = (RendererCar*) ehandler->user;
botlcm_pose_t pose;
if (atrans_get_local_pose (self->atrans, &pose) < 0)
return -1;
double ray_start_body[3];
bot_vector_subtract_3d (ray_start, pose.pos, ray_start_body);
bot_quat_rotate_rev (pose.orientation, ray_start_body);
double ray_dir_body[3] = { ray_dir[0], ray_dir[1], ray_dir[2] };
bot_quat_rotate_rev (pose.orientation, ray_dir_body);
bot_vector_normalize_3d (ray_dir_body);
point3d_t car_pos_body = { 1.3, 0, 1 };
point3d_t box_size = { 4.6, 2, 1.4 };
double t = geom_ray_axis_aligned_box_intersect_3d (POINT3D(ray_start_body),
POINT3D (ray_dir_body), &car_pos_body, &box_size, NULL);
if (isfinite (t)) return t;
self->ehandler.hovering = 0;
return -1;
}
// column-major (opengl compatible) order. We need this because we
// need to be able to recompute the model matrix without requiring the
// correct GL context to be current.
static void look_at_to_matrix(const double eye[3], const double lookat[3], const double _up[3], double M[16])
{
double up[3];
memcpy(up, _up, 3 * sizeof(double));
bot_vector_normalize_3d(up);
double f[3];
bot_vector_subtract_3d(lookat, eye, f);
bot_vector_normalize_3d(f);
double s[3], u[3];
bot_vector_cross_3d(f, up, s);
bot_vector_cross_3d(s, f, u);
// row-major
double R[16];
memset(R, 0, sizeof(R));
R[0] = s[0];
R[1] = s[1];
R[2] = s[2];
R[4] = u[0];
R[5] = u[1];
R[6] = u[2];
R[8] = -f[0];
R[9] = -f[1];
R[10] = -f[2];
R[15] = 1;
double T[16];
memset(T, 0, sizeof(T));
T[0] = 1;
T[3] = -eye[0];
T[5] = 1;
T[7] = -eye[1];
T[10] = 1;
T[11] = -eye[2];
T[15] = 1;
double MT[16];
bot_matrix_multiply_4x4_4x4(R, T, MT);
bot_matrix_transpose_4x4d(MT, M);
}
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 update_follow_target(BotViewHandler *vhandler, const double pos[3], const double quat[4])
{
BotDefaultViewHandler *dvh = (BotDefaultViewHandler*) vhandler->user;
if (dvh->have_last && (vhandler->follow_mode & BOT_FOLLOW_YAW)) {
// compute the vectors from the vehicle to the lookat and eye
// point and then project them given the new position of the car/
double v2eye[3];
bot_vector_subtract_3d(dvh->lastpos, dvh->eye, v2eye);
double v2look[3];
bot_vector_subtract_3d(dvh->lastpos, dvh->lookat, v2look);
double vxy[3] = { 1, 0, 0 };
bot_quat_rotate (quat, vxy);
vxy[2] = 0;
// where was the car pointing last time?
double oxy[3] = { 1, 0, 0 };
bot_quat_rotate (dvh->lastquat, oxy);
oxy[2] = 0;
double theta = bot_vector_angle_2d (oxy, vxy);
double zaxis[3] = { 0, 0, 1 };
double q[4];
bot_angle_axis_to_quat (theta, zaxis, q);
bot_quat_rotate(q, v2look);
bot_vector_subtract_3d(pos, v2look, dvh->lookat);
bot_quat_rotate(q, v2eye);
bot_vector_subtract_3d(pos, v2eye, dvh->eye);
bot_quat_rotate (q, dvh->up);
// the above algorithm "builds in" a BOT_FOLLOW_POS behavior.
} else if (dvh->have_last && (vhandler->follow_mode & BOT_FOLLOW_POS)) {
double dpos[3];
for (int i = 0; i < 3; i++)
dpos[i] = pos[i] - dvh->lastpos[i];
for (int i = 0; i < 3; i++) {
dvh->eye[i] += dpos[i];
dvh->lookat[i] += dpos[i];
}
} else {
// when the follow target moves, we want rotations to still behave
// correctly. Rotations use the lookat point as the center of rotation,
// so adjust the lookat point so that it is on the same plane as the
// follow target.
double look_dir[3];
bot_vector_subtract_3d(dvh->lookat, dvh->eye, look_dir);
bot_vector_normalize_3d(look_dir);
if (fabs(look_dir[2]) > 0.0001) {
double dist = (dvh->lastpos[2] - dvh->lookat[2]) / look_dir[2];
for (int i = 0; i < 3; i++)
dvh->lookat[i] += dist * look_dir[i];
}
}
look_at_changed(dvh);
dvh->have_last = 1;
memcpy(dvh->lastpos, pos, 3 * sizeof(double));
memcpy(dvh->lastquat, quat, 4 * sizeof(double));
}
static int mouse_motion (BotViewer *viewer, BotEventHandler *ehandler,
const double ray_start[3], const double ray_dir[3], const GdkEventMotion *event)
{
BotDefaultViewHandler *dvh = (BotDefaultViewHandler*) ehandler->user;
double dy = event->y - dvh->last_mouse_y;
double dx = event->x - dvh->last_mouse_x;
double look[3];
bot_vector_subtract_3d (dvh->lookat, dvh->eye, look);
if (event->state & GDK_BUTTON3_MASK) {
double xy_len = bot_vector_magnitude_2d (look);
double init_elevation = atan2 (look[2], xy_len);
double left[3];
bot_vector_cross_3d (dvh->up, look, left);
bot_vector_normalize_3d (left);
double delevation = -dy * 0.005;
if (delevation + init_elevation < -M_PI/2) {
delevation = -M_PI/2 - init_elevation;
}
if (delevation + init_elevation > M_PI/8) {
delevation = M_PI/8 - init_elevation;
}
double q[4];
bot_angle_axis_to_quat(-delevation, left, q);
double newlook[3];
memcpy (newlook, look, sizeof (newlook));
bot_quat_rotate (q, newlook);
double dazimuth = -dx * 0.01;
double zaxis[] = { 0, 0, 1 };
bot_angle_axis_to_quat (dazimuth, zaxis, q);
bot_quat_rotate (q, newlook);
bot_quat_rotate (q, left);
bot_vector_subtract_3d (dvh->lookat, newlook, dvh->eye);
bot_vector_cross_3d (newlook, left, dvh->up);
look_at_changed(dvh);
}
else if (event->state & GDK_BUTTON2_MASK) {
double init_eye_dist = bot_vector_magnitude_3d (look);
double ded = pow (10, dy * 0.01);
double eye_dist = init_eye_dist * ded;
if (eye_dist > EYE_MAX_DIST)
eye_dist = EYE_MAX_DIST;
else if (eye_dist < EYE_MIN_DIST)
eye_dist = EYE_MIN_DIST;
double le[3];
memcpy (le, look, sizeof (le));
bot_vector_normalize_3d (le);
bot_vector_scale_3d (le, eye_dist);
bot_vector_subtract_3d (dvh->lookat, le, dvh->eye);
look_at_changed(dvh);
}
else if (event->state & GDK_BUTTON1_MASK) {
double dx = event->x - dvh->last_mouse_x;
double dy = event->y - dvh->last_mouse_y;
window_space_pan(dvh, dvh->manipulation_point, dx, dy, 1);
}
dvh->last_mouse_x = event->x;
dvh->last_mouse_y = event->y;
bot_viewer_request_redraw(viewer);
return 1;
}
/** 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);
}
}
