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