static void on_pose(const lcm_recv_buf_t *rbuf, const char *channel, const botlcm_pose_t *pose, void *user_data) { RendererCar *self = (RendererCar*) user_data; ViewHandler *vhandler = self->viewer->view_handler; double lastpos[3] = {0,0,0}; if (bot_ptr_circular_size(self->path)) memcpy(lastpos, bot_ptr_circular_index(self->path, 0), 3 * sizeof(double)); double diff[3]; bot_vector_subtract_3d(pose->pos, lastpos, diff); if (bot_vector_magnitude_3d(diff) > 2.0) { // clear the buffer if we jump bot_ptr_circular_clear(self->path); } if (bot_vector_magnitude_3d(diff) > 0.1 || bot_ptr_circular_size(self->path)==0) { double *p = (double*) calloc(3, sizeof(double)); memcpy(p, pose->pos, sizeof(double)*3); bot_ptr_circular_add(self->path, p); } if (vhandler && vhandler->update_follow_target && !self->teleport_car) { vhandler->update_follow_target(vhandler, pose->pos, pose->orientation); } if (!self->did_teleport) on_find_button(NULL, self); self->did_teleport = 1; int64_t dt = pose->utime - self->last_pose.utime; double r = bot_conf_get_double_or_default (self->config, "renderer_car.wheel_radius", 0.3); if (self->last_pose.utime) { int i; for (i = 0; i < 4; i++) { self->wheelpos[i] += self->wheelspeeds[i] * dt * 1e-6 / r; self->wheelpos[i] = bot_mod2pi (self->wheelpos[i]); } } memcpy (&self->last_pose, &pose, sizeof (botlcm_pose_t)); viewer_request_redraw(self->viewer); }
static void update_gl_matrices(BotViewer *viewer, BotViewHandler *vhandler) { BotDefaultViewHandler *dvh = (BotDefaultViewHandler*) vhandler->user; glGetIntegerv(GL_VIEWPORT, dvh->viewport); dvh->width = dvh->viewport[2]; dvh->height = dvh->viewport[3]; dvh->aspect_ratio = ((double) dvh->width) / dvh->height; glMatrixMode (GL_PROJECTION); glLoadIdentity (); if (dvh->projection_type == BOT_VIEW_ORTHOGRAPHIC) { double le[3]; bot_vector_subtract_3d (dvh->eye, dvh->lookat, le); double dist = bot_vector_magnitude_3d (le) * tan (dvh->fov_degrees * M_PI / 180 / 2); glOrtho (-dist*dvh->aspect_ratio, dist*dvh->aspect_ratio, -dist, dist, -10*dist, EYE_MAX_DIST * 2); } else { gluPerspective (dvh->fov_degrees, dvh->aspect_ratio, 0.1, EYE_MAX_DIST * 2); } glGetDoublev(GL_PROJECTION_MATRIX, dvh->projection_matrix); glMatrixMode (GL_MODELVIEW); glLoadIdentity (); glMultMatrixd(dvh->model_matrix); }
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 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); } }
static void on_laser(const lcm_recv_buf_t *rbuf, const char *channel, const bot_core_planar_lidar_t *msg, void *user) { RendererLaser *self = (RendererLaser*) user; g_assert(self); /* get the laser channel object based on channel name */ laser_channel *lchan = g_hash_table_lookup(self->channels_hash, channel); if (lchan == NULL) { /* allocate and initialize laser channel structure */ lchan = (laser_channel*) calloc(1, sizeof(laser_channel)); g_assert(lchan); lchan->enabled = 1; lchan->name = bot_param_get_planar_lidar_name_from_lcm_channel(self->bot_param, channel); lchan->channel = strdup(channel); lchan->projector = laser_projector_new(self->bot_param, self->bot_frames, lchan->name, 1); char param_prefix[1024]; bot_param_get_planar_lidar_prefix(NULL, lchan->name, param_prefix, sizeof(param_prefix)); char color_key[1024]; sprintf(color_key, "%s.viewer_color", param_prefix); double color[3]; int color_size = bot_param_get_double_array(self->bot_param, color_key, color, 3); if (color_size != 3) { ERR("Error: Missing or funny color for planar LIDAR " "configuration key: '%s'\n", color_key); lchan->color[0] = 1; lchan->color[1] = 1; lchan->color[2] = 1; } else { lchan->color[0] = color[0]; lchan->color[1] = color[1]; lchan->color[2] = color[2]; } lchan->scans = bot_ptr_circular_new(self->param_max_buffer_size, laser_scan_destroy, NULL); g_assert(lchan->scans); /* add laser channel to hash table and array */ g_hash_table_insert(self->channels_hash, lchan->channel, lchan); g_ptr_array_add(self->channels, lchan); /* add check box */ if (self->viewer) bot_gtk_param_widget_add_booleans(self->pw, 0, lchan->name, lchan->enabled, NULL); } /* TODO: Optimization - allocate space for local points from a circular buffer instead of calling calloc for each scan */ laser_projected_scan *lscan = laser_create_projected_scan_from_planar_lidar(lchan->projector, msg, bot_frames_get_root_name(self->bot_frames)); if (lscan == NULL) return; //probably didn't have a pose message yet... if(lchan->scans->capacity != self->param_max_buffer_size){ bot_ptr_circular_resize(lchan->scans, self->param_max_buffer_size); } if (bot_ptr_circular_size(lchan->scans) > 0) { laser_projected_scan *last_scan = bot_ptr_circular_index(lchan->scans, 0); /* check for a large time difference between scans (typical when jumping around an LCM log file) */ gboolean time_jump = FALSE; int64_t dt = msg->utime - last_scan->utime; if (dt < -OLD_HISTORY_THRESHOLD || dt > OLD_HISTORY_THRESHOLD) time_jump = TRUE; /* spacial decimation */ gboolean stationary = FALSE; BotTrans delta; if (self->param_spacial_decimate && self->param_scan_memory > 10) { bot_trans_invert_and_compose(&lscan->origin, &last_scan->origin, &delta); double dist = bot_vector_magnitude_3d(delta.trans_vec); double rot; double axis[3]; bot_quat_to_angle_axis(delta.rot_quat, &rot, axis); if (dist < SPACIAL_DECIMATION_LIMIT && rot < ANGULAR_DECIMATION_LIMIT) { stationary = TRUE; } } if (stationary) { laser_scan_destroy(NULL, lscan); return; } } bot_ptr_circular_add(lchan->scans, lscan); if (self->viewer) bot_viewer_request_redraw(self->viewer); return; }