-
Notifications
You must be signed in to change notification settings - Fork 1
/
LineOfSight.h
86 lines (84 loc) · 2.84 KB
/
LineOfSight.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
#ifndef LINEOFSIGHT_H
#define LINEOFSIGHT_H
#include <cMap.h>
class LineOfSight
{
public:
bool checkLine(int i1, int j1, int i2, int j2, const cMap& map)
{
int delta_i = std::abs(i1 - i2);
int delta_j = std::abs(j1 - j2);
int step_i = (i1 < i2 ? 1 : -1);
int step_j = (j1 < j2 ? 1 : -1);
int error = 0;
int i = i1;
int j = j1;
int sep_value = delta_i*delta_i + delta_j*delta_j;
if(delta_i == 0)
{
for(; j != j2; j += step_j)
if(map.CellIsObstacle(i, j))
return false;
}
else if(delta_j == 0)
{
for(; i != i2; i += step_i)
if(map.CellIsObstacle(i, j))
return false;
}
else if(delta_i > delta_j)
{
for(; i != i2; i += step_i)
{
if(map.CellIsObstacle(i, j))
return false;
if(map.CellIsObstacle(i, j + step_j))
return false;
error += delta_j;
if(error > delta_i)
{
if(((error << 1) - delta_i - delta_j)*((error << 1) - delta_i - delta_j) < sep_value)
if(map.CellIsObstacle(i + step_i, j))
return false;
if((3*delta_i - ((error << 1) - delta_j))*(3*delta_i - ((error << 1) - delta_j)) < sep_value)
if(map.CellIsObstacle(i, j + 2*step_j))
return false;
j += step_j;
error -= delta_i;
}
}
if(map.CellIsObstacle(i, j))
return false;
if(map.CellIsObstacle(i, j + step_j))
return false;
}
else
{
for(; j != j2; j += step_j)
{
if(map.CellIsObstacle(i, j))
return false;
if(map.CellIsObstacle(i + step_i, j))
return false;
error += delta_i;
if(error > delta_j)
{
if(((error << 1) - delta_i - delta_j)*((error << 1) - delta_i - delta_j) < sep_value)
if(map.CellIsObstacle(i, j + step_j))
return false;
if((3*delta_j - ((error << 1) - delta_i))*(3*delta_j - ((error << 1) - delta_i)) < sep_value)
if(map.CellIsObstacle(i + 2*step_i, j))
return false;
i += step_i;
error -= delta_j;
}
}
if(map.CellIsObstacle(i, j))
return false;
if(map.CellIsObstacle(i + step_i, j))
return false;
}
return true;
}
};
#endif // LINEOFSIGHT_H