43 unsigned int src_i_, src_j_;
46 class CachedDistanceMap
49 CachedDistanceMap(
double scale,
double max_dist) :
50 distances_(NULL), scale_(scale), max_dist_(max_dist)
52 cell_radius_ = max_dist / scale;
53 distances_ =
new double *[cell_radius_+2];
54 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++)
59 distances_[i][j] = sqrt(i*i + j*j);
67 for(
int i=0; i<=cell_radius_+1; i++)
68 delete[] distances_[i];
79 bool operator<(
const CellData& a,
const CellData& b)
81 return a.map_->cells[MAP_INDEX(a.map_, a.i_, a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist;
85 get_distance_map(
double scale,
double max_dist)
87 static CachedDistanceMap* cdm = NULL;
89 if(!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist))
93 cdm =
new CachedDistanceMap(scale, max_dist);
99 static unsigned int delta(
const unsigned int x,
const unsigned int y)
108 void enqueue(map_t* map,
unsigned int i,
unsigned int j,
109 unsigned int src_i,
unsigned int src_j,
110 std::priority_queue<CellData>& Q,
111 CachedDistanceMap* cdm,
112 unsigned char* marked)
114 if(marked[MAP_INDEX(map, i, j)])
117 unsigned int di = delta(i, src_i);
118 unsigned int dj = delta(j, src_j);
119 double distance = cdm->distances_[di][dj];
121 if(distance > cdm->cell_radius_)
124 map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;
135 marked[MAP_INDEX(map, i, j)] = 1;
139 void map_update_cspace(map_t *map,
double max_occ_dist)
141 unsigned char* marked;
142 std::priority_queue<CellData> Q;
144 marked =
new unsigned char[map->size_x*map->size_y];
145 memset(marked, 0,
sizeof(
unsigned char) * map->size_x*map->size_y);
147 map->max_occ_dist = max_occ_dist;
149 CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist);
154 for(
int i=0; i<map->size_x; i++)
156 cell.src_i_ = cell.i_ = i;
157 for(
int j=0; j<map->size_y; j++)
159 if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1)
161 map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
162 cell.src_j_ = cell.j_ = j;
163 marked[MAP_INDEX(map, i, j)] = 1;
167 map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
173 CellData current_cell = Q.top();
174 if(current_cell.i_ > 0)
175 enqueue(map, current_cell.i_-1, current_cell.j_,
176 current_cell.src_i_, current_cell.src_j_,
178 if(current_cell.j_ > 0)
179 enqueue(map, current_cell.i_, current_cell.j_-1,
180 current_cell.src_i_, current_cell.src_j_,
182 if((
int)current_cell.i_ < map->size_x - 1)
183 enqueue(map, current_cell.i_+1, current_cell.j_,
184 current_cell.src_i_, current_cell.src_j_,
186 if((
int)current_cell.j_ < map->size_y - 1)
187 enqueue(map, current_cell.i_, current_cell.j_+1,
188 current_cell.src_i_, current_cell.src_j_,
200 void map_update_cspace(map_t *map,
double max_occ_dist)
206 map_cell_t *cell, *ncell;
208 map->max_occ_dist = max_occ_dist;
209 s = (int) ceil(map->max_occ_dist / map->scale);
212 for (j = 0; j < map->size_y; j++)
214 for (i = 0; i < map->size_x; i++)
216 cell = map->cells + MAP_INDEX(map, i, j);
217 cell->occ_dist = map->max_occ_dist;
222 for (j = 0; j < map->size_y; j++)
224 for (i = 0; i < map->size_x; i++)
226 cell = map->cells + MAP_INDEX(map, i, j);
227 if (cell->occ_state != +1)
233 for (nj = -s; nj <= +s; nj++)
235 for (ni = -s; ni <= +s; ni++)
237 if (!MAP_VALID(map, i + ni, j + nj))
240 ncell = map->cells + MAP_INDEX(map, i + ni, j + nj);
241 d = map->scale * sqrt(ni * ni + nj * nj);
243 if (d < ncell->occ_dist)
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.