started implementing pathfinding

Author: digocorbellini

Grid.hpp

@@ -5,6 +5,7 @@
 #include "GridCellStates.hpp"
 #include "math.h"
 #include <stdio.h>
+#include <vector>
 
 using namespace std;
 using namespace sf;
@@ -40,10 +41,56 @@ class Grid
 	int getGridHeight();
 
 	bool validCoords(int x, int y);
+
+	vector<T*> *getNeighbours(int x, int y);
 
 	~Grid();
 };
 
+/// <summary>
+/// Get a list of all of the neighbouring cells around the given
+/// grid coordinates
+/// </summary>
+/// <param name="x">the x coordinate of a cell in the grid</param>
+/// <param name="y">the x coordinate of a cellin the grid</param>
+/// <returns>a list of all of the neighbouring cells around the given
+/// grid coordinates if the coordinates are valid and null otherwise</returns>
+template <typename T>
+vector<T*> *Grid<T>::getNeighbours(int x, int y)
+{
+	// check for invalid coords
+	if (!validCoords(x, y))
+	{
+		return NULL;
+	}
+
+	// create a list to store the neighbouring cells
+	vector<T*>* neighbours = new vector<T*>();
+
+	// find all neighbours around given coordinates
+	for (int relativeX = -1; relativeX < 2; relativeX++)
+	{
+		for (int relativeY = -1; relativeY < 2; relativeY++)
+		{
+			// skip the center cell
+			if (x == 0 && y == 0)
+			{
+				continue;
+			}
+
+			int currX = x + relativeX;
+			int currY = y + relativeY;
+			// add neighbouring cell if it exists in the grid
+			if (validCoords(currX, currY))
+			{
+				neighbours->push_back(getValueAt(currX, currY));
+			}
+		}
+	}
+
+	return neighbours;
+}
+
 /// <summary>
 /// Get the grid's height
 /// </summary>
@@ -115,8 +162,8 @@ Grid<T>::Grid(int width, int height, int cellSize)
 /// <summary>
 /// Get the value at the given grid coordinates
 /// </summary>
-/// <param name="x">the row cooridnate</param>
-/// <param name="y">the column coordinate</param>
+/// <param name="relativeX">the row cooridnate</param>
+/// <param name="relativeY">the column coordinate</param>
 /// <returns>if the coordinates are valid, returns the value at the given grid 
 /// coordinates, and returns false otherwise</returns>
 template <typename T>
@@ -133,8 +180,8 @@ T *Grid<T>::getValueAt(int x, int y)
 /// <summary>
 /// Convert the grid position to a screen position
 /// </summary>
-/// <param name="x">the x coordinate of the desired grid space</param>
-/// <param name="y">the y coordinate of the dedired grid space</param>
+/// <param name="relativeX">the relativeX coordinate of the desired grid space</param>
+/// <param name="relativeY">the relativeY coordinate of the dedired grid space</param>
 /// <returns>a screen position in pixels as a Vector2f of the given cell
 /// in the grid if the given coordinates are valid, otherwise returns NULL</returns>
 template <typename T>
@@ -163,8 +210,8 @@ Vector2i Grid<T>::screenToGrid(Vector2i pos)
 /// <summary>
 /// Check to see if the given grid coordinates are valid
 /// </summary>
-/// <param name="x">the x coordinate of the desired cell</param>
-/// <param name="y"the y coordinate of the desired cell></param>
+/// <param name="relativeX">the relativeX coordinate of the desired cell</param>
+/// <param name="relativeY"the relativeY coordinate of the desired cell></param>
 /// <returns>true if the cell is valid and false otherwise</returns>
 template <typename T>
 bool Grid<T>::validCoords(int x, int y)
@@ -175,8 +222,8 @@ bool Grid<T>::validCoords(int x, int y)
 /// <summary>
 /// Set the value of the cell at the given coordinates
 /// </summary>
-/// <param name="x">the x coordinate of the cell</param>
-/// <param name="y">the y coordinate of the cell</param>
+/// <param name="relativeX">the relativeX coordinate of the cell</param>
+/// <param name="relativeY">the relativeY coordinate of the cell</param>
 /// <param name="val">the new value of the given cell</param>
 /// <returns>true if the cell is valid and false otherwise</returns>
 template <typename T>

