Fawkes API  Fawkes Development Version
map_cspace.cpp
1 
2 /***************************************************************************
3  * cspace.cpp: Cached distance map
4  *
5  * Created: Thu May 24 18:46:12 2012
6  * Copyright 2000 Brian Gerkey
7  * 2000 Kasper Stoy
8  * 2012 Tim Niemueller [www.niemueller.de]
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 /* From:
25  * Player - One Hell of a Robot Server (LGPL)
26  * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
27  * gerkey@usc.edu kaspers@robotics.usc.edu
28  */
29 
30 #include <queue>
31 #include <math.h>
32 #include <stdlib.h>
33 #include <string.h>
34 #include "map.h"
35 
36 /// @cond EXTERNAL
37 
38 class CellData
39 {
40  public:
41  map_t* map_;
42  unsigned int i_, j_;
43  unsigned int src_i_, src_j_;
44 };
45 
46 class CachedDistanceMap
47 {
48  public:
49  CachedDistanceMap(double scale, double max_dist) :
50  distances_(NULL), scale_(scale), max_dist_(max_dist)
51  {
52  cell_radius_ = max_dist / scale;
53  distances_ = new double *[cell_radius_+2];
54  for(int i=0; i<=cell_radius_+1; i++)
55  {
56  distances_[i] = new double[cell_radius_+2];
57  for(int j=0; j<=cell_radius_+1; j++)
58  {
59  distances_[i][j] = sqrt(i*i + j*j);
60  }
61  }
62  }
63  ~CachedDistanceMap()
64  {
65  if(distances_)
66  {
67  for(int i=0; i<=cell_radius_+1; i++)
68  delete[] distances_[i];
69  delete[] distances_;
70  }
71  }
72  double** distances_;
73  double scale_;
74  double max_dist_;
75  int cell_radius_;
76 };
77 
78 
79 bool operator<(const CellData& a, const CellData& b)
80 {
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;
82 }
83 
84 CachedDistanceMap*
85 get_distance_map(double scale, double max_dist)
86 {
87  static CachedDistanceMap* cdm = NULL;
88 
89  if(!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist))
90  {
91  if(cdm)
92  delete cdm;
93  cdm = new CachedDistanceMap(scale, max_dist);
94  }
95 
96  return cdm;
97 }
98 
99 static unsigned int delta(const unsigned int x, const unsigned int y)
100 {
101  if(x < y)
102  {
103  return y - x;
104  }
105  return x - y;
106 }
107 
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)
113 {
114  if(marked[MAP_INDEX(map, i, j)])
115  return;
116 
117  unsigned int di = delta(i, src_i);
118  unsigned int dj = delta(j, src_j);
119  double distance = cdm->distances_[di][dj];
120 
121  if(distance > cdm->cell_radius_)
122  return;
123 
124  map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;
125 
126  CellData cell;
127  cell.map_ = map;
128  cell.i_ = i;
129  cell.j_ = j;
130  cell.src_i_ = src_i;
131  cell.src_j_ = src_j;
132 
133  Q.push(cell);
134 
135  marked[MAP_INDEX(map, i, j)] = 1;
136 }
137 
138 // Update the cspace distance values
139 void map_update_cspace(map_t *map, double max_occ_dist)
140 {
141  unsigned char* marked;
142  std::priority_queue<CellData> Q;
143 
144  marked = new unsigned char[map->size_x*map->size_y];
145  memset(marked, 0, sizeof(unsigned char) * map->size_x*map->size_y);
146 
147  map->max_occ_dist = max_occ_dist;
148 
149  CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist);
150 
151  // Enqueue all the obstacle cells
152  CellData cell;
153  cell.map_ = map;
154  for(int i=0; i<map->size_x; i++)
155  {
156  cell.src_i_ = cell.i_ = i;
157  for(int j=0; j<map->size_y; j++)
158  {
159  if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1)
160  {
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;
164  Q.push(cell);
165  }
166  else
167  map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
168  }
169  }
170 
171  while(!Q.empty())
172  {
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_,
177  Q, cdm, marked);
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_,
181  Q, cdm, marked);
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_,
185  Q, cdm, marked);
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_,
189  Q, cdm, marked);
190 
191  Q.pop();
192  }
193 
194  delete[] marked;
195 }
196 
197 #if 0
198 // TODO: replace this with a more efficient implementation. Not crucial,
199 // because we only do it once, at startup.
200 void map_update_cspace(map_t *map, double max_occ_dist)
201 {
202  int i, j;
203  int ni, nj;
204  int s;
205  double d;
206  map_cell_t *cell, *ncell;
207 
208  map->max_occ_dist = max_occ_dist;
209  s = (int) ceil(map->max_occ_dist / map->scale);
210 
211  // Reset the distance values
212  for (j = 0; j < map->size_y; j++)
213  {
214  for (i = 0; i < map->size_x; i++)
215  {
216  cell = map->cells + MAP_INDEX(map, i, j);
217  cell->occ_dist = map->max_occ_dist;
218  }
219  }
220 
221  // Find all the occupied cells and update their neighbours
222  for (j = 0; j < map->size_y; j++)
223  {
224  for (i = 0; i < map->size_x; i++)
225  {
226  cell = map->cells + MAP_INDEX(map, i, j);
227  if (cell->occ_state != +1)
228  continue;
229 
230  cell->occ_dist = 0;
231 
232  // Update adjacent cells
233  for (nj = -s; nj <= +s; nj++)
234  {
235  for (ni = -s; ni <= +s; ni++)
236  {
237  if (!MAP_VALID(map, i + ni, j + nj))
238  continue;
239 
240  ncell = map->cells + MAP_INDEX(map, i + ni, j + nj);
241  d = map->scale * sqrt(ni * ni + nj * nj);
242 
243  if (d < ncell->occ_dist)
244  ncell->occ_dist = d;
245  }
246  }
247  }
248  }
249 
250  return;
251 }
252 #endif
253 
254 /// @endcond
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:62