working algorithm, just need to refine it

7 files changed, 144 insertions, 38 deletions

diff --git a/bin/main b/bin/main
index d49be03..fff4c61 100755
--- a/bin/main
+++ b/bin/main
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<Node> 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;
@@ -78,6 +92,37 @@ T PriorityQueue<T>::get_first() {
 }
 
 template<typename T>
+bool PriorityQueue<T>::check_item(const T& item) {
+	for(unsigned int i = 0; i < size; ++i)
+		if(priority_array[i] == item)
+			return true;
+	return false;
+}
+
+template<typename T>
+int PriorityQueue<T>::get_index(const T& item) {
+	for(unsigned int i = 0; i < size; ++i)
+		if(priority_array[i] == item)
+			return i;
+	return -1;
+}
+
+template<typename T>
+void PriorityQueue<T>::remove_item(const T& item) {
+	remove_queue(get_index(item));
+}
+
+template<typename T>
+const T& PriorityQueue<T>::at(const unsigned int& i) const {
+	return priority_array[i];
+}
+
+template<typename T>
+const T& PriorityQueue<T>::operator[](const unsigned int& i) const {
+	return priority_array[i];
+}
+
+template<typename T>
 void PriorityQueue<T>::resize_queue(double factor) {
 	capacity = (double)capacity * factor;
 	T *tmp_array = new T[capacity];
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 <cmath>
+#include <iostream>
+
+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<Node> 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) {