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 void enqueue(map_t* map,
unsigned int i,
unsigned int j,
100 unsigned int src_i,
unsigned int src_j,
101 std::priority_queue<CellData>& Q,
102 CachedDistanceMap* cdm,
103 unsigned char* marked)
105 if(marked[MAP_INDEX(map, i, j)])
108 unsigned int di = abs(i - src_i);
109 unsigned int dj = abs(j - src_j);
110 double distance = cdm->distances_[di][dj];
112 if(distance > cdm->cell_radius_)
115 map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;
126 marked[MAP_INDEX(map, i, j)] = 1;
130 void map_update_cspace(map_t *map,
double max_occ_dist)
132 unsigned char* marked;
133 std::priority_queue<CellData> Q;
135 marked =
new unsigned char[map->size_x*map->size_y];
136 memset(marked, 0,
sizeof(
unsigned char) * map->size_x*map->size_y);
138 map->max_occ_dist = max_occ_dist;
140 CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist);
145 for(
int i=0; i<map->size_x; i++)
147 cell.src_i_ = cell.i_ = i;
148 for(
int j=0; j<map->size_y; j++)
150 if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1)
152 map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
153 cell.src_j_ = cell.j_ = j;
154 marked[MAP_INDEX(map, i, j)] = 1;
158 map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
164 CellData current_cell = Q.top();
165 if(current_cell.i_ > 0)
166 enqueue(map, current_cell.i_-1, current_cell.j_,
167 current_cell.src_i_, current_cell.src_j_,
169 if(current_cell.j_ > 0)
170 enqueue(map, current_cell.i_, current_cell.j_-1,
171 current_cell.src_i_, current_cell.src_j_,
173 if((
int)current_cell.i_ < map->size_x - 1)
174 enqueue(map, current_cell.i_+1, current_cell.j_,
175 current_cell.src_i_, current_cell.src_j_,
177 if((
int)current_cell.j_ < map->size_y - 1)
178 enqueue(map, current_cell.i_, current_cell.j_+1,
179 current_cell.src_i_, current_cell.src_j_,
191 void map_update_cspace(map_t *map,
double max_occ_dist)
197 map_cell_t *cell, *ncell;
199 map->max_occ_dist = max_occ_dist;
200 s = (int) ceil(map->max_occ_dist / map->scale);
203 for (j = 0; j < map->size_y; j++)
205 for (i = 0; i < map->size_x; i++)
207 cell = map->cells + MAP_INDEX(map, i, j);
208 cell->occ_dist = map->max_occ_dist;
213 for (j = 0; j < map->size_y; j++)
215 for (i = 0; i < map->size_x; i++)
217 cell = map->cells + MAP_INDEX(map, i, j);
218 if (cell->occ_state != +1)
224 for (nj = -s; nj <= +s; nj++)
226 for (ni = -s; ni <= +s; ni++)
228 if (!MAP_VALID(map, i + ni, j + nj))
231 ncell = map->cells + MAP_INDEX(map, i + ni, j + nj);
232 d = map->scale * sqrt(ni * ni + nj * nj);
234 if (d < ncell->occ_dist)
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.