44 unsigned int src_i_, src_j_;
47 class CachedDistanceMap
50 CachedDistanceMap(
double scale,
double max_dist)
51 : distances_(NULL), scale_(scale), max_dist_(max_dist)
53 cell_radius_ = max_dist / scale;
54 distances_ =
new double *[cell_radius_ + 2];
55 for (
int i = 0; i <= cell_radius_ + 1; i++) {
56 distances_[i] =
new double[cell_radius_ + 2];
57 for (
int j = 0; j <= cell_radius_ + 1; j++) {
58 distances_[i][j] = sqrt(i * i + j * j);
63 CachedDistanceMap(
const CachedDistanceMap &other)
64 : distances_(NULL), scale_(other.scale_), max_dist_(other.max_dist_)
66 cell_radius_ = other.cell_radius_;
67 distances_ =
new double *[cell_radius_ + 2];
68 for (
int i = 0; i <= cell_radius_ + 1; i++) {
69 distances_[i] =
new double[cell_radius_ + 2];
70 for (
int j = 0; j <= cell_radius_ + 1; j++) {
71 distances_[i][j] = other.distances_[i][j];
79 for (
int i = 0; i <= cell_radius_ + 1; i++)
80 delete[] distances_[i];
86 operator=(
const CachedDistanceMap &other)
92 for (
int i = 0; i <= cell_radius_ + 1; i++)
93 delete[] distances_[i];
98 scale_ = other.scale_;
99 max_dist_ = other.max_dist_;
101 cell_radius_ = other.cell_radius_;
102 distances_ =
new double *[cell_radius_ + 2];
103 for (
int i = 0; i <= cell_radius_ + 1; i++) {
104 distances_[i] =
new double[cell_radius_ + 2];
105 for (
int j = 0; j <= cell_radius_ + 1; j++) {
106 distances_[i][j] = other.distances_[i][j];
120 operator<(
const CellData &a,
const CellData &b)
122 return a.map_->cells[MAP_INDEX(a.map_, a.i_, a.j_)].occ_dist
123 > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist;
127 get_distance_map(
double scale,
double max_dist)
129 static CachedDistanceMap *cdm = NULL;
131 if (!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist)) {
134 cdm =
new CachedDistanceMap(scale, max_dist);
141 delta(
const unsigned int x,
const unsigned int y)
155 std::priority_queue<CellData> &Q,
156 CachedDistanceMap * cdm,
157 unsigned char * marked)
159 if (marked[MAP_INDEX(map, i, j)])
162 unsigned int di = delta(i, src_i);
163 unsigned int dj = delta(j, src_j);
164 double distance = cdm->distances_[di][dj];
166 if (distance > cdm->cell_radius_)
169 map->cells[MAP_INDEX(map, i, j)].occ_dist =
distance * map->scale;
180 marked[MAP_INDEX(map, i, j)] = 1;
185 map_update_cspace(map_t *map,
double max_occ_dist)
187 unsigned char * marked;
188 std::priority_queue<CellData> Q;
190 marked =
new unsigned char[map->size_x * map->size_y];
191 memset(marked, 0,
sizeof(
unsigned char) * map->size_x * map->size_y);
193 map->max_occ_dist = max_occ_dist;
195 CachedDistanceMap *cdm = get_distance_map(map->scale, map->max_occ_dist);
200 for (
int i = 0; i < map->size_x; i++) {
201 cell.src_i_ = cell.i_ = i;
202 for (
int j = 0; j < map->size_y; j++) {
203 if (map->cells[MAP_INDEX(map, i, j)].occ_state == +1) {
204 map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
205 cell.src_j_ = cell.j_ = j;
206 marked[MAP_INDEX(map, i, j)] = 1;
209 map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
214 CellData current_cell = Q.top();
215 if (current_cell.i_ > 0)
224 if (current_cell.j_ > 0)
233 if ((
int)current_cell.i_ < map->size_x - 1)
242 if ((
int)current_cell.j_ < map->size_y - 1)
261 void map_update_cspace(map_t *map,
double max_occ_dist)
267 map_cell_t *cell, *ncell;
269 map->max_occ_dist = max_occ_dist;
270 s = (int) ceil(map->max_occ_dist / map->scale);
273 for (j = 0; j < map->size_y; j++)
275 for (i = 0; i < map->size_x; i++)
277 cell = map->cells + MAP_INDEX(map, i, j);
278 cell->occ_dist = map->max_occ_dist;
283 for (j = 0; j < map->size_y; j++)
285 for (i = 0; i < map->size_x; i++)
287 cell = map->cells + MAP_INDEX(map, i, j);
288 if (cell->occ_state != +1)
294 for (nj = -s; nj <= +s; nj++)
296 for (ni = -s; ni <= +s; ni++)
298 if (!MAP_VALID(map, i + ni, j + nj))
301 ncell = map->cells + MAP_INDEX(map, i + ni, j + nj);
302 d = map->scale * sqrt(ni * ni + nj * nj);
304 if (d < ncell->occ_dist)
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
bool operator<(const Uuid &uuid, const Uuid &other) noexcept
Compare two Uuids.