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
33namespace fawkes {
34
35/** @class ColliFastObstacle <plugins/colli/search/obstacle.h>
36 * This is an implementation of a a fast obstacle.
37 */
39{
40public:
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>
51 {
52 return occupied_cells_;
53 }
54
55 /** Get the key
56 * @return The key
57 */
58 inline int
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
73protected:
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
81private:
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{
91public:
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{
100public:
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 */
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 */
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
This is an implementation of a a fast ellipse.
Definition: obstacle.h:99
ColliFastEllipse(int width, int height, colli_cell_cost_t &costs, bool obstacle_increasement=true)
Constructor for FastEllipse.
Definition: obstacle.h:145
This is an implementation of a a fast obstacle.
Definition: obstacle.h:39
int get_key()
Get the key.
Definition: obstacle.h:59
const std::vector< int > get_obstacle()
Return the occupied cells with their values.
Definition: obstacle.h:50
void set_key(int key)
Set key.
Definition: obstacle.h:68
std::vector< int > occupied_cells_
Aligned array of the occ cells, size is dividable through 3, because: [i] = x coord,...
Definition: obstacle.h:79
This is an implementation of a a fast rectangle.
Definition: obstacle.h:90
ColliFastRectangle(int width, int height, colli_cell_cost_t &costs)
Constructor for FastRectangle.
Definition: obstacle.h:112
Fawkes library namespace.
double sqr(double x)
Fast square multiplication.
Definition: common.h:37
Costs of occupancy-grid cells.
Definition: types.h:50
unsigned int far
The cost for a cell near an obstacle (distance="near")
Definition: types.h:54
unsigned int occ
The cost for an occupied cell.
Definition: types.h:51
unsigned int mid
The cost for a cell near an obstacle (distance="near")
Definition: types.h:53
unsigned int near
The cost for a cell near an obstacle (distance="near")
Definition: types.h:52