23 #ifndef __PLUGINS_COLLI_SEARCH_OBSTACLE_H_ 24 #define __PLUGINS_COLLI_SEARCH_OBSTACLE_H_ 26 #include "../common/types.h" 27 #include <utils/math/common.h> 46 occupied_cells_.clear();
54 return occupied_cells_;
111 int y_start = -width/2;
112 int x_start = -height/2;
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 );
120 if( x < -2 || x >= height+2 || y < -2 || y >= width+2 ) {
121 occupied_cells_.push_back( costs.
far );
123 }
else if( x < -1 || x >= height+1 || y < - 1 || y >= width+1 ) {
124 occupied_cells_.push_back( costs.
mid );
126 }
else if( x < 0 || x >= height || y < 0 || y >= width ) {
127 occupied_cells_.push_back( costs.
near );
130 occupied_cells_.push_back( costs.
occ );
143 ColliFastEllipse::ColliFastEllipse(
int width,
int height,
colli_cell_cost_t &costs,
bool obstacle_increasement )
146 float dist_near = 1000.f;
147 float dist_middle = 1000.f;
148 float dist_far = 1000.f;
150 int radius_width = round(width/2.f);
151 int radius_height = round(height/2.f);
153 int maxRad = std::max( radius_width, radius_height );
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));
162 if( (dist > 1.f) && (dist_near > 1.f)
163 && (dist_middle > 1.f) && (dist_far > 1.f) ) {
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 );
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 );
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 );
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 );
const std::vector< int > get_obstacle()
Return the occupied cells with their values.
int get_key()
Get the key.
Fawkes library namespace.
This is an implementation of a a fast obstacle.
This is an implementation of a a fast ellipse.
unsigned int far
The cost for a cell near an obstacle (distance="near")
double sqr(double x)
Fast square multiplication.
Costs of occupancy-grid cells.
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.
void set_key(int key)
Set key.
unsigned int occ
The cost for an occupied cell.
This is an implementation of a a fast rectangle.
unsigned int mid
The cost for a cell near an obstacle (distance="near")
unsigned int near
The cost for a cell near an obstacle (distance="near")