unsigned int *get_n_sim_locs_table(unsigned int *size) {
unsigned int i, j, *table;
if (get_mode() == STRATIFY) {
if (val_data->id > -1) {
*size = val_data->maxstratum - val_data->minstratum + 2;
table = (unsigned int *) emalloc(*size * sizeof(int));
for (i = 0; i < *size; i++)
table[i] = 0;
for (i = 0; i < val_data->n_list; i++)
table[val_data->list[i]->u.stratum]++;
} else {
*size = (int) STRATUM(masks[0]->cellmax) -
(int) STRATUM(masks[0]->cellmin) + 1;
table = (unsigned int *) emalloc(*size * sizeof(int));
for (i = 0; i < *size; i++)
table[i] = 0;
for (i = 0; i < masks[0]->rows; i++) {
for (j = 0; j < masks[0]->cols; j++) {
if (!map_cell_is_mv(masks[0], i, j))
table[(int) STRATUM(map_get_cell(masks[0], i, j))]++;
}
}
}
} else {
*size = (int) get_n_vars();
table = (unsigned int *) emalloc(*size * sizeof(int));
for (i = 0; i < *size; i++)
table[i] = n_pred_locs;
}
return table;
}
bool OccupancyMap::lineOfSight(double x1, double y1, double x2, double y2,
double max_occ_dist /* = 0.0 */,
bool allow_unknown /* = false */) const {
if (map_ == NULL) {
return true;
}
if (map_->max_occ_dist < max_occ_dist) {
ROS_ERROR("OccupancyMap::lineOfSight() CSpace has been calculated up to %f, "
"but max_occ_dist=%.2f",
map_->max_occ_dist, max_occ_dist);
ROS_BREAK();
}
// March along the line between (x1, y1) and (x2, y2) until the point passes
// beyond (x2, y2).
double step_size = map_->scale / 2.0;
double dy = y2 - y1;
double dx = x2 - x1;
double t = atan2(dy, dx);
double ct = cos(t);
double st = sin(t);
double mag_sq = dy * dy + dx * dx;
// fprintf(stderr, "t: %f ct: %f st: %f\n", t, ct, st);
double line_x = x1, line_y = y1;
// Terminate when current point defines a vector longer than original vector
while (pow(line_x - x1, 2) + pow(line_y - y1, 2) < mag_sq) {
// fprintf(stderr, "%f %f\n", line_x, line_y);
map_cell_t *cell = map_get_cell(map_, line_x, line_y, 0);
if (cell == NULL) {
// ROS_WARN_THROTTLE(5, "lineOfSight() Beyond map edge");
return false;
} else if (cell->occ_state == map_cell_t::OCCUPIED ||
(!allow_unknown && cell->occ_state == map_cell_t::UNKNOWN) ||
cell->occ_dist < max_occ_dist) {
return false;
}
line_x += step_size * ct;
line_y += step_size * st;
}
return true;
}
/*@ Map.cell(r,c)
*# Returns a cell on the map at row {{r}}, column {{c}} as
*# a {{CellObj}} instance.\n
*# {{r}} must be between {{1}} and {{Map.ROWS}} inclusive.
*# {{c}} must be between {{1}} and {{Map.COLS}} inclusive.
*/
static int get_cell_obj(lua_State *L) {
int r = luaL_checknumber(L,1) - 1;
int c = luaL_checknumber(L,2) - 1;
struct lustate_data *sd = get_state_data(L);
struct map_cell **o;
assert(sd->map);
if(c < 0 || c >= sd->map->nc)
luaL_error(L, "Invalid r value in Map.cell()");
if(r < 0 || r >= sd->map->nr)
luaL_error(L, "Invalid c value in Map.cell()");
o = lua_newuserdata(L, sizeof *o);
luaL_setmetatable(L, "CellObj");
*o = map_get_cell(sd->map, c, r);
return 1;
}
static int get_map_location(DPOINT *loc, int random_path,
unsigned int *row, unsigned int *col) {
/*
* return a not-yet-visited map cell (in *row, *col), or
* 0 if all cells are visited
*/
double value;
int at_end = 0;
if (! random_path) {
if (loc->u.stratum != -2) { /* don't increase when still at (0,0): */
if (*col < masks[0]->cols - 1) /* goto next col in this row: */
(*col)++;
else if (*row < masks[0]->rows - 1) { /* start at next row */
*col = 0;
(*row)++;
} else /* at end of map: */
at_end = 1;
}
} else
at_end = (get_random_cell(masks[0], row, col) == 0);
loc->u.stratum = -1; /* in case of MV in mask map: skip get_est() */
if (!at_end && !map_cell_is_mv(masks[0], *row, *col)) {
value = map_get_cell(masks[0], *row, *col);
if (((int) STRATUM(value)) >= 0)
loc->u.stratum = (int) STRATUM(value);
else {
if (get_mode() == STRATIFY)
pr_warning("negative stratum value %g set to zero", value);
loc->u.stratum = 0;
}
est[0] = value;
if (! DEBUG_TRACE)
print_progress(n_done++, n_pred_locs);
}
return (at_end == 0);
}
static double *get_maskX(DATA **data, DPOINT *p,
unsigned int row, unsigned int col) {
static double *d = NULL;
static int totX = 0, *posMask = NULL;
int i, j, k, l;
static DATA *bl = NULL;
static GRIDMAP **local_masks = NULL;
if (data == NULL) {
if (d != NULL) {
efree(d);
d = NULL;
efree(posMask);
posMask = NULL;
local_masks = NULL;
totX = 0;
}
return NULL;
}
if (d == NULL) { /* first time calling */
for (i = 0, totX = 0; i < get_n_vars(); i++)
totX += data[i]->n_X;
posMask = (int *) emalloc(totX * sizeof(int));
d = (double *) emalloc(totX * sizeof(double));
for (i = 0, k = 0; i < get_n_vars(); i++) {
for (j = 0; j < data[i]->n_X; j++) {
if (data[i]->colX[j] > 0)
posMask[k] = 1;
if (data[i]->colX[j] == 0)
posMask[k] = 0;
if (data[i]->colX[j] < -1)
posMask[k] = -1;
k++;
}
}
if (get_mode() == STRATIFY) {
if (get_n_masks() > 1)
local_masks = masks + 1; /* skip the first (= strata) map */
} else
local_masks = masks;
}
bl = block_discr(bl, get_block_p(), p);
/* bl is a single point-list with p if IS_POINT(p) */
for (i = 0, k = 0; i < get_n_vars(); i++) {
for (j = 0; j < data[i]->n_X; j++) {
if (data[i]->colX[j] < -1) {
/* do eventual block averaging here: */
for (l = 0, d[k] = 0.0; l < bl->n_list; l++)
d[k] += bl->list[l]->u.weight *
calc_polynomial(bl->list[l], data[i]->colX[j]);
}
k++;
}
}
for (i = 0, j = 0; i < totX; i++) {
switch(posMask[i]) {
case -1:
/* is done above */
break;
case 1:
if (map_cell_is_mv(local_masks[j], row, col))
ErrMsg(ER_IMPOSVAL, "missing value in one of the mask maps");
d[i] = map_get_cell(local_masks[j], row, col);
j++;
break;
case 0:
d[i] = (double) 1.0;
break;
default:
ErrMsg(ER_IMPOSVAL, "get_maskX()");
break;
}
}
return d;
}
const map_cell_t* OccupancyMap::getCell(double x, double y) const {
return map_get_cell(map_, x, y, 0);
}
