23#ifndef _PLUGINS_COLLI_SEARCH_OBSTACLE_H_
24#define _PLUGINS_COLLI_SEARCH_OBSTACLE_H_
26#include "../common/types.h"
28#include <utils/math/common.h>
49 inline const std::vector<int>
104 bool obstacle_increasement =
true);
114 int y_start = -width / 2;
115 int x_start = -height / 2;
118 for (
int x = -3; x < height + 3; ++x) {
119 for (
int y = -3; y < width + 3; ++y) {
123 if (x < -2 || x >= height + 2 || y < -2 || y >= width + 2) {
126 }
else if (x < -1 || x >= height + 1 || y < -1 || y >= width + 1) {
129 }
else if (x < 0 || x >= height || y < 0 || y >= width) {
148 bool obstacle_increasement)
151 float dist_near = 1000.f;
152 float dist_middle = 1000.f;
153 float dist_far = 1000.f;
155 int radius_width = round(width / 2.f);
156 int radius_height = round(height / 2.f);
158 int maxRad = std::max(radius_width, radius_height);
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);
164 sqr((
float)y / (
float)(radius_width + 2)) +
sqr((
float)x / (
float)(radius_height + 2));
166 sqr((
float)y / (
float)(radius_width + 4)) +
sqr((
float)x / (
float)(radius_height + 4));
168 sqr((
float)x / (
float)(radius_width + 8)) +
sqr((
float)y / (
float)(radius_height + 8));
170 if ((dist > 1.f) && (dist_near > 1.f) && (dist_middle > 1.f) && (dist_far > 1.f)) {
173 }
else if ((dist > 1.f) && (dist_near > 1.f) && (dist_middle > 1.f) && (dist_far <= 1.f)) {
178 }
else if ((dist > 1.f) && (dist_near > 1.f) && (dist_middle <= 1.f)) {
183 }
else if ((dist > 1.f) && (dist_near <= 1.f) && (dist_middle <= 1.f)) {
188 }
else if ((dist <= 1.f) && (dist_near <= 1.f) && (dist_middle <= 1.f)) {
This is an implementation of a a fast ellipse.
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.
int get_key()
Get the key.
const std::vector< int > get_obstacle()
Return the occupied cells with their values.
void set_key(int key)
Set key.
std::vector< int > occupied_cells_
Aligned array of the occ cells, size is dividable through 3, because: [i] = x coord,...
This is an implementation of a a fast rectangle.
ColliFastRectangle(int width, int height, colli_cell_cost_t &costs)
Constructor for FastRectangle.
Fawkes library namespace.
double sqr(double x)
Fast square multiplication.
Costs of occupancy-grid cells.
unsigned int far
The cost for a cell near an obstacle (distance="near")
unsigned int occ
The cost for an occupied cell.
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")