00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include <queue>
00023 #include <math.h>
00024 #include <stdlib.h>
00025 #include <string.h>
00026 #include "map.h"
00027
00028 class CellData
00029 {
00030 public:
00031 map_t* map_;
00032 unsigned int i_, j_;
00033 unsigned int src_i_, src_j_;
00034 };
00035
00036 class CachedDistanceMap
00037 {
00038 public:
00039 CachedDistanceMap(double scale, double max_dist) :
00040 distances_(NULL), scale_(scale), max_dist_(max_dist)
00041 {
00042 cell_radius_ = max_dist / scale;
00043 distances_ = new double *[cell_radius_+2];
00044 for(int i=0; i<=cell_radius_+1; i++)
00045 {
00046 distances_[i] = new double[cell_radius_+2];
00047 for(int j=0; j<=cell_radius_+1; j++)
00048 {
00049 distances_[i][j] = sqrt(i*i + j*j);
00050 }
00051 }
00052 }
00053 ~CachedDistanceMap()
00054 {
00055 if(distances_)
00056 {
00057 for(int i=0; i<=cell_radius_+1; i++)
00058 delete[] distances_[i];
00059 delete[] distances_;
00060 }
00061 }
00062 double** distances_;
00063 double scale_;
00064 double max_dist_;
00065 int cell_radius_;
00066 };
00067
00068
00069 bool operator<(const CellData& a, const CellData& b)
00070 {
00071 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;
00072 }
00073
00074 CachedDistanceMap*
00075 get_distance_map(double scale, double max_dist)
00076 {
00077 static CachedDistanceMap* cdm = NULL;
00078
00079 if(!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist))
00080 {
00081 if(cdm)
00082 delete cdm;
00083 cdm = new CachedDistanceMap(scale, max_dist);
00084 }
00085
00086 return cdm;
00087 }
00088
00089 void enqueue(map_t* map, unsigned int i, unsigned int j,
00090 unsigned int src_i, unsigned int src_j,
00091 std::priority_queue<CellData>& Q,
00092 CachedDistanceMap* cdm,
00093 unsigned char* marked)
00094 {
00095 if(marked[MAP_INDEX(map, i, j)])
00096 return;
00097
00098 unsigned int di = abs(i - src_i);
00099 unsigned int dj = abs(j - src_j);
00100 double distance = cdm->distances_[di][dj];
00101
00102 if(distance > cdm->cell_radius_)
00103 return;
00104
00105 map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;
00106
00107 CellData cell;
00108 cell.map_ = map;
00109 cell.i_ = i;
00110 cell.j_ = j;
00111 cell.src_i_ = src_i;
00112 cell.src_j_ = src_j;
00113
00114 Q.push(cell);
00115
00116 marked[MAP_INDEX(map, i, j)] = 1;
00117 }
00118
00119
00120 void map_update_cspace(map_t *map, double max_occ_dist)
00121 {
00122 unsigned char* marked;
00123 std::priority_queue<CellData> Q;
00124
00125 marked = new unsigned char[map->size_x*map->size_y];
00126 memset(marked, 0, sizeof(unsigned char) * map->size_x*map->size_y);
00127
00128 map->max_occ_dist = max_occ_dist;
00129
00130 CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist);
00131
00132
00133 CellData cell;
00134 cell.map_ = map;
00135 for(int i=0; i<map->size_x; i++)
00136 {
00137 cell.src_i_ = cell.i_ = i;
00138 for(int j=0; j<map->size_y; j++)
00139 {
00140 if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1)
00141 {
00142 map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
00143 cell.src_j_ = cell.j_ = j;
00144 marked[MAP_INDEX(map, i, j)] = 1;
00145 Q.push(cell);
00146 }
00147 else
00148 map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
00149 }
00150 }
00151
00152 while(!Q.empty())
00153 {
00154 CellData current_cell = Q.top();
00155 if(current_cell.i_ > 0)
00156 enqueue(map, current_cell.i_-1, current_cell.j_,
00157 current_cell.src_i_, current_cell.src_j_,
00158 Q, cdm, marked);
00159 if(current_cell.j_ > 0)
00160 enqueue(map, current_cell.i_, current_cell.j_-1,
00161 current_cell.src_i_, current_cell.src_j_,
00162 Q, cdm, marked);
00163 if((int)current_cell.i_ < map->size_x - 1)
00164 enqueue(map, current_cell.i_+1, current_cell.j_,
00165 current_cell.src_i_, current_cell.src_j_,
00166 Q, cdm, marked);
00167 if((int)current_cell.j_ < map->size_y - 1)
00168 enqueue(map, current_cell.i_, current_cell.j_+1,
00169 current_cell.src_i_, current_cell.src_j_,
00170 Q, cdm, marked);
00171
00172 Q.pop();
00173 }
00174
00175 delete[] marked;
00176 }
00177
00178 #if 0
00179
00180
00181 void map_update_cspace(map_t *map, double max_occ_dist)
00182 {
00183 int i, j;
00184 int ni, nj;
00185 int s;
00186 double d;
00187 map_cell_t *cell, *ncell;
00188
00189 map->max_occ_dist = max_occ_dist;
00190 s = (int) ceil(map->max_occ_dist / map->scale);
00191
00192
00193 for (j = 0; j < map->size_y; j++)
00194 {
00195 for (i = 0; i < map->size_x; i++)
00196 {
00197 cell = map->cells + MAP_INDEX(map, i, j);
00198 cell->occ_dist = map->max_occ_dist;
00199 }
00200 }
00201
00202
00203 for (j = 0; j < map->size_y; j++)
00204 {
00205 for (i = 0; i < map->size_x; i++)
00206 {
00207 cell = map->cells + MAP_INDEX(map, i, j);
00208 if (cell->occ_state != +1)
00209 continue;
00210
00211 cell->occ_dist = 0;
00212
00213
00214 for (nj = -s; nj <= +s; nj++)
00215 {
00216 for (ni = -s; ni <= +s; ni++)
00217 {
00218 if (!MAP_VALID(map, i + ni, j + nj))
00219 continue;
00220
00221 ncell = map->cells + MAP_INDEX(map, i + ni, j + nj);
00222 d = map->scale * sqrt(ni * ni + nj * nj);
00223
00224 if (d < ncell->occ_dist)
00225 ncell->occ_dist = d;
00226 }
00227 }
00228 }
00229 }
00230
00231 return;
00232 }
00233 #endif