$search
00001 /* 00002 * Player - One Hell of a Robot Server 00003 * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 00004 * gerkey@usc.edu kaspers@robotics.usc.edu 00005 * 00006 * This library is free software; you can redistribute it and/or 00007 * modify it under the terms of the GNU Lesser General Public 00008 * License as published by the Free Software Foundation; either 00009 * version 2.1 of the License, or (at your option) any later version. 00010 * 00011 * This library is distributed in the hope that it will be useful, 00012 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00014 * Lesser General Public License for more details. 00015 * 00016 * You should have received a copy of the GNU Lesser General Public 00017 * License along with this library; if not, write to the Free Software 00018 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 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 // Update the cspace distance values 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 // Enqueue all the obstacle cells 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 // TODO: replace this with a more efficient implementation. Not crucial, 00180 // because we only do it once, at startup. 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 // Reset the distance values 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 // Find all the occupied cells and update their neighbours 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 // Update adjacent cells 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