From 64a9f25d64c59ba2d6fd8c232d8be0aaa6a5da31 Mon Sep 17 00:00:00 2001 From: ymherklotz Date: Sat, 10 Dec 2016 15:24:21 +0000 Subject: working algorithm, just need to refine it --- bin/main | Bin 236616 -> 291712 bytes include/astar.hpp | 15 +++++--- include/node.hpp | 7 ---- include/priority_queue.hpp | 45 ++++++++++++++++++++++++ src/astar.cpp | 86 +++++++++++++++++++++++++++++++++++++++++---- src/main.cpp | 7 ++++ src/node.cpp | 22 +----------- 7 files changed, 144 insertions(+), 38 deletions(-) diff --git a/bin/main b/bin/main index d49be03..fff4c61 100755 Binary files a/bin/main and b/bin/main differ diff --git a/include/astar.hpp b/include/astar.hpp index a3d2178..8668c88 100644 --- a/include/astar.hpp +++ b/include/astar.hpp @@ -9,11 +9,12 @@ // defines the number of nodes one can go to // we don't need this as we will have the queues -//#define NEIGHBOUR_NUM 4 +#define NEIGHBOUR_NUM 4 // TODO add constructors and functions to calculate heuristics etc.. class AStar { public: + AStar(); AStar(int *curr_graph, const unsigned int& width, const unsigned int& height); private: PriorityQueue open_set; @@ -22,14 +23,20 @@ private: int *graph; int graph_width; int graph_height; + int path_length; - Node current_node, start_node, end_node; + Node start_node, end_node; - void set_start_end(); + bool start_algorithm(); - Node get_neighbour(const Node& n_in, const int& neighbour_num); + Node get_neighbour(Node& n_in, const int& neighbour_num); void calc_heuristic(Node& n); + void calc_f(Node& n); + + bool check_item_vec(const Node& n); + int get_index_vec(const Node& n); + void remove_from_vec(const Node& n); }; #endif // ASTAR_HPP diff --git a/include/node.hpp b/include/node.hpp index 3b81b1c..4b19560 100644 --- a/include/node.hpp +++ b/include/node.hpp @@ -10,13 +10,6 @@ class Node { public: Node(); - void set_x(int x_in); - void set_y(int y_in); - void set_h_score(int h); - void set_g_score(int g); - - void compute_f(); - // overloading operators for ease of use. friend bool operator<(const Node& n1, const Node& n2); friend bool operator==(const Node& n1, const Node& n2); diff --git a/include/priority_queue.hpp b/include/priority_queue.hpp index c9fd6f8..af9a4b1 100644 --- a/include/priority_queue.hpp +++ b/include/priority_queue.hpp @@ -20,6 +20,20 @@ public: // returns the lowest priority item T get_first(); + + // check if item is in queue + bool check_item(const T& item); + + // gets index of item + int get_index(const T& item); + + // removes item from queue + void remove_item(const T& item); + + // return element at position; + const T& at(const unsigned int& i) const; + + const T& operator[](const unsigned int& i) const; private: // size of the current queue unsigned int size; @@ -77,6 +91,37 @@ T PriorityQueue::get_first() { return priority_array[0]; } +template +bool PriorityQueue::check_item(const T& item) { + for(unsigned int i = 0; i < size; ++i) + if(priority_array[i] == item) + return true; + return false; +} + +template +int PriorityQueue::get_index(const T& item) { + for(unsigned int i = 0; i < size; ++i) + if(priority_array[i] == item) + return i; + return -1; +} + +template +void PriorityQueue::remove_item(const T& item) { + remove_queue(get_index(item)); +} + +template +const T& PriorityQueue::at(const unsigned int& i) const { + return priority_array[i]; +} + +template +const T& PriorityQueue::operator[](const unsigned int& i) const { + return priority_array[i]; +} + template void PriorityQueue::resize_queue(double factor) { capacity = (double)capacity * factor; diff --git a/src/astar.cpp b/src/astar.cpp index b556d76..ba89595 100644 --- a/src/astar.cpp +++ b/src/astar.cpp @@ -1,27 +1,101 @@ #include "astar.hpp" -AStar::AStar(int *curr_graph, const unsigned int& width, const unsigned int& height) : graph(curr_graph), graph_width(width), graph_height(height) { - set_start_end(); - open_set.push(start_node); +#include +#include + +AStar::AStar() : graph(NULL), graph_width(0), graph_height(0), path_length(1) { } -void AStar::set_start_end() { +AStar::AStar(int *curr_graph, const unsigned int& width, const unsigned int& height) : graph(curr_graph), graph_width(width), graph_height(height), path_length(1) { for(unsigned int i = 0; i < graph_width * graph_height; ++i) if(graph[i] == 3) { start_node.x = i % graph_width; start_node.y = i / graph_width; start_node.g_score = 0; + calc_heuristic(start_node); + calc_f(start_node); } else if(graph[i] == 2) { end_node.x = i % graph_width; end_node.y = i / graph_width; - end_node.h_score = 0; } + open_set.push(start_node); + start_algorithm(); } -Node AStar::get_neighbour(const Node& n_in, const int& neighbour_num) { +bool AStar::start_algorithm() { + while(open_set.get_first() != end_node) { + Node current = open_set.pop(); + closed_set.push_back(current); + for(unsigned int i = 0; i < NEIGHBOUR_NUM; ++i) { + Node n = get_neighbour(current, i); + if(graph[n.y * graph_width + n.x] != 1) { + int cost = current.g_score + path_length; + if(open_set.check_item(n)) + if(cost < open_set[open_set.get_index(n)].g_score) + open_set.remove_item(n); + if(check_item_vec(n)) + if(cost < closed_set[get_index_vec(n)].g_score) + remove_from_vec(n); + if(!open_set.check_item(n) && !check_item_vec(n)) { + n.g_score = cost; + calc_f(n); + open_set.push(n); + } + } + } + } + return true; +} + +Node AStar::get_neighbour(Node& n_in, const int& neighbour_num) { + Node n; + if(neighbour_num == 0) { + n.x = n_in.x; + n.y = n_in.y - 1; + } else if(neighbour_num == 1) { + n.x = n_in.x + 1; + n.y = n_in.y; + } else if(neighbour_num == 2) { + n.x = n_in.x; + n.y = n_in.y + 1; + } else { + n.x = n_in.x - 1; + n.y = n_in.y; } + + n.previous_node = &n_in; + calc_heuristic(n); + + return n; } void AStar::calc_heuristic(Node& n) { + n.h_score = path_length * (abs(n.x - end_node.x) + abs(n.y - end_node.y)); +} + +void AStar::calc_f(Node& n) { + n.f_score = n.g_score + n.h_score; +} + +bool AStar::check_item_vec(const Node& n) { + for(unsigned int i = 0; i < closed_set.size(); ++i) + if(closed_set[i] == n) + return true; + return false; +} + +int AStar::get_index_vec(const Node& n) { + for(unsigned int i = 0; i < closed_set.size(); ++i) + if(closed_set[i] == n) + return i; + return -1; +} + +void AStar::remove_from_vec(const Node& n) { + std::vector tmp_set; + for(unsigned int i = 0; i < closed_set.size(); ++i) + if(closed_set[i] != n) + tmp_set.push_back(closed_set[i]); + closed_set = tmp_set; } diff --git a/src/main.cpp b/src/main.cpp index 494ff43..c0b9b00 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -16,5 +16,12 @@ using namespace std; int main(int argc, char *argv[]) { + int graph[9] = { + 3, 0, 0, + 0, 1, 1, + 0, 0, 2 + }; + AStar a(graph, 3, 3); + return 0; } diff --git a/src/node.cpp b/src/node.cpp index 9273376..b61c154 100644 --- a/src/node.cpp +++ b/src/node.cpp @@ -3,26 +3,6 @@ Node::Node() : previous_node(NULL), f_score(-1), g_score(0), h_score(-1), x(0), y(0) { } -void Node::set_x(int x_in) { - x = x_in; -} - -void Node::set_y(int y_in) { - y = y_in; -} - -void Node::set_h_score(int h) { - h_score = h; -} - -void Node::set_g_score(int g) { - g_score = g; -} - -void Node::compute_f() { - f_score = g_score + h_score; -} - bool operator<(const Node& n1, const Node& n2) { if(n1.f_score == n2.f_score) return n1.h_score < n2.h_score; @@ -30,7 +10,7 @@ bool operator<(const Node& n1, const Node& n2) { } bool operator==(const Node& n1, const Node& n2) { - return n1.f_score == n2.f_score; + return n1.x == n2.x && n1.y == n2.y; } bool operator>(const Node& n1, const Node& n2) { -- cgit