LineOfSight.h
Go to the documentation of this file.
1 
6 #ifndef LINEOFSIGHT_H
7 #define LINEOFSIGHT_H
8 
9 
10 #define CN_OBSTACLE 1
11 
12 #include <vector>
13 #include <math.h>
14 #include <algorithm>
15 #include <iostream>
16 
17 #include "Const.h"
18 
19 
30 class LineOfSight
31 {
32 public:
37  LineOfSight(double agentSize = 0.5)
38  {
39  this->agentSize = agentSize;
40  int add_x, add_y, num = agentSize + 0.5 - CN_EPS;
41  for(int x = -num; x <= +num; x++)
42  for(int y = -num; y <= +num; y++)
43  {
44  add_x = x != 0 ? 1 : 0;
45  add_y = y != 0 ? 1 : 0;
46  if((pow(2*abs(x) - add_x, 2) + pow(2*abs(y) - add_y, 2)) < pow(2*agentSize, 2))
47  cells.push_back({x, y});
48  }
49  if(cells.empty())
50  cells.push_back({0,0});
51  }
52 
57  void setSize(double agentSize)
58  {
59  this->agentSize = agentSize;
60  int add_x, add_y, num = agentSize + 0.5 - CN_EPS;
61  cells.clear();
62  for(int x = -num; x <= +num; x++)
63  for(int y = -num; y <= +num; y++)
64  {
65  add_x = x != 0 ? 1 : 0;
66  add_y = y != 0 ? 1 : 0;
67  if((pow(2*abs(x) - add_x, 2) + pow(2*abs(y) - add_y, 2)) < pow(2*agentSize, 2))
68  cells.push_back({x, y});
69  }
70  if(cells.empty())
71  cells.push_back({0,0});
72  }
73 
83  template <class T>
84  std::vector<std::pair<int, int>> getCellsCrossedByLine(int x1, int y1, int x2, int y2, const T &map)
85  {
86  std::vector<std::pair<int, int>> lineCells(0);
87  if(x1 == x2 && y1 == y2)
88  {
89  for(auto cell:cells)
90  lineCells.push_back({x1+cell.first, y1+cell.second});
91  return lineCells;
92  }
93  int delta_x = std::abs(x1 - x2);
94  int delta_y = std::abs(y1 - y2);
95  if((delta_x >= delta_y && x1 > x2) || (delta_y > delta_x && y1 > y2))
96  {
97  std::swap(x1, x2);
98  std::swap(y1, y2);
99  }
100  int step_x = (x1 < x2 ? 1 : -1);
101  int step_y = (y1 < y2 ? 1 : -1);
102  int error = 0, x = x1, y = y1;
103  int k, num;
104  std::pair<int, int> add;
105  int gap = agentSize*sqrt(pow(delta_x, 2) + pow(delta_y, 2)) + double(delta_x + delta_y)/2 - CN_EPS;
106 
107  if(delta_x >= delta_y)
108  {
109  int extraCheck = agentSize*delta_y/sqrt(pow(delta_x, 2) + pow(delta_y, 2)) + 0.5 - CN_EPS;
110  for(int n = 1; n <= extraCheck; n++)
111  {
112  error += delta_y;
113  num = (gap - error)/delta_x;
114  for(k = 1; k <= num; k++)
115  lineCells.push_back({x1 - n*step_x, y1 + k*step_y});
116  for(k = 1; k <= num; k++)
117  lineCells.push_back({x2 + n*step_x, y2 - k*step_y});
118  }
119  error = 0;
120  for(x = x1; x != x2 + step_x; x+=step_x)
121  {
122  lineCells.push_back({x, y});
123  if(x < x2 - extraCheck)
124  {
125  num = (gap + error)/delta_x;
126  for(k = 1; k <= num; k++)
127  lineCells.push_back({x, y + k*step_y});
128  }
129  if(x > x1 + extraCheck)
130  {
131  num = (gap - error)/delta_x;
132  for(k = 1; k <= num; k++)
133  lineCells.push_back({x, y - k*step_y});
134  }
135  error += delta_y;
136  if((error<<1) > delta_x)
137  {
138  y += step_y;
139  error -= delta_x;
140  }
141  }
142  }
143  else
144  {
145  int extraCheck = agentSize*delta_x/sqrt(pow(delta_x, 2) + pow(delta_y, 2)) + 0.5 - CN_EPS;
146  for(int n = 1; n <= extraCheck; n++)
147  {
148  error += delta_x;
149  num = (gap - error)/delta_y;
150  for(k = 1; k <= num; k++)
151  lineCells.push_back({x1 + k*step_x, y1 - n*step_y});
152  for(k = 1; k <= num; k++)
153  lineCells.push_back({x2 - k*step_x, y2 + n*step_y});
154  }
155  error = 0;
156  for(y = y1; y != y2 + step_y; y += step_y)
157  {
158  lineCells.push_back({x, y});
159  if(y < y2 - extraCheck)
160  {
161  num = (gap + error)/delta_y;
162  for(k = 1; k <= num; k++)
163  lineCells.push_back({x + k*step_x, y});
164  }
165  if(y > y1 + extraCheck)
166  {
167  num = (gap - error)/delta_y;
168  for(k = 1; k <= num; k++)
169  lineCells.push_back({x - k*step_x, y});
170  }
171  error += delta_x;
172  if((error<<1) > delta_y)
173  {
174  x += step_x;
175  error -= delta_y;
176  }
177  }
178  }
179  for(k = 0; k < cells.size(); k++)
180  {
181  add = {x1 + cells[k].first, y1 + cells[k].second};
182  if(std::find(lineCells.begin(), lineCells.end(), add) == lineCells.end())
183  lineCells.push_back(add);
184  add = {x2 + cells[k].first, y2 + cells[k].second};
185  if(std::find(lineCells.begin(), lineCells.end(), add) == lineCells.end())
186  lineCells.push_back(add);
187  }
188 
189  for(auto it = lineCells.begin(); it != lineCells.end(); it++)
190  if(!map.CellOnGrid(it->first, it->second))
191  {
192  lineCells.erase(it);
193  it = lineCells.begin();
194  }
195  return lineCells;
196  }
197 
205  template <class T>
206  bool checkTraversability(int x, int y, const T &map)
207  {
208  for(int k = 0; k < cells.size(); k++)
209  if(!map.CellOnGrid(x + cells[k].first, y + cells[k].second) || map.CellIsObstacle(x + cells[k].first, y + cells[k].second))
210  return false;
211  return true;
212  }
213 
214 
224  template <class T>
225  bool checkLine(int x1, int y1, int x2, int y2, const T &map)
226  {
227  //std::cout<<x1<<" "<<y1<<" "<<x2<<" "<<y2<<" "<<"\n";
228  if(!checkTraversability(x1, y1, map) || !checkTraversability(x2, y2, map)) //additional cheсk of start and goal traversability,
229  return false; //it can be removed if they are already checked
230 
231  int delta_x = std::abs(x1 - x2);
232  int delta_y = std::abs(y1 - y2);
233  if((delta_x > delta_y && x1 > x2) || (delta_y >= delta_x && y1 > y2))
234  {
235  std::swap(x1, x2);
236  std::swap(y1, y2);
237  }
238  int step_x = (x1 < x2 ? 1 : -1);
239  int step_y = (y1 < y2 ? 1 : -1);
240  int error = 0, x = x1, y = y1;
241  int gap = agentSize*sqrt(pow(delta_x, 2) + pow(delta_y, 2)) + double(delta_x + delta_y)/2 - CN_EPS;
242  int k, num;
243 
244  if(delta_x > delta_y)
245  {
246  int extraCheck = agentSize*delta_y/sqrt(pow(delta_x, 2) + pow(delta_y, 2)) + 0.5 - CN_EPS;
247  for(int n = 1; n <= extraCheck; n++)
248  {
249  error += delta_y;
250  num = (gap - error)/delta_x;
251  for(k = 1; k <= num; k++)
252  if(map.CellOnGrid(x1 - n*step_x, y1 + k*step_y))
253  if(map.CellIsObstacle(x1 - n*step_x, y1 + k*step_y))
254  return false;
255  for(k = 1; k <= num; k++)
256  if(map.CellOnGrid(x2 + n*step_x, y2 - k*step_y))
257  if(map.CellIsObstacle(x2 + n*step_x, y2 - k*step_y))
258  return false;
259  }
260  error = 0;
261  for(x = x1; x != x2 + step_x; x+=step_x)
262  {
263  if(map.CellIsObstacle(x, y))
264  return false;
265  if(x < x2 - extraCheck)
266  {
267  num = (gap + error)/delta_x;
268  for(k = 1; k <= num; k++)
269  if(map.CellIsObstacle(x, y + k*step_y))
270  return false;
271  }
272  if(x > x1 + extraCheck)
273  {
274  num = (gap - error)/delta_x;
275  for(k = 1; k <= num; k++)
276  if(map.CellIsObstacle(x, y - k*step_y))
277  return false;
278  }
279  error += delta_y;
280  if((error<<1) > delta_x)
281  {
282  y += step_y;
283  error -= delta_x;
284  }
285  }
286  }
287  else
288  {
289  int extraCheck = agentSize*delta_x/sqrt(pow(delta_x, 2) + pow(delta_y, 2)) + 0.5 - CN_EPS;
290  for(int n = 1; n <= extraCheck; n++)
291  {
292  error += delta_x;
293  num = (gap - error)/delta_y;
294  for(k = 1; k <= num; k++)
295  if(map.CellOnGrid(x1 + k*step_x, y1 - n*step_y))
296  if(map.CellIsObstacle(x1 + k*step_x, y1 - n*step_y))
297  return false;
298  for(k = 1; k <= num; k++)
299  if(map.CellOnGrid(x2 - k*step_x, y2 + n*step_y))
300  if(map.CellIsObstacle(x2 - k*step_x, y2 + n*step_y))
301  return false;
302  }
303  error = 0;
304  for(y = y1; y != y2 + step_y; y += step_y)
305  {
306  if(map.CellIsObstacle(x, y))
307  return false;
308  if(y < y2 - extraCheck)
309  {
310  num = (gap + error)/delta_y;
311  for(k = 1; k <= num; k++)
312  if(map.CellIsObstacle(x + k*step_x, y))
313  return false;
314  }
315  if(y > y1 + extraCheck)
316  {
317  num = (gap - error)/delta_y;
318  for(k = 1; k <= num; k++)
319  if(map.CellIsObstacle(x - k*step_x, y))
320  return false;
321  }
322  error += delta_x;
323  if((error<<1) > delta_y)
324  {
325  x += step_x;
326  error -= delta_y;
327  }
328  }
329  }
330  return true;
331  }
332 
333 
340  std::vector<std::pair<int, int>> getCells(int i, int j)
341  {
342  std::vector<std::pair<int, int>> cells;
343  for(int k=0; k<this->cells.size(); k++)
344  cells.push_back({i+this->cells[k].first,j+this->cells[k].second});
345  return cells;
346  }
347 private:
349  double agentSize;
350  std::vector<std::pair<int, int>> cells; //cells that are affected by agent's body
352 };
353 
354 #endif // LINEOFSIGHT_H
LineOfSight::checkTraversability
bool checkTraversability(int x, int y, const T &map)
Checks traversability of all cells on grid affected by agent&#39;s body.
Definition: LineOfSight.h:206
LineOfSight::LineOfSight
LineOfSight(double agentSize=0.5)
LineOfSight constructor.
Definition: LineOfSight.h:37
LineOfSight::setSize
void setSize(double agentSize)
Set size of agent.
Definition: LineOfSight.h:57
LineOfSight::checkLine
bool checkLine(int x1, int y1, int x2, int y2, const T &map)
Checks line-of-sight between two cells.
Definition: LineOfSight.h:225
Const.h
File contains main constants.
LineOfSight::getCells
std::vector< std::pair< int, int > > getCells(int i, int j)
Returns all cells on grid that are affected by agent in position (i,j).
Definition: LineOfSight.h:340
LineOfSight::getCellsCrossedByLine
std::vector< std::pair< int, int > > getCellsCrossedByLine(int x1, int y1, int x2, int y2, const T &map)
Returns all cells on grid that are affected by agent during moving along a line.
Definition: LineOfSight.h:84
LineOfSight
This class implements line-of-sight function between two cells on the grid for a variable size of age...
Definition: LineOfSight.h:30
CN_EPS
#define CN_EPS
Epsilon for float number operations definition.
Definition: Const.h:14

ORCAStar
Author(s): Stepan Drgachev
autogenerated on Wed Jul 15 2020 16:13:14