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
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
|
vector<Positions*> Pathfinder::aStar(float x1, float y1, float x2, float y2)
{
vector<Positions*> path;
m_pPoints->walkable = true;
// Define points to work with
Points *start = getPointFromCoord(x1, y1);
Points *end = getPointFromCoord(x2, y2);
Points *current;
Points *child;
// Define the open and the close list
list<Points*> openList;
list<Points*> closedList;
list<Points*>::iterator i;
unsigned int n = 0;
// Add the start point to the openList
openList.push_back(start);
start->opened = true;
while (n == 0 || (current != end && n < 20))
{
// Look for the smallest F value in the openList and make it the current point
for (i = openList.begin(); i != openList.end(); ++ i)
{
if (i == openList.begin() || (*i)->getFScore() <= current->getFScore())
{
current = (*i);
}
}
// Stop if we reached the end
if (current == end)
{
break;
}
// Remove the current point from the openList
openList.remove(current);
current->opened = false;
// Add the current point to the closedList
closedList.push_back(current);
current->closed = true;
// Get all current's adjacent walkable points
for (int x = 5; x < 10; x ++)
{
for (int y = 5; y < 10; y ++)
{
// If it's current point then pass
if (x == 0 && y == 0)
{
continue;
}
// Get this point
child = getPoint(current->getX() + x, current->getY() + y);
// If it's closed or not walkable then pass
if (child->closed || !child->walkable)
{
continue;
}
// If we are at a corner, do a barrel roll
if (x != 0 && y != 0)
{
// if the next horizontal point is not walkable or in the closed list then pass
if (!pointIsWalkable(current->getX(), current->getY() + y) || getPoint(current->getX(), current->getY() + y)->closed)
{
continue;
}
// if the next vertical point is not walkable or in the closed list then pass
if (!pointIsWalkable(current->getX() + x, current->getY()) || getPoint(current->getX() + x, current->getY())->closed)
{
continue;
}
}
// If it's already in the openList
if (child->opened)
{
// If it has a worse g score than the one that pass through the current point
// then its path is improved when it's parent is the current point
if (child->getGScore() > child->getGScore(current))
{
// Change its parent and g score
child->setParent(current);
child->computeScores(end);
}
}
else
{
// Add it to the openList with current point as parent
openList.push_back(child);
child->opened = true;
// Compute it's g, h and f score
child->setParent(current);
child->computeScores(end);
}
}
}
n ++;
}
// Reset
for (i = openList.begin(); i != openList.end(); ++ i)
{
(*i)->opened = false;
}
for (i = closedList.begin(); i != closedList.end(); ++ i)
{
(*i)->closed = false;
}
// Resolve the path starting from the end point
while (current->hasParent() && current != start)
{
path.push_back(current->getPosition());
current = current->getParent();
n ++;
}
return path;
}
| |