PathFinder.hpp

@@ -5,6 +5,8 @@
 #include "SFML/Graphics.hpp"
 #include "Grid.hpp"
 #include "GridCellStates.hpp"
+#include <vector>
+#include <unordered_set>
 
 using namespace std;
 using namespace sf;
@@ -18,7 +20,16 @@ class PathFinder
 	// node for each grid position
 	struct GridNode
 	{
-		GridValue val;
+		GridValue val; // value of this node
+		Vector2i gridPos; // position of this node in the grid
+		GridNode *parentNode; // node that came before this node in the path
+		int gCost; // distance from starting node
+		int hCost; // distance from end node
+
+		int fCost()
+		{
+			return gCost + hCost;
+		}
 	};
 
 private:
@@ -29,6 +40,17 @@ class PathFinder
 	Vector2i* startPos;
 	Vector2i* endPos;
 
+	// custom hashing for GridNodes
+	struct NodeHash
+	{
+		size_t operator()(const GridNode& node) const
+		{
+			return node.gCost + node.hCost + (int)node.val;
+		}
+	};
+
+	int getDistance(GridNode* node1, GridNode* node2);
+
 public:
 	PathFinder(int width, int height, int cellSize);
 
@@ -44,6 +66,8 @@ class PathFinder
 
 	bool setValAt(Vector2i pos, GridValue val);
 
+	GridNode* getShortestPath();
+
 private:
 	void initializeNodes();
 
@@ -58,7 +82,9 @@ void PathFinder::initializeNodes()
 	{
 		for (int y = 0; y < grid->getGridHeight(); y++)
 		{
-			grid->setValAt(x, y, new GridNode());
+			GridNode* newNode = new GridNode();
+			newNode->gridPos = Vector2i(x, y);
+			grid->setValAt(x, y, newNode);
 		}
 	}
 }
@@ -246,6 +272,67 @@ bool PathFinder::setValAt(Vector2i pos, GridValue val)
 	return setValAt(gridPos.x, gridPos.y, val);
 }
 
+int PathFinder::getDistance(GridNode* node1, GridNode* node2)
+{
+	int xDist = abs(node1->gridPos.x - node2->gridPos.x);
+	int yDist = abs(node1->gridPos.y - node2->gridPos.y);
+
+	int numOfDiagonals = min(xDist, yDist);
+	int numOfRegMoves = abs(xDist - yDist);
+}
+
+PathFinder::GridNode* PathFinder::getShortestPath()
+{
+	GridNode* startNode = grid->getValueAt(startPos->x, startPos->y);
+	GridNode* endNode = grid->getValueAt(endPos->x, endPos->y);
+
+	vector<GridNode*> openList;
+	unordered_set<GridNode*, NodeHash> closedSet;
+
+	openList.push_back(startNode);
+
+	while (openList.size() > 0)
+	{
+		// find node with lowest fcost or lowest hcost if fcost are the same
+		GridNode* lowestCostNode = openList[0];
+		int pos = 0;
+		for (int i = 1; i < openList.size(); i++)
+		{
+			GridNode* currNode = openList[i];
+			if (currNode->fCost() < lowestCostNode->fCost()
+					|| (currNode->fCost() == lowestCostNode->fCost()
+					&& currNode->hCost <= lowestCostNode->hCost))
+			{
+				lowestCostNode = currNode;
+				pos = i;
+			}
+		}
+
+		// remove lowest cost node from open list
+		vector<GridNode*>::iterator it = openList.begin() + pos;
+		openList.erase(it);
+		// add lowest cost node to closed set
+		closedSet.insert(lowestCostNode);
+
+		Vector2i lowestCostPos = lowestCostNode->gridPos;
+		vector<GridNode*>* neighbours = grid->getNeighbours(lowestCostPos.x, lowestCostPos.y);
+		for (int i = 0; i < neighbours->size(); i++)
+		{
+			GridNode* currNode = (*neighbours)[i];
+
+			if (currNode->val == GridValue::OCCUPIED
+				|| closedSet.find(currNode) != closedSet.end())
+			{
+				// go to next neighbour if this node is either occupied
+				// or is in the closed set
+				continue;
+			}
+
+
+		}
+
+	}
+}
 
 
 #endif // !PATH_FINDER_H