Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * cspace.cpp: Cached distance map 00004 * 00005 * Created: Thu May 24 18:46:12 2012 00006 * Copyright 2000 Brian Gerkey 00007 * 2000 Kasper Stoy 00008 * 2012 Tim Niemueller [www.niemueller.de] 00009 ****************************************************************************/ 00010 00011 /* This program is free software; you can redistribute it and/or modify 00012 * it under the terms of the GNU General Public License as published by 00013 * the Free Software Foundation; either version 2 of the License, or 00014 * (at your option) any later version. 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL file in the doc directory. 00022 */ 00023 00024 /* From: 00025 * Player - One Hell of a Robot Server (LGPL) 00026 * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 00027 * gerkey@usc.edu kaspers@robotics.usc.edu 00028 */ 00029 00030 #include <queue> 00031 #include <math.h> 00032 #include <stdlib.h> 00033 #include <string.h> 00034 #include "map.h" 00035 00036 /// @cond EXTERNAL 00037 00038 class CellData 00039 { 00040 public: 00041 map_t* map_; 00042 unsigned int i_, j_; 00043 unsigned int src_i_, src_j_; 00044 }; 00045 00046 class CachedDistanceMap 00047 { 00048 public: 00049 CachedDistanceMap(double scale, double max_dist) : 00050 distances_(NULL), scale_(scale), max_dist_(max_dist) 00051 { 00052 cell_radius_ = max_dist / scale; 00053 distances_ = new double *[cell_radius_+2]; 00054 for(int i=0; i<=cell_radius_+1; i++) 00055 { 00056 distances_[i] = new double[cell_radius_+2]; 00057 for(int j=0; j<=cell_radius_+1; j++) 00058 { 00059 distances_[i][j] = sqrt(i*i + j*j); 00060 } 00061 } 00062 } 00063 ~CachedDistanceMap() 00064 { 00065 if(distances_) 00066 { 00067 for(int i=0; i<=cell_radius_+1; i++) 00068 delete[] distances_[i]; 00069 delete[] distances_; 00070 } 00071 } 00072 double** distances_; 00073 double scale_; 00074 double max_dist_; 00075 int cell_radius_; 00076 }; 00077 00078 00079 bool operator<(const CellData& a, const CellData& b) 00080 { 00081 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; 00082 } 00083 00084 CachedDistanceMap* 00085 get_distance_map(double scale, double max_dist) 00086 { 00087 static CachedDistanceMap* cdm = NULL; 00088 00089 if(!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist)) 00090 { 00091 if(cdm) 00092 delete cdm; 00093 cdm = new CachedDistanceMap(scale, max_dist); 00094 } 00095 00096 return cdm; 00097 } 00098 00099 void enqueue(map_t* map, unsigned int i, unsigned int j, 00100 unsigned int src_i, unsigned int src_j, 00101 std::priority_queue<CellData>& Q, 00102 CachedDistanceMap* cdm, 00103 unsigned char* marked) 00104 { 00105 if(marked[MAP_INDEX(map, i, j)]) 00106 return; 00107 00108 unsigned int di = abs(i - src_i); 00109 unsigned int dj = abs(j - src_j); 00110 double distance = cdm->distances_[di][dj]; 00111 00112 if(distance > cdm->cell_radius_) 00113 return; 00114 00115 map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale; 00116 00117 CellData cell; 00118 cell.map_ = map; 00119 cell.i_ = i; 00120 cell.j_ = j; 00121 cell.src_i_ = src_i; 00122 cell.src_j_ = src_j; 00123 00124 Q.push(cell); 00125 00126 marked[MAP_INDEX(map, i, j)] = 1; 00127 } 00128 00129 // Update the cspace distance values 00130 void map_update_cspace(map_t *map, double max_occ_dist) 00131 { 00132 unsigned char* marked; 00133 std::priority_queue<CellData> Q; 00134 00135 marked = new unsigned char[map->size_x*map->size_y]; 00136 memset(marked, 0, sizeof(unsigned char) * map->size_x*map->size_y); 00137 00138 map->max_occ_dist = max_occ_dist; 00139 00140 CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist); 00141 00142 // Enqueue all the obstacle cells 00143 CellData cell; 00144 cell.map_ = map; 00145 for(int i=0; i<map->size_x; i++) 00146 { 00147 cell.src_i_ = cell.i_ = i; 00148 for(int j=0; j<map->size_y; j++) 00149 { 00150 if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1) 00151 { 00152 map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0; 00153 cell.src_j_ = cell.j_ = j; 00154 marked[MAP_INDEX(map, i, j)] = 1; 00155 Q.push(cell); 00156 } 00157 else 00158 map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist; 00159 } 00160 } 00161 00162 while(!Q.empty()) 00163 { 00164 CellData current_cell = Q.top(); 00165 if(current_cell.i_ > 0) 00166 enqueue(map, current_cell.i_-1, current_cell.j_, 00167 current_cell.src_i_, current_cell.src_j_, 00168 Q, cdm, marked); 00169 if(current_cell.j_ > 0) 00170 enqueue(map, current_cell.i_, current_cell.j_-1, 00171 current_cell.src_i_, current_cell.src_j_, 00172 Q, cdm, marked); 00173 if((int)current_cell.i_ < map->size_x - 1) 00174 enqueue(map, current_cell.i_+1, current_cell.j_, 00175 current_cell.src_i_, current_cell.src_j_, 00176 Q, cdm, marked); 00177 if((int)current_cell.j_ < map->size_y - 1) 00178 enqueue(map, current_cell.i_, current_cell.j_+1, 00179 current_cell.src_i_, current_cell.src_j_, 00180 Q, cdm, marked); 00181 00182 Q.pop(); 00183 } 00184 00185 delete[] marked; 00186 } 00187 00188 #if 0 00189 // TODO: replace this with a more efficient implementation. Not crucial, 00190 // because we only do it once, at startup. 00191 void map_update_cspace(map_t *map, double max_occ_dist) 00192 { 00193 int i, j; 00194 int ni, nj; 00195 int s; 00196 double d; 00197 map_cell_t *cell, *ncell; 00198 00199 map->max_occ_dist = max_occ_dist; 00200 s = (int) ceil(map->max_occ_dist / map->scale); 00201 00202 // Reset the distance values 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 cell->occ_dist = map->max_occ_dist; 00209 } 00210 } 00211 00212 // Find all the occupied cells and update their neighbours 00213 for (j = 0; j < map->size_y; j++) 00214 { 00215 for (i = 0; i < map->size_x; i++) 00216 { 00217 cell = map->cells + MAP_INDEX(map, i, j); 00218 if (cell->occ_state != +1) 00219 continue; 00220 00221 cell->occ_dist = 0; 00222 00223 // Update adjacent cells 00224 for (nj = -s; nj <= +s; nj++) 00225 { 00226 for (ni = -s; ni <= +s; ni++) 00227 { 00228 if (!MAP_VALID(map, i + ni, j + nj)) 00229 continue; 00230 00231 ncell = map->cells + MAP_INDEX(map, i + ni, j + nj); 00232 d = map->scale * sqrt(ni * ni + nj * nj); 00233 00234 if (d < ncell->occ_dist) 00235 ncell->occ_dist = d; 00236 } 00237 } 00238 } 00239 } 00240 00241 return; 00242 } 00243 #endif 00244 00245 /// @endcond