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 #include <utils/math/common.h>
28 
29 #include <vector>
30 #include <cmath>
31 
32 namespace fawkes
33 {
34 #if 0 /* just to make Emacs auto-indent happy */
35 }
36 #endif
37 
38 /** @class ColliFastObstacle <plugins/colli/search/obstacle.h>
39  * This is an implementation of a a fast obstacle.
40  */
42 {
43  public:
45  {
46  occupied_cells_.clear();
47  }
48 
49  /** Return the occupied cells with their values
50  * @return vector containing the occupied cells (alternating x and y coordinates)
51  */
52  inline const std::vector< int > get_obstacle()
53  {
54  return occupied_cells_;
55  }
56 
57  /** Get the key
58  * @return The key
59  */
60  inline int get_key() {
61  return key_;
62  }
63 
64  /** Set key.
65  * @param key the new key
66  */
67  inline void set_key( int key ) {
68  key_ = key;
69  }
70 
71  protected:
72  /** Aligned array of the occ cells, size is dividable through 3, because:
73  * [i] = x coord,
74  * [i+1] = y coord,
75  * [i+2] = costs
76  */
77  std::vector< int > occupied_cells_;
78 
79  private:
80  // a unique identifier for each obstacle
81  int key_;
82 };
83 
84 /** @class ColliFastRectangle
85  * This is an implementation of a a fast rectangle.
86  */
88 {
89  public:
90  ColliFastRectangle(int width, int height, colli_cell_cost_t &costs);
91 };
92 
93 /** @class ColliFastEllipse
94  * This is an implementation of a a fast ellipse.
95  */
97 {
98  public:
99  ColliFastEllipse(int width, int height, colli_cell_cost_t &costs, bool obstacle_increasement = true);
100 };
101 
102 
103 /** Constructor for FastRectangle.
104  * @param width radius width of the new rectangle
105  * @param height radius height of the new rectangle
106  * @param costs struct containing the occ-grid cell costs
107  */
108 inline
110 {
111  int y_start = -width/2;
112  int x_start = -height/2;
113 
114  //consider (0,0) to be bottom-left corner of obstacle.
115  for( int x=-3; x<height + 3; ++x ) {
116  for( int y=-3; y<width + 3; ++y ) {
117  occupied_cells_.push_back( x_start + x );
118  occupied_cells_.push_back( y_start + y );
119 
120  if( x < -2 || x >= height+2 || y < -2 || y >= width+2 ) {
121  occupied_cells_.push_back( costs.far );
122 
123  } else if( x < -1 || x >= height+1 || y < - 1 || y >= width+1 ) {
124  occupied_cells_.push_back( costs.mid );
125 
126  } else if( x < 0 || x >= height || y < 0 || y >= width ) {
127  occupied_cells_.push_back( costs.near );
128 
129  } else {
130  occupied_cells_.push_back( costs.occ );
131  }
132  }
133  }
134 }
135 
136 /** Constructor for FastEllipse.
137  * @param width radius width of the new ellipse
138  * @param height radius height of the new ellipse
139  * @param costs struct containing the occ-grid cell costs
140  * @param obstacle_increasement Increase obstacles?
141  */
142 inline
143 ColliFastEllipse::ColliFastEllipse( int width, int height, colli_cell_cost_t &costs, bool obstacle_increasement )
144 {
145  float dist = 1000.f;
146  float dist_near = 1000.f;
147  float dist_middle = 1000.f;
148  float dist_far = 1000.f;
149 
150  int radius_width = round(width/2.f);
151  int radius_height = round(height/2.f);
152 
153  int maxRad = std::max( radius_width, radius_height );
154 
155  for( int y = -(maxRad+8); y <= (maxRad+8); y++ ) {
156  for( int x = -(maxRad+8); x <= (maxRad+8); x++ ) {
157  dist = sqr((float)y/(float)radius_width) + sqr((float)x/(float)radius_height);
158  dist_near = sqr((float)y/(float)(radius_width+2)) + sqr((float)x/(float)(radius_height+2));
159  dist_middle = sqr((float)y/(float)(radius_width+4)) + sqr((float)x/(float)(radius_height+4));
160  dist_far = sqr((float)x/(float)(radius_width+8)) + sqr((float)y/(float)(radius_height+8));
161 
162  if( (dist > 1.f) && (dist_near > 1.f)
163  && (dist_middle > 1.f) && (dist_far > 1.f) ) {
164  // not in grid!
165 
166  } else if( (dist > 1.f) && (dist_near > 1.f)
167  && (dist_middle > 1.f) && (dist_far <= 1.f) ) {
168  occupied_cells_.push_back( x );
169  occupied_cells_.push_back( y );
170  occupied_cells_.push_back( costs.far );
171 
172  } else if( (dist > 1.f) && (dist_near > 1.f)
173  && (dist_middle <= 1.f) ) {
174  occupied_cells_.push_back( x );
175  occupied_cells_.push_back( y );
176  occupied_cells_.push_back( costs.mid );
177 
178  } else if( (dist > 1.f) && (dist_near <= 1.f)
179  && (dist_middle <= 1.f) ) {
180  occupied_cells_.push_back( x );
181  occupied_cells_.push_back( y );
182  occupied_cells_.push_back( costs.near );
183 
184  } else if( (dist <= 1.f) && (dist_near <= 1.f)
185  && (dist_middle <= 1.f) ) {
186  occupied_cells_.push_back( x );
187  occupied_cells_.push_back( y );
188  occupied_cells_.push_back( costs.occ );
189  }
190  }
191  }
192 }
193 
194 
195 } // namespace fawkes
196 
197 #endif
const std::vector< int > get_obstacle()
Return the occupied cells with their values.
Definition: obstacle.h:52
int get_key()
Get the key.
Definition: obstacle.h:60
Fawkes library namespace.
ColliFastRectangle(int width, int height, colli_cell_cost_t &costs)
Constructor for FastRectangle.
Definition: obstacle.h:109
ColliFastEllipse(int width, int height, colli_cell_cost_t &costs, bool obstacle_increasement=true)
Constructor for FastEllipse.
Definition: obstacle.h:143
This is an implementation of a a fast obstacle.
Definition: obstacle.h:41
This is an implementation of a a fast ellipse.
Definition: obstacle.h:96
unsigned int far
The cost for a cell near an obstacle (distance="near")
Definition: types.h:55
double sqr(double x)
Fast square multiplication.
Definition: common.h:37
Costs of occupancy-grid cells.
Definition: types.h:51
std::vector< int > occupied_cells_
Aligned array of the occ cells, size is dividable through 3, because: [i] = x coord, [i+1] = y coord, [i+2] = costs.
Definition: obstacle.h:77
void set_key(int key)
Set key.
Definition: obstacle.h:67
unsigned int occ
The cost for an occupied cell.
Definition: types.h:52
This is an implementation of a a fast rectangle.
Definition: obstacle.h:87
unsigned int mid
The cost for a cell near an obstacle (distance="near")
Definition: types.h:54
unsigned int near
The cost for a cell near an obstacle (distance="near")
Definition: types.h:53