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> 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 ) {
120 if( x < -2 || x >= height+2 || y < -2 || y >= width+2 ) {
123 }
else if( x < -1 || x >= height+1 || y < - 1 || y >= width+1 ) {
126 }
else if( x < 0 || x >= height || y < 0 || y >= width ) {
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) ) {
172 }
else if( (dist > 1.f) && (dist_near > 1.f)
173 && (dist_middle <= 1.f) ) {
178 }
else if( (dist > 1.f) && (dist_near <= 1.f)
179 && (dist_middle <= 1.f) ) {
184 }
else if( (dist <= 1.f) && (dist_near <= 1.f)
185 && (dist_middle <= 1.f) ) {
const std::vector< int > get_obstacle()
Return the occupied cells with their values.
int get_key()
Get the key.
Fawkes library namespace.
ColliFastRectangle(int width, int height, colli_cell_cost_t &costs)
Constructor for FastRectangle.
ColliFastEllipse(int width, int height, colli_cell_cost_t &costs, bool obstacle_increasement=true)
Constructor for FastEllipse.
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")