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 "map.h"
31 
32 #include <math.h>
33 #include <queue>
34 #include <stdlib.h>
35 #include <string.h>
36 
37 /// @cond EXTERNAL
38 
39 class CellData
40 {
41 public:
42  map_t * map_;
43  unsigned int i_, j_;
44  unsigned int src_i_, src_j_;
45 };
46 
47 class CachedDistanceMap
48 {
49 public:
50  CachedDistanceMap(double scale, double max_dist)
51  : distances_(NULL), scale_(scale), max_dist_(max_dist)
52  {
53  cell_radius_ = max_dist / scale;
54  distances_ = new double *[cell_radius_ + 2];
55  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++) {
58  distances_[i][j] = sqrt(i * i + j * j);
59  }
60  }
61  }
62 
63  CachedDistanceMap(const CachedDistanceMap &other)
64  : distances_(NULL), scale_(other.scale_), max_dist_(other.max_dist_)
65  {
66  cell_radius_ = other.cell_radius_;
67  distances_ = new double *[cell_radius_ + 2];
68  for (int i = 0; i <= cell_radius_ + 1; i++) {
69  distances_[i] = new double[cell_radius_ + 2];
70  for (int j = 0; j <= cell_radius_ + 1; j++) {
71  distances_[i][j] = other.distances_[i][j];
72  }
73  }
74  }
75 
76  ~CachedDistanceMap()
77  {
78  if (distances_) {
79  for (int i = 0; i <= cell_radius_ + 1; i++)
80  delete[] distances_[i];
81  delete[] distances_;
82  }
83  }
84 
85  CachedDistanceMap &
86  operator=(const CachedDistanceMap &other)
87  {
88  if (&other == this)
89  return *this;
90 
91  if (distances_) {
92  for (int i = 0; i <= cell_radius_ + 1; i++)
93  delete[] distances_[i];
94  delete[] distances_;
95  }
96 
97  distances_ = NULL;
98  scale_ = other.scale_;
99  max_dist_ = other.max_dist_;
100 
101  cell_radius_ = other.cell_radius_;
102  distances_ = new double *[cell_radius_ + 2];
103  for (int i = 0; i <= cell_radius_ + 1; i++) {
104  distances_[i] = new double[cell_radius_ + 2];
105  for (int j = 0; j <= cell_radius_ + 1; j++) {
106  distances_[i][j] = other.distances_[i][j];
107  }
108  }
109 
110  return *this;
111  }
112 
113  double **distances_;
114  double scale_;
115  double max_dist_;
116  int cell_radius_;
117 };
118 
119 bool
120 operator<(const CellData &a, const CellData &b)
121 {
122  return a.map_->cells[MAP_INDEX(a.map_, a.i_, a.j_)].occ_dist
123  > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist;
124 }
125 
126 CachedDistanceMap *
127 get_distance_map(double scale, double max_dist)
128 {
129  static CachedDistanceMap *cdm = NULL;
130 
131  if (!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist)) {
132  if (cdm)
133  delete cdm;
134  cdm = new CachedDistanceMap(scale, max_dist);
135  }
136 
137  return cdm;
138 }
139 
140 static unsigned int
141 delta(const unsigned int x, const unsigned int y)
142 {
143  if (x < y) {
144  return y - x;
145  }
146  return x - y;
147 }
148 
149 void
150 enqueue(map_t * map,
151  unsigned int i,
152  unsigned int j,
153  unsigned int src_i,
154  unsigned int src_j,
155  std::priority_queue<CellData> &Q,
156  CachedDistanceMap * cdm,
157  unsigned char * marked)
158 {
159  if (marked[MAP_INDEX(map, i, j)])
160  return;
161 
162  unsigned int di = delta(i, src_i);
163  unsigned int dj = delta(j, src_j);
164  double distance = cdm->distances_[di][dj];
165 
166  if (distance > cdm->cell_radius_)
167  return;
168 
169  map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;
170 
171  CellData cell;
172  cell.map_ = map;
173  cell.i_ = i;
174  cell.j_ = j;
175  cell.src_i_ = src_i;
176  cell.src_j_ = src_j;
177 
178  Q.push(cell);
179 
180  marked[MAP_INDEX(map, i, j)] = 1;
181 }
182 
183 // Update the cspace distance values
184 void
185 map_update_cspace(map_t *map, double max_occ_dist)
186 {
187  unsigned char * marked;
188  std::priority_queue<CellData> Q;
189 
190  marked = new unsigned char[map->size_x * map->size_y];
191  memset(marked, 0, sizeof(unsigned char) * map->size_x * map->size_y);
192 
193  map->max_occ_dist = max_occ_dist;
194 
195  CachedDistanceMap *cdm = get_distance_map(map->scale, map->max_occ_dist);
196 
197  // Enqueue all the obstacle cells
198  CellData cell;
199  cell.map_ = map;
200  for (int i = 0; i < map->size_x; i++) {
201  cell.src_i_ = cell.i_ = i;
202  for (int j = 0; j < map->size_y; j++) {
203  if (map->cells[MAP_INDEX(map, i, j)].occ_state == +1) {
204  map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
205  cell.src_j_ = cell.j_ = j;
206  marked[MAP_INDEX(map, i, j)] = 1;
207  Q.push(cell);
208  } else
209  map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
210  }
211  }
212 
213  while (!Q.empty()) {
214  CellData current_cell = Q.top();
215  if (current_cell.i_ > 0)
216  enqueue(map,
217  current_cell.i_ - 1,
218  current_cell.j_,
219  current_cell.src_i_,
220  current_cell.src_j_,
221  Q,
222  cdm,
223  marked);
224  if (current_cell.j_ > 0)
225  enqueue(map,
226  current_cell.i_,
227  current_cell.j_ - 1,
228  current_cell.src_i_,
229  current_cell.src_j_,
230  Q,
231  cdm,
232  marked);
233  if ((int)current_cell.i_ < map->size_x - 1)
234  enqueue(map,
235  current_cell.i_ + 1,
236  current_cell.j_,
237  current_cell.src_i_,
238  current_cell.src_j_,
239  Q,
240  cdm,
241  marked);
242  if ((int)current_cell.j_ < map->size_y - 1)
243  enqueue(map,
244  current_cell.i_,
245  current_cell.j_ + 1,
246  current_cell.src_i_,
247  current_cell.src_j_,
248  Q,
249  cdm,
250  marked);
251 
252  Q.pop();
253  }
254 
255  delete[] marked;
256 }
257 
258 #if 0
259 // TODO: replace this with a more efficient implementation. Not crucial,
260 // because we only do it once, at startup.
261 void map_update_cspace(map_t *map, double max_occ_dist)
262 {
263  int i, j;
264  int ni, nj;
265  int s;
266  double d;
267  map_cell_t *cell, *ncell;
268 
269  map->max_occ_dist = max_occ_dist;
270  s = (int) ceil(map->max_occ_dist / map->scale);
271 
272  // Reset the distance values
273  for (j = 0; j < map->size_y; j++)
274  {
275  for (i = 0; i < map->size_x; i++)
276  {
277  cell = map->cells + MAP_INDEX(map, i, j);
278  cell->occ_dist = map->max_occ_dist;
279  }
280  }
281 
282  // Find all the occupied cells and update their neighbours
283  for (j = 0; j < map->size_y; j++)
284  {
285  for (i = 0; i < map->size_x; i++)
286  {
287  cell = map->cells + MAP_INDEX(map, i, j);
288  if (cell->occ_state != +1)
289  continue;
290 
291  cell->occ_dist = 0;
292 
293  // Update adjacent cells
294  for (nj = -s; nj <= +s; nj++)
295  {
296  for (ni = -s; ni <= +s; ni++)
297  {
298  if (!MAP_VALID(map, i + ni, j + nj))
299  continue;
300 
301  ncell = map->cells + MAP_INDEX(map, i + ni, j + nj);
302  d = map->scale * sqrt(ni * ni + nj * nj);
303 
304  if (d < ncell->occ_dist)
305  ncell->occ_dist = d;
306  }
307  }
308  }
309  }
310 
311  return;
312 }
313 #endif
314 
315 /// @endcond
fawkes::distance
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:63