-
Notifications
You must be signed in to change notification settings - Fork 0
/
PathFind.cpp
347 lines (344 loc) · 14.7 KB
/
PathFind.cpp
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
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
//Josh Stone JStone3
#include "PathFind.h"
//Function used to check whether the goal has been reached.
bool CheckFinish(SCoords* currentNode, int x, int y)
{
if (x == currentNode->x && y == currentNode->y)
{
return true;
}
else
{
return false;
}
}
//Function used to search a list for a specific value.
//Returns true if node is on list. If node isn't on list then returns false.
bool FindValue(deque <unique_ptr < SCoords > > &list, SCoords n)
{
for (auto p = list.begin(); p != list.end(); p++)
{
if ((*p)->x == n.x && (*p)->y == n.y)
{
return true;
}
}
return false;
}
//Function used to search closed list and check whether a node has already been visited/generated.
//This function is used in the event that we find a cheaper route of reaching a specific node.
//If this is true then we update the node using the new values and add it to the open list.
void FindValueRemove(deque <unique_ptr < SCoords > > &closedList, SCoords n, deque <unique_ptr < SCoords > > &openList)
{
unique_ptr < SCoords > temp(new SCoords);
auto p = closedList.begin(); // set p to the beginning of the loop
while (p != closedList.end()) // while not at the end of the loop
{
if ((*p)->x == n.x && (*p)->y == n.y)
{
temp->x = (*p)->x;
temp->y = (*p)->y;
temp->cost = (*p)->cost;
temp->pParent = (*p)->pParent;
temp->score = (*p)->score;
openList.push_front(move(temp));
p = closedList.erase(p);
}
else
{
p++; //Advance through the loop
}
}
}
//Function used to add a new node to the open list.
void InsertNode(deque <unique_ptr < SCoords > > &openList, SCoords node)
{
unique_ptr<SCoords> temp(new SCoords);
temp->x = node.x;
temp->y = node.y;
temp->cost = node.cost;
temp->pParent = node.pParent;
temp->score = node.score;
openList.push_front(move(temp));
}
//Function used to calculate the hueristic of a node. The heuristic is the absolute distance from the end to a node measured square by square.
int HeuristicCalc(SCoords mapEnd, SCoords n)
{
int x, y;
//The heuristics are calcualted for both the x and y axis.
x = mapEnd.x - n.x;
y = mapEnd.y - n.y;
//Both values are then checked if they are below 0. If the values are negative then they are multiplied by -1 to make them positive.
//Negative values wouldn't give us the absolute distance.
if (x < 0)
{
x *= -1;
}
if (y < 0)
{
y *= -1;
}
return x + y;
}
//Function used in the sorting algorithm to sort the list by score from lowest to highest.
//Returns true or false based on which score is higher.
bool CompareCoords(unique_ptr<SCoords>& lhs, unique_ptr<SCoords>& rhs)
{
return lhs->score < rhs->score;
}
//Function used to generate the new nodes using the current node from the open list.
void GenerateNodes(SCoords &north, SCoords &east, SCoords &south, SCoords &west, SCoords* currentNode)
{
north = { currentNode->x, currentNode->y + 1 };//To generate north we add 1 to the y value.
east = { currentNode->x + 1, currentNode->y };//To generate east we add 1 to the x value.
south = { currentNode->x, currentNode->y - 1 };//To generate south we subtract 1 from the y value.
west = { currentNode->x - 1, currentNode->y };//To generate west we subtract 1 from the x value.
}
//Function used to check if a node is already on the open or the closed list and then return the cost of that node.
//This will be used to determine whether to update the values of a node if it has already been visited.
int GetCost(deque <unique_ptr < SCoords > > &openList, deque <unique_ptr < SCoords > > &closedList, SCoords node)
{
for (auto p = openList.begin(); p != openList.end(); p++)
{
if ((*p)->x == node.x && (*p)->y == node.y)
{
return (*p)->cost;
}
}
for (auto p = closedList.begin(); p != closedList.end(); p++)
{
if ((*p)->x == node.x && (*p)->y == node.y)
{
return (*p)->cost;
}
}
}
//Function used to calculate the path from start to finish.
void CalculatePath(SCoords* currentNode, int mapArray[10][10], int newCost, int existingCost, SCoords &node, SCoords mapEnd,
deque <unique_ptr < SCoords > > &openList, deque <unique_ptr < SCoords > > &closedList, bool &validNode)
{
int wallCost = 0;
int arrayOffset = 9;//An offset is needed for the array because a 2 dimensional array starts from the top left but the map coordinate system starts in the bottom right.
existingCost = GetCost(openList, closedList, node);//This is used to store the existing cost of the newly generated node if it is already on open or closed list.
newCost = currentNode->cost + mapArray[arrayOffset - node.y][node.x];//newCost is the cost of the new node generated by adding the terrain cost of the new node to the cost of its parent,
//in this case the current node.
if (mapArray[arrayOffset - node.y][node.x] != wallCost)//This check is used to ensure that any nodes that happen to be walls are not added to the lists because we cannot move through walls.
{
//We then check to see if the newly generated node is already on the open or closed lists
//and if its new cost is greater than the cost of the node on the list then the new node is discarded.
if ((FindValue(openList, node) || FindValue(closedList, node)) && newCost >= existingCost)
{
return;
}
else
{
validNode = true;
node.pParent = currentNode;//We set the parent of the new node to the current node to allow us to create a route by following the parent chain.
node.score = newCost + HeuristicCalc(mapEnd, node);
node.cost = newCost;
//We then check to see if the new node is on open or closed list.
if (FindValue(openList, node) || FindValue(closedList, node))
{
//If the value is on the closed list then we need to remove it from the closed list and add the new value to open list.
if (FindValue(closedList, node))
{
FindValueRemove(closedList, node, openList);
}
}
//We then check if the value isnt on either list. If it isn't then we add it to open list as it is a node that hasn't previously been generated.
if (!FindValue(openList, node) || !FindValue(closedList, node))
{
InsertNode(openList, node);
}
}
}
}
//Function used to move through the chain of parents and create the route from start to end for the map.
void GenerateRoute(deque <unique_ptr < SCoords > > &openList, vector<int> &routeX, vector<int> &routeY)
{
SCoords *path;//Pointer variable that will be used to advance through the parents of each node in the final path.
path = openList.back().get();//Path is set to the back of open list as this is where the end node is placed.
//Check used to stop moving along the parent chain as the start nodes parent is set to 0.
while (path != 0)
{
//We push the x and y values of point along the route onto 2 vectors. 1 stores the x values and 1 stores the y values.
routeX.push_back((*path).x);
routeY.push_back((*path).y);
path = path->pParent;//This is used to advance along the parent chain.
}
//After we have put the route into the vectors we need to reverse the vectors because we start reading the parent chain from the end point but we want to output the route from the start point.
reverse(routeX.begin(), routeX.end());
reverse(routeY.begin(), routeY.end());
}
//Function used to read in files and store information in appropriate variables.
void ReadFiles(ifstream& infile, string SCoordsName, SCoords& start, SCoords& end, string mapName, int mapArray[10][10])
{
//Read in file containing start and end poin of map.
infile.open(SCoordsName);
//Error check incase input file cannot be found.
if (!infile)
{
cout << "ERROR: ";
cout << "Can't open input file\n";
}
while (!infile.eof())
{
infile >> start.x >> start.y;
infile >> end.x >> end.y;
}
infile.close();
//Read in file containing terrain costs of map.
infile.open(mapName);
if (!infile)
{
cout << "ERROR: ";
cout << "Can't open input file\n";
}
while (!infile.eof())
{
for (int i = 0; i < 10; i++)
{
for (int j = 0; j < 10; j++)
{
infile >> mapArray[i][j];
///open the file and put in the and then the new coordinates that we generate will relate to a positon in the array
} //and this will allow us to get the cost of that square.
}
}
infile.close();
}
//Function used to set up open list and push start point onto the list.
void InitialiseList(SCoords mapStart, SCoords mapEnd, deque <unique_ptr < SCoords > > &openList, deque <unique_ptr < SCoords > > &closedList, int &newCost, int &existingCost)
{
openList.clear();
closedList.clear();
int heuristic = HeuristicCalc(mapEnd, mapStart);
unique_ptr<SCoords> temp(new SCoords);
temp->x = mapStart.x;
temp->y = mapStart.y;
temp->score = heuristic;
temp->pParent = 0;//Parent set to 0 so that we know when to stop advancing through the parent chain when creating the route.
temp->cost = 0;
openList.push_back(move(temp));
//These variables are then reset.
newCost = 0;
existingCost = 0;
}
//Function used to reset all variables involved in displaying real time output of the A Star search.
void resetValues(int &pathCounter, float &timer, bool &goalReached, bool &routeGenerated, bool &displayRoute, bool &validNode, bool &startSearch, vector<int> &routeX, vector<int> &routeY)
{
pathCounter = 0;//Variables used to move through vector that contains the final route for each map.
timer = 0;//Variable used to automatically display final route.
goalReached = false;//Variable used to indicate if the end node has been reached.
routeGenerated = false;//Variable used to indicate if the route has been generated and stored in appropriate vectors.
displayRoute = false;//Variable used to automatically display final route.
validNode = false;//Variable used to indicate if the node that has been generated is valid and not a wall. This is used to control what skin is applied to a square if any.
startSearch = false;//Variable used to start the A Star search.
//Vectors that are used to store the x and y coordinates of the nodes in the path from start to end are cleared.
routeX.clear();
routeY.clear();
}
//Function used to tie together whole program and allow us to use a single function to invoke an A Star search.
void PathFind(bool &goalReached, deque <unique_ptr < SCoords > > &openList, deque <unique_ptr < SCoords > > &closedList, SCoords mapEnd, SCoords &north, SCoords &east, SCoords &south, SCoords &west
, int mapArray[10][10], IModel* mapSquares[10][10], IMesh* squareMesh, int newCost, int existingCost, string visitedNodeSkin, string openListNodeSkin, bool &validNode)
{
unique_ptr<SCoords> currentNode(new SCoords);
if (!goalReached || openList.empty())
{
currentNode.reset(new SCoords);//Reset the current node every loop because we move current node onto closed list at the end of every loop.
currentNode = move(openList.front());//We then set current node to the first value on open list and pop the value from open list.
openList.pop_front();
mapSquares[currentNode->y][currentNode->x]->SetSkin(visitedNodeSkin);
if (CheckFinish(currentNode.get(), mapEnd.x, mapEnd.y))//Check if the end goal has been reached. If it has then we exit the loop after dispalying an end message.
{
openList.push_back(move(currentNode));//We then push the final node back onto open list so we can move through it's parent chain.
goalReached = true;
return;
}
//If the current node isn't the end goal then we generate new nodes.
else
{
GenerateNodes(north, east, south, west, currentNode.get());
//Then we check if the nodes have already been visited by checking openlist, if they haven't then we insert them.
CalculatePath(currentNode.get(), mapArray, newCost, existingCost, north, mapEnd, openList, closedList, validNode);
//If the node is a valid node,i.e. the node isn't a wall then a skin is applied to the square to indicate that it is a viable node.
if (validNode)
{
mapSquares[north.y][north.x]->SetSkin(openListNodeSkin);
}
validNode = false;
CalculatePath(currentNode.get(), mapArray, newCost, existingCost, east, mapEnd, openList, closedList, validNode);
if (validNode)
{
mapSquares[east.y][east.x]->SetSkin(openListNodeSkin);
}
validNode = false;
CalculatePath(currentNode.get(), mapArray, newCost, existingCost, south, mapEnd, openList, closedList, validNode);
if (validNode)
{
mapSquares[south.y][south.x]->SetSkin(openListNodeSkin);
}
validNode = false;
CalculatePath(currentNode.get(), mapArray, newCost, existingCost, west, mapEnd, openList, closedList, validNode);
if (validNode)
{
mapSquares[west.y][west.x]->SetSkin(openListNodeSkin);
}
validNode = false;
closedList.push_back(move(currentNode));
}
//The list is then sorted based on score from lowest to highest.
sort(openList.begin(), openList.end(), CompareCoords);
}
}
//Function used to allocate skins to each square based on the terrain cost of the square which is gathered from the appropriate map array.
void setGrid(IMesh* squareMesh, IModel* mapSquares[10][10], string water, string wood, string open, string wall,
string start, string end, int mapArray[10][10], SCoords mapStart, SCoords mapEnd)
{
int arrayOffset = 9;//An offset is used when accessing the array as the original map file coordinate system counts from left to right in the x axis and from bottom to top in the y axis.
//However when accessing the array in the program the array counts from top to bottom in it's y axis. This means we need to use the offset to ensure that the arrays
//act the same when using the y coordinate of a square to access its tile cost.
int wallCost = 0;
int openCost = 1;
int woodCost = 2;
int waterCost = 3;
for (int i = 0; i < 10; i++)//rows
{
for (int j = 0; j < 10; j++)//collums
{
mapSquares[i][j]->SetSkin(open);
}
}
//Each square has a different skin based on its cost which is retrieved from the appropriate map array.
for (int i = 0; i < 10; i++)//rows
{
for (int j = 0; j < 10; j++)//collums
{
if (mapArray[arrayOffset - i][j] == wallCost)
{
mapSquares[i][j]->SetSkin(wall);
}
if (mapArray[arrayOffset - i][j] == openCost)
{
mapSquares[i][j]->SetSkin(open);
}
if (mapArray[arrayOffset - i][j] == woodCost)
{
mapSquares[i][j]->SetSkin(wood);
}
if (mapArray[arrayOffset - i][j] == waterCost)
{
mapSquares[i][j]->SetSkin(water);
}
}
}
mapSquares[mapStart.y][mapStart.x]->SetSkin(start);
mapSquares[mapEnd.y][mapEnd.x]->SetSkin(end);
}
//Function used to reset the camera to its starting position and rotate it correctly.
void cameraReset(ICamera* &camera)
{
camera->SetPosition(50, 100, -65);
camera->ResetOrientation();
camera->RotateX(40);
}