Fawkes API  Fawkes Development Version
obstacle.h
1 
2 /***************************************************************************
3  * obstacle.h - A fast obstacle
4  *
5  * Created: Wed Apr 30 16:03:23 2014
6  * Copyright 2002 Stefan Jacobs
7  * 2013-2014 Bahram Maleki-Fard
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #ifndef _PLUGINS_COLLI_SEARCH_OBSTACLE_H_
24 #define _PLUGINS_COLLI_SEARCH_OBSTACLE_H_
25 
26 #include "../common/types.h"
27 
28 #include <utils/math/common.h>
29 
30 #include <cmath>
31 #include <vector>
32 
33 namespace fawkes {
34 
35 /** @class ColliFastObstacle <plugins/colli/search/obstacle.h>
36  * This is an implementation of a a fast obstacle.
37  */
38 class ColliFastObstacle
39 {
40 public:
41  ~ColliFastObstacle()
42  {
43  occupied_cells_.clear();
44  }
45 
46  /** Return the occupied cells with their values
47  * @return vector containing the occupied cells (alternating x and y coordinates)
48  */
49  inline const std::vector<int>
50  get_obstacle()
51  {
52  return occupied_cells_;
53  }
54 
55  /** Get the key
56  * @return The key
57  */
58  inline int
59  get_key()
60  {
61  return key_;
62  }
63 
64  /** Set key.
65  * @param key the new key
66  */
67  inline void
68  set_key(int key)
69  {
70  key_ = key;
71  }
72 
73 protected:
74  /** Aligned array of the occ cells, size is dividable through 3, because:
75  * [i] = x coord,
76  * [i+1] = y coord,
77  * [i+2] = costs
78  */
79  std::vector<int> occupied_cells_;
80 
81 private:
82  // a unique identifier for each obstacle
83  int key_;
84 };
85 
86 /** @class ColliFastRectangle
87  * This is an implementation of a a fast rectangle.
88  */
90 {
91 public:
92  ColliFastRectangle(int width, int height, colli_cell_cost_t &costs);
93 };
94 
95 /** @class ColliFastEllipse
96  * This is an implementation of a a fast ellipse.
97  */
99 {
100 public:
101  ColliFastEllipse(int width,
102  int height,
103  colli_cell_cost_t &costs,
104  bool obstacle_increasement = true);
105 };
106 
107 /** Constructor for FastRectangle.
108  * @param width radius width of the new rectangle
109  * @param height radius height of the new rectangle
110  * @param costs struct containing the occ-grid cell costs
111  */
112 inline ColliFastRectangle::ColliFastRectangle(int width, int height, colli_cell_cost_t &costs)
113 {
114  int y_start = -width / 2;
115  int x_start = -height / 2;
116 
117  //consider (0,0) to be bottom-left corner of obstacle.
118  for (int x = -3; x < height + 3; ++x) {
119  for (int y = -3; y < width + 3; ++y) {
120  occupied_cells_.push_back(x_start + x);
121  occupied_cells_.push_back(y_start + y);
122 
123  if (x < -2 || x >= height + 2 || y < -2 || y >= width + 2) {
124  occupied_cells_.push_back(costs.far);
125 
126  } else if (x < -1 || x >= height + 1 || y < -1 || y >= width + 1) {
127  occupied_cells_.push_back(costs.mid);
128 
129  } else if (x < 0 || x >= height || y < 0 || y >= width) {
130  occupied_cells_.push_back(costs.near);
131 
132  } else {
133  occupied_cells_.push_back(costs.occ);
134  }
135  }
136  }
137 }
138 
139 /** Constructor for FastEllipse.
140  * @param width radius width of the new ellipse
141  * @param height radius height of the new ellipse
142  * @param costs struct containing the occ-grid cell costs
143  * @param obstacle_increasement Increase obstacles?
144  */
145 inline ColliFastEllipse::ColliFastEllipse(int width,
146  int height,
147  colli_cell_cost_t &costs,
148  bool obstacle_increasement)
149 {
150  float dist = 1000.f;
151  float dist_near = 1000.f;
152  float dist_middle = 1000.f;
153  float dist_far = 1000.f;
154 
155  int radius_width = round(width / 2.f);
156  int radius_height = round(height / 2.f);
157 
158  int maxRad = std::max(radius_width, radius_height);
159 
160  for (int y = -(maxRad + 8); y <= (maxRad + 8); y++) {
161  for (int x = -(maxRad + 8); x <= (maxRad + 8); x++) {
162  dist = sqr((float)y / (float)radius_width) + sqr((float)x / (float)radius_height);
163  dist_near =
164  sqr((float)y / (float)(radius_width + 2)) + sqr((float)x / (float)(radius_height + 2));
165  dist_middle =
166  sqr((float)y / (float)(radius_width + 4)) + sqr((float)x / (float)(radius_height + 4));
167  dist_far =
168  sqr((float)x / (float)(radius_width + 8)) + sqr((float)y / (float)(radius_height + 8));
169 
170  if ((dist > 1.f) && (dist_near > 1.f) && (dist_middle > 1.f) && (dist_far > 1.f)) {
171  // not in grid!
172 
173  } else if ((dist > 1.f) && (dist_near > 1.f) && (dist_middle > 1.f) && (dist_far <= 1.f)) {
174  occupied_cells_.push_back(x);
175  occupied_cells_.push_back(y);
176  occupied_cells_.push_back(costs.far);
177 
178  } else if ((dist > 1.f) && (dist_near > 1.f) && (dist_middle <= 1.f)) {
179  occupied_cells_.push_back(x);
180  occupied_cells_.push_back(y);
181  occupied_cells_.push_back(costs.mid);
182 
183  } else if ((dist > 1.f) && (dist_near <= 1.f) && (dist_middle <= 1.f)) {
184  occupied_cells_.push_back(x);
185  occupied_cells_.push_back(y);
186  occupied_cells_.push_back(costs.near);
187 
188  } else if ((dist <= 1.f) && (dist_near <= 1.f) && (dist_middle <= 1.f)) {
189  occupied_cells_.push_back(x);
190  occupied_cells_.push_back(y);
191  occupied_cells_.push_back(costs.occ);
192  }
193  }
194  }
195 }
196 
197 } // namespace fawkes
198 
199 #endif
fawkes::ColliFastObstacle
Definition: obstacle.h:44
fawkes::colli_cell_cost_t::near
unsigned int near
The cost for a cell near an obstacle (distance="near")
Definition: types.h:59
fawkes::colli_cell_cost_t::mid
unsigned int mid
The cost for a cell near an obstacle (distance="near")
Definition: types.h:60
fawkes::ColliFastObstacle::occupied_cells_
std::vector< int > occupied_cells_
Aligned array of the occ cells, size is dividable through 3, because: [i] = x coord,...
Definition: obstacle.h:85
fawkes::ColliFastObstacle::set_key
void set_key(int key)
Set key.
Definition: obstacle.h:74
fawkes::colli_cell_cost_t::occ
unsigned int occ
The cost for an occupied cell.
Definition: types.h:58
fawkes::ColliFastEllipse::ColliFastEllipse
ColliFastEllipse(int width, int height, colli_cell_cost_t &costs, bool obstacle_increasement=true)
Constructor for FastEllipse.
Definition: obstacle.h:151
fawkes::ColliFastRectangle
Definition: obstacle.h:95
fawkes
fawkes::ColliFastObstacle::get_key
int get_key()
Get the key.
Definition: obstacle.h:65
fawkes::ColliFastRectangle::ColliFastRectangle
ColliFastRectangle(int width, int height, colli_cell_cost_t &costs)
Constructor for FastRectangle.
Definition: obstacle.h:118
fawkes::ColliFastObstacle::get_obstacle
const std::vector< int > get_obstacle()
Return the occupied cells with their values.
Definition: obstacle.h:56
fawkes::ColliFastEllipse
Definition: obstacle.h:104
fawkes::colli_cell_cost_t
Costs of occupancy-grid cells.
Definition: types.h:56
fawkes::sqr
double sqr(double x)
Fast square multiplication.
Definition: common.h:43
fawkes::colli_cell_cost_t::far
unsigned int far
The cost for a cell near an obstacle (distance="near")
Definition: types.h:61