Skip to content

Commit c3119f6

Browse files
mlsdpkmlsdpk
committed
Add Dijkstra and Astar
1 parent cee2e18 commit c3119f6

File tree

13 files changed

+508
-9
lines changed

13 files changed

+508
-9
lines changed

‎CMakeLists.txt‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,8 @@ include_directories(include/States)
99
include_directories(include/States/Algorithms)
1010
include_directories(include/States/Algorithms/BFS)
1111
include_directories(include/States/Algorithms/DFS)
12+
include_directories(include/States/Algorithms/DIJKSTRA)
13+
include_directories(include/States/Algorithms/ASTAR)
1214

1315
set(EXECUTABLE_NAME "main")
1416

@@ -33,6 +35,14 @@ add_library(dfs
3335
src/States/Algorithms/DFS/DFS.cpp
3436
)
3537

38+
add_library(dijkstra
39+
src/States/Algorithms/DIJKSTRA/DIJKSTRA.cpp
40+
)
41+
42+
add_library(astar
43+
src/States/Algorithms/ASTAR/ASTAR.cpp
44+
)
45+
3646
# Find any version 2.X of SFML
3747
find_package(SFML 2.5.1 COMPONENTS system window graphics network audio)
3848

@@ -45,4 +55,6 @@ target_link_libraries(${EXECUTABLE_NAME}
4555
main_menu
4656
bfs
4757
dfs
58+
dijkstra
59+
astar
4860
)

‎include/States/Algorithms/ASTAR/ASTAR.h‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
#pragma once
2+
3+
#include <queue>
4+
5+
#include "Algorithm.h"
6+
#include "Gui.h"
7+
#include "Node.h"
8+
9+
// custom function for returning minimum distance node
10+
// to be used in priority queue
11+
struct MinimumDistanceASTAR {
12+
// operator overloading
13+
bool operator()(const std::shared_ptr<Node> &n1,
14+
const std::shared_ptr<Node> &n2) const {
15+
return n1->getFDistance() > n2->getFDistance();
16+
}
17+
};
18+
19+
class ASTAR : public Algorithm {
20+
private:
21+
// ASTAR related
22+
std::priority_queue<std::shared_ptr<Node>, std::vector<std::shared_ptr<Node>>,
23+
MinimumDistanceASTAR>
24+
frontier_;
25+
26+
// override initialization Functions
27+
void initAlgorithm() override;
28+
29+
public:
30+
// Constructor
31+
ASTAR(sf::RenderWindow *window, std::stack<std::unique_ptr<State>> &states);
32+
33+
// Destructor
34+
virtual ~ASTAR();
35+
36+
// override update functions
37+
void updateNodes() override;
38+
39+
// override render functions
40+
void renderNodes() override;
41+
42+
// ASTAR algorithm function
43+
void solveConcurrently(
44+
std::shared_ptr<Node> nodeStart, std::shared_ptr<Node> nodeEnd,
45+
std::shared_ptr<MessageQueue<bool>> message_queue) override;
46+
47+
// new utility function
48+
double L1_Distance(const std::shared_ptr<Node> &n1,
49+
const std::shared_ptr<Node> &n2);
50+
};

‎include/States/Algorithms/Algorithm.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -58,7 +58,7 @@ class Algorithm : public State {
5858
// initialization Functions
5959
void initFonts();
6060
void initColors();
61-
void initBackground();
61+
void initBackground(const std::string& algo_name);
6262
void initButtons();
6363
void initVariables();
6464
void initNodes();
@@ -69,7 +69,7 @@ class Algorithm : public State {
6969
public:
7070
// Constructor
7171
Algorithm(sf::RenderWindow* window,
72-
std::stack<std::unique_ptr<State>>& states);
72+
std::stack<std::unique_ptr<State>>& states, std::string algo_name);
7373

7474
// Destructor
7575
virtual ~Algorithm();

‎include/States/Algorithms/DIJKSTRA/DIJKSTRA.h‎

Lines changed: 51 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
#pragma once
2+
3+
#include <queue>
4+
5+
#include "Algorithm.h"
6+
#include "Gui.h"
7+
#include "Node.h"
8+
9+
// custom function for returning minimum distance node
10+
// to be used in priority queue
11+
struct MinimumDistanceDIJKSTRA {
12+
// operator overloading
13+
bool operator()(const std::shared_ptr<Node> &n1,
14+
const std::shared_ptr<Node> &n2) const {
15+
return n1->getGDistance() > n2->getGDistance();
16+
}
17+
};
18+
19+
class DIJKSTRA : public Algorithm {
20+
private:
21+
// DIJKSTRA related
22+
std::priority_queue<std::shared_ptr<Node>, std::vector<std::shared_ptr<Node>>,
23+
MinimumDistanceDIJKSTRA>
24+
frontier_;
25+
26+
// override initialization Functions
27+
void initAlgorithm() override;
28+
29+
public:
30+
// Constructor
31+
DIJKSTRA(sf::RenderWindow *window,
32+
std::stack<std::unique_ptr<State>> &states);
33+
34+
// Destructor
35+
virtual ~DIJKSTRA();
36+
37+
// override update functions
38+
void updateNodes() override;
39+
40+
// override render functions
41+
void renderNodes() override;
42+
43+
// DIJKSTRA algorithm function
44+
void solveConcurrently(
45+
std::shared_ptr<Node> nodeStart, std::shared_ptr<Node> nodeEnd,
46+
std::shared_ptr<MessageQueue<bool>> message_queue) override;
47+
48+
// new utility function
49+
double L1_Distance(const std::shared_ptr<Node> &n1,
50+
const std::shared_ptr<Node> &n2);
51+
};

‎include/States/Algorithms/Node.h‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
#pragma once
22

3+
#include <math.h>
4+
35
#include <SFML/Graphics.hpp>
46
#include <memory>
57
#include <vector>
@@ -14,6 +16,8 @@ class Node {
1416
sf::Vector2i pos_;
1517
std::vector<std::shared_ptr<Node>> vecNeighbours_;
1618
std::shared_ptr<Node> parent_;
19+
double gDist_;
20+
double fDist_;
1721

1822
public:
1923
// Constructor
@@ -32,6 +36,8 @@ class Node {
3236
sf::Vector2i getPos();
3337
std::shared_ptr<Node> getParentNode();
3438
const std::vector<std::shared_ptr<Node>>* getNeighbours() const;
39+
const double getGDistance() const;
40+
const double getFDistance() const;
3541

3642
// Mutators
3743
void setObstacle(bool b);
@@ -41,4 +47,6 @@ class Node {
4147
void setPosition(sf::Vector2i pos);
4248
void setNeighbours(std::shared_ptr<Node> node);
4349
void setParentNode(std::shared_ptr<Node> node);
50+
void setGDistance(double dist);
51+
void setFDistance(double dist);
4452
};

‎include/States/MainMenu_State.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,10 @@
44
#include "State.h"
55

66
// Algorithms headers
7+
#include "ASTAR.h"
78
#include "BFS.h"
89
#include "DFS.h"
10+
#include "DIJKSTRA.h"
911

1012
class MainMenu_State : public State {
1113
private:

‎src/States/Algorithms/ASTAR/ASTAR.cpp‎

Lines changed: 183 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,183 @@
1+
#include "ASTAR.h"
2+
3+
// Constructor
4+
ASTAR::ASTAR(sf::RenderWindow *window,
5+
std::stack<std::unique_ptr<State>> &states)
6+
: Algorithm(window, states, "A-STAR") {}
7+
8+
// Destructor
9+
ASTAR::~ASTAR() {}
10+
11+
// override initAlgorithm() function
12+
void ASTAR::initAlgorithm() {
13+
// initialize ASTAR by clearing frontier and add start node
14+
while (!frontier_.empty()) {
15+
frontier_.pop();
16+
}
17+
nodeStart_->setGDistance(0.0);
18+
nodeStart_->setFDistance(L1_Distance(nodeStart_, nodeEnd_));
19+
frontier_.push(nodeStart_);
20+
}
21+
22+
// override updateNodes() function
23+
void ASTAR::updateNodes() {
24+
if (sf::Mouse::isButtonPressed(sf::Mouse::Left) && getKeyTime()) {
25+
int localY = ((mousePositionWindow_.x - 300) / gridSize_);
26+
int localX = ((mousePositionWindow_.y - 60) / gridSize_);
27+
28+
if (localX >= 0 && localX < mapHeight_ / gridSize_) {
29+
if (localY >= 0 && localY < mapWidth_ / gridSize_) {
30+
if (!Algorithm_solved_) {
31+
if ((sf::Keyboard::isKeyPressed(sf::Keyboard::LShift))) {
32+
nodeStart_ = nodes_[(mapWidth_ / gridSize_) * localX + localY];
33+
} else if ((sf::Keyboard::isKeyPressed(sf::Keyboard::LControl))) {
34+
nodeEnd_ = nodes_[(mapWidth_ / gridSize_) * localX + localY];
35+
} else {
36+
nodes_[(mapWidth_ / gridSize_) * localX + localY]->setObstacle(
37+
!nodes_[(mapWidth_ / gridSize_) * localX + localY]
38+
->isObstacle());
39+
}
40+
} else {
41+
if ((sf::Keyboard::isKeyPressed(sf::Keyboard::LControl))) {
42+
nodeEnd_ = nodes_[(mapWidth_ / gridSize_) * localX + localY];
43+
}
44+
}
45+
}
46+
}
47+
}
48+
}
49+
50+
// override renderNodes() function
51+
void ASTAR::renderNodes() {
52+
for (int x = 0; x < mapHeight_ / gridSize_; x++) {
53+
for (int y = 0; y < mapWidth_ / gridSize_; y++) {
54+
float size = static_cast<float>(gridSize_);
55+
sf::RectangleShape rectangle(sf::Vector2f(size, size));
56+
rectangle.setOutlineThickness(2.f);
57+
rectangle.setOutlineColor(BGN_COL);
58+
rectangle.setPosition(300 + y * size, 60 + x * size);
59+
60+
if (nodes_[(mapWidth_ / gridSize_) * x + y]->isObstacle()) {
61+
rectangle.setFillColor(OBST_COL);
62+
} else {
63+
rectangle.setFillColor(IDLE_COL);
64+
}
65+
66+
if (nodes_[(mapWidth_ / gridSize_) * x + y]->isVisited()) {
67+
rectangle.setFillColor(VISITED_COL);
68+
}
69+
70+
if (nodes_[(mapWidth_ / gridSize_) * x + y]->isFrontier()) {
71+
rectangle.setFillColor(FRONTIER_COL);
72+
}
73+
74+
if (nodes_[(mapWidth_ / gridSize_) * x + y]->isPath()) {
75+
rectangle.setFillColor(START_COL);
76+
nodes_[(mapWidth_ / gridSize_) * x + y]->setPath(false);
77+
}
78+
79+
if (nodes_[(mapWidth_ / gridSize_) * x + y] == nodeStart_) {
80+
rectangle.setFillColor(START_COL);
81+
}
82+
83+
if (nodes_[(mapWidth_ / gridSize_) * x + y] == nodeEnd_) {
84+
rectangle.setFillColor(END_BORDER_COL);
85+
}
86+
87+
window_->draw(rectangle);
88+
}
89+
}
90+
91+
// visualizing path
92+
if (nodeEnd_ != nullptr) {
93+
std::shared_ptr<Node> current = nodeEnd_;
94+
95+
while (current->getParentNode() != nullptr && current != nodeStart_) {
96+
current->setPath(true);
97+
current = current->getParentNode();
98+
}
99+
}
100+
}
101+
102+
double ASTAR::L1_Distance(const std::shared_ptr<Node> &n1,
103+
const std::shared_ptr<Node> &n2) {
104+
return fabs(n1->getPos().x - n2->getPos().x) +
105+
fabs(n1->getPos().y - n2->getPos().y);
106+
}
107+
108+
void ASTAR::solveConcurrently(
109+
std::shared_ptr<Node> nodeStart, std::shared_ptr<Node> nodeEnd,
110+
std::shared_ptr<MessageQueue<bool>> message_queue) {
111+
// copy assignment
112+
// thread-safe due to shared_ptrs
113+
std::shared_ptr<Node> s_nodeStart = nodeStart;
114+
std::shared_ptr<Node> s_nodeEnd = nodeEnd;
115+
std::shared_ptr<MessageQueue<bool>> s_message_queue = message_queue;
116+
117+
bool solved = false;
118+
119+
double cycleDuration = 1; // duration of a single simulation cycle in ms
120+
// init stop watch
121+
auto lastUpdate = std::chrono::system_clock::now();
122+
123+
while (true) {
124+
// compute time difference to stop watch
125+
long timeSinceLastUpdate =
126+
std::chrono::duration_cast<std::chrono::milliseconds>(
127+
std::chrono::system_clock::now() - lastUpdate)
128+
.count();
129+
130+
if (timeSinceLastUpdate >= cycleDuration) {
131+
////////////////////////////
132+
// run the main algorithm //
133+
////////////////////////////
134+
if (!frontier_.empty()) {
135+
std::shared_ptr<Node> nodeCurrent = frontier_.top();
136+
nodeCurrent->setFrontier(false);
137+
nodeCurrent->setVisited(true);
138+
frontier_.pop();
139+
140+
if (nodeCurrent == s_nodeEnd) {
141+
solved = true;
142+
}
143+
144+
for (auto nodeNeighbour : *nodeCurrent->getNeighbours()) {
145+
if (nodeNeighbour->isVisited() || nodeNeighbour->isObstacle()) {
146+
continue;
147+
}
148+
149+
double dist = nodeCurrent->getGDistance() +
150+
L1_Distance(nodeCurrent, nodeNeighbour);
151+
152+
if (dist < nodeNeighbour->getGDistance()) {
153+
nodeNeighbour->setParentNode(nodeCurrent);
154+
nodeNeighbour->setGDistance(dist);
155+
156+
// f = g + h
157+
double f_dist = nodeCurrent->getGDistance() +
158+
L1_Distance(nodeNeighbour, s_nodeEnd);
159+
nodeNeighbour->setFDistance(f_dist);
160+
nodeNeighbour->setFrontier(true);
161+
frontier_.push(nodeNeighbour);
162+
}
163+
}
164+
} else {
165+
solved = true;
166+
}
167+
////////////////////////////
168+
169+
// reset stop watch for next cycle
170+
lastUpdate = std::chrono::system_clock::now();
171+
}
172+
173+
// sends an update method to the message queue using move semantics
174+
auto ftr = std::async(std::launch::async, &MessageQueue<bool>::send,
175+
s_message_queue, std::move(solved));
176+
ftr.wait();
177+
178+
if (solved) return;
179+
180+
// sleep at every iteration to reduce CPU usage
181+
std::this_thread::sleep_for(std::chrono::milliseconds(1));
182+
}
183+
}

0 commit comments

Comments
 (0)