-
Notifications
You must be signed in to change notification settings - Fork 0
/
PathFinder.cpp
107 lines (89 loc) · 3.12 KB
/
PathFinder.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
#include "PathFinder.h"
#include <cmath>
bool operator>(const Node& left, const Node& right)
{
return left.weight > right.weight;
}
PathFinder::PathFinder(std::pair<int, int> start, std::pair<int, int> target, const std::vector<int>& map, std::pair<int, int> mapDimensions)
: _start(start)
, _target(target)
, _map(map)
, _mapDimensions(mapDimensions)
{
const int nodesAmount = _mapDimensions.first * _mapDimensions.second;
_distances.assign(nodesAmount, -1);
_parentIndexes.assign(nodesAmount, -1);
}
bool PathFinder::FindPath(std::vector<int>& outPath)
{
const int startIndex = FindIndex(_start.first, _start.second);
const int targetIndex = FindIndex(_target.first, _target.second);
// push start node
_nodesQueue.push(Node(startIndex, _start.first, _start.second, -1, 0, 0));
// traverse through the nodes to calculate the distance
while (!_nodesQueue.empty()) {
Node node = _nodesQueue.top();
_nodesQueue.pop();
if (node.index == targetIndex) {
_distances[node.index] = node.distanceFromStart;
_parentIndexes[node.index] = node.parentIndex;
break;
}
CalculateData(node);
}
const int targetDistance = _distances[targetIndex];
if (targetDistance != -1) {
int i = 1;
int currentIndex = targetIndex;
outPath.resize(targetDistance);
while (currentIndex != startIndex && i <= targetDistance) {
outPath[targetDistance - i] = currentIndex;
currentIndex = _parentIndexes[currentIndex];
++i;
}
return true;
}
return false;
}
void PathFinder::CalculateData(const Node& node)
{
// return if we've already calculated data for this node
if (_distances[node.index] != -1) {
return;
}
_distances[node.index] = node.distanceFromStart;
_parentIndexes[node.index] = node.parentIndex;
// check the left node
if (0 <= node.x - 1) {
AddNodeToQueue(node, node.x - 1, node.y);
}
// check the right node
if (node.x + 1 < _mapDimensions.first) {
AddNodeToQueue(node, node.x + 1, node.y);
}
// check the top node
if (0 <= node.y - 1) {
AddNodeToQueue(node, node.x, node.y - 1);
}
// check the bottom node
if (node.y + 1 < _mapDimensions.second) {
AddNodeToQueue(node, node.x, node.y + 1);
}
}
void PathFinder::AddNodeToQueue(const Node& parentNode, const int x, const int y)
{
const int index = FindIndex(x, y);
if (_distances[index] == -1 && _map[index] == 1) {
const int distanceFromStart = parentNode.distanceFromStart + 1;
const int weight = distanceFromStart + CalculateDistance(x, y);
_nodesQueue.push(Node(index, x, y, parentNode.index, distanceFromStart, weight));
}
}
int PathFinder::FindIndex(const int x, const int y)
{
return y * _mapDimensions.first + x;
}
int PathFinder::CalculateDistance(const int x, const int y)
{
return std::abs(_target.first - x) + std::abs(_target.second - y);
}