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
39class CellData
40{
41public:
42 map_t * map_;
43 unsigned int i_, j_;
44 unsigned int src_i_, src_j_;
45};
46
47class CachedDistanceMap
48{
49public:
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
119bool
120operator<(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
126CachedDistanceMap *
127get_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
140static unsigned int
141delta(const unsigned int x, const unsigned int y)
142{
143 if (x < y) {
144 return y - x;
145 }
146 return x - y;
147}
148
149void
150enqueue(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
184void
185map_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.
261void 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
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:59
bool operator<(const Uuid &uuid, const Uuid &other) noexcept
Compare two Uuids.
Definition: uuid.cpp:120