void create_local_map( MAP2 *map, logtools_lasersens2_data_t data, logtools_rmove2_t movement ) { int i, max_num_linepoints; static logtools_rpos2_t npos = {0.0, 0.0, 0.0 }; static logtools_rmove2_t nmove = {0.0, 0.0, 0.0 }; logtools_rpos2_t rpos; logtools_ivector2_t start, end; logtools_vector2_t lpos; static int first_time = TRUE; static logtools_grid_line_t line; logtools_rmove2_t offset; if (first_time) { max_num_linepoints = (int) (2 * ( settings.local_map_max_range / settings.local_map_resolution )); line.grid = (logtools_ivector2_t *) malloc( max_num_linepoints * sizeof(logtools_ivector2_t) ); first_time = FALSE; } offset.forward = movement.forward; // + data.laser.offset[i].forward; offset.sideward = movement.sideward; // + data.laser.offset[i].sideward; offset.rotation = movement.rotation; // + data.laser.offset[i].rotation; rpos = logtools_rpos2_backwards_with_movement2( npos, offset ); compute_map_pos_from_rpos( rpos, map, &start ); for (i=0;i<data.laser.numvalues;i++) { lpos = logtools_compute_laser_points( rpos, data.laser.val[i], nmove, // data.laser.offset[i], data.laser.angle[i] ); if (compute_map_pos_from_vec2( lpos, map, &end )) { if ( data.laser.val[i]<settings.local_map_max_range && end.x>=0 && end.x<map->mapsize.x && end.y>=0 && end.y<map->mapsize.y ) { set_mapsumpoint( map, end.x, end.y ); set_maphitpoint( map, end.x, end.y, 1.0 ); } } } }
void create_local_treemap( carmen_vascocore_map_t * map, carmen_vascocore_extd_laser_t data, carmen_move_t movement ) { int i; carmen_point_t rpos; carmen_ivec2_t start, end; carmen_vec2_t lpos; rpos = carmen_point_backwards_from_move( movement ); compute_map_pos_from_pos( rpos, *map, &start ); for (i=0;i<data.numvalues;i++) { lpos = carmen_laser_point( rpos, data.val[i], data.angle[i] ); if (compute_map_pos_from_vec2( lpos, *map, &end )) { if ( data.val[i]<carmen_vascocore_settings.local_map_max_range && end.x>=0 && end.x<map->mapsize.x && end.y>=0 && end.y<map->mapsize.y ) { set_mapsumpoint( map, end.x, end.y ); } } } }