-
Notifications
You must be signed in to change notification settings - Fork 1
Expand file tree
/
Copy pathPathfinding.cpp
More file actions
112 lines (86 loc) · 2.77 KB
/
Pathfinding.cpp
File metadata and controls
112 lines (86 loc) · 2.77 KB
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
#include "Pathfinding.hpp"
void Pathfinder::pathfind(std::vector<Node>& nodes, Node* start_node, Node* end_node) {
std::vector<Node*> open_nodes;
std::vector<Node*> closed_nodes;
open_nodes.push_back(start_node);
while (open_nodes.size() > 0) {
Node* node = open_nodes[0];
int id = 0;
for (int i = 1; i < open_nodes.size(); i++) {
auto& opnode = open_nodes[i];
if (opnode->f_cost < node->f_cost || opnode->f_cost == node->f_cost) {
if (opnode->h_cost < node->h_cost) {
node = opnode;
id = i;
}
}
}
closed_nodes.push_back(node);
open_nodes.erase(open_nodes.begin() + id);
node = closed_nodes.back();
for (auto& n : this->getNeighbours(nodes, node)) {
if (!n->walkable || this->contains(closed_nodes, *n))
continue;
const int new_cost_to_neighbour = node->g_cost + this->getDistanceConst(*node, *n);
if (new_cost_to_neighbour < n->g_cost || !this->contains(open_nodes, *n)) {
n->g_cost = new_cost_to_neighbour;
n->h_cost = this->getDistanceConst(*n,*end_node);
n->f_cost = n->g_cost + n->h_cost;
n->parent = node;
if (*n == end_node) {
this->retracePath(n, start_node);
return;
}
if (!this->contains(open_nodes, *n))
open_nodes.push_back(n);
}
}
}
//LOG("sus");
}
void Pathfinder::retracePath(Node* end_node, Node* start_node) {
Node* current_node = end_node;
current_node->sign = '@';
while (*current_node != start_node) {
//LOG("sus");
current_node->sign = '@';
current_node = current_node->parent;
/*if (current_node == nullptr || current_node->parent == nullptr)
break;*/
}
current_node->sign = '@';
}
int Pathfinder::getIndex(const int X, const int Y) const {
return Y* SIZE_X + X;
}
int Pathfinder::getDistanceConst(const Node& n1, const Node& n2) const {
int dstX = abs(static_cast<int>(n1.x - n2.x));
int dstY = abs(static_cast<int>(n1.y - n2.y));
if (dstX > dstY)
return 14 * dstY + 10 * (dstX - dstY);
return 14 * dstX + 10 * (dstY - dstX);
}
bool Pathfinder::contains(std::vector<Node*> nodes, const Node& node) const {
for (auto& n : nodes)
if (*n == &node)
return true;
return false;
}
std::vector<Node*> Pathfinder::getNeighbours(std::vector<Node>& nodes, Node* current_node) {
std::vector<Node*> neighbours;
for (int y = -1; y <= 1; y++) {
for (int x = -1; x <= 1; x++) {
const int X = current_node->x + x;
const int Y = current_node->y + y;
if (X < 0 || Y < 0 || X > SIZE_X - 1 || Y > SIZE_Y - 1)
continue;
if (x == 0 && y == 0)
continue;
const int index = this->getIndex(X, Y);
if (index < 0 || index > nodes.size() - 1)
continue;
neighbours.push_back(&nodes[index]);
}
}
return neighbours;
}