Added PathFinding A* Video

OneLoneCoder_PathFinding_AStar.cpp

@@ -0,0 +1,290 @@
+/*
+OneLoneCoder.com - PathFinding A*
+"No more getting lost..." - @Javidx9
+
+Disclaimer
+~~~~~~~~~~
+I don't care what you use this for. It's intended to be educational, and perhaps
+to the oddly minded - a little bit of fun. Please hack this, change it and use it
+in any way you see fit. BUT, you acknowledge that I am not responsible for anything
+bad that happens as a result of your actions. However, if good stuff happens, I
+would appreciate a shout out, or at least give the blog some publicity for me.
+Cheers!
+
+Background
+~~~~~~~~~~
+The A* path finding algorithm is a widely used and powerful shortest path
+finding node traversal algorithm. A heuristic is used to bias the algorithm
+towards success. This code is probably more interesting than the video. :-/
+
+
+Author
+~~~~~~
+Twitter: @javidx9
+Blog: www.onelonecoder.com
+
+Video:
+~~~~~~
+https://youtu.be/icZj67PTFhc
+
+Last Updated: 08/10/2017
+*/
+
+#include <iostream>
+#include <string>
+#include <algorithm>
+using namespace std;
+
+#include "olcConsoleGameEngine.h"
+
+class OneLoneCoder_PathFinding : public olcConsoleGameEngine
+{
+public:
+	OneLoneCoder_PathFinding()
+	{
+		m_sAppName = L"Path Finding";
+	}
+
+private:
+
+	struct sNode
+	{
+		bool bObstacle = false;			// Is the node an obstruction?
+		bool bVisited = false;			// Have we searched this node before?
+		float fGlobalGoal;				// Distance to goal so far
+		float fLocalGoal;				// Distance to goal if we took the alternative route
+		int x;							// Nodes position in 2D space
+		int y;
+		vector<sNode*> vecNeighbours;	// Connections to neighbours
+		sNode* parent;					// Node connecting to this node that offers shortest parent
+	};
+
+	sNode *nodes = nullptr;
+	int nMapWidth = 16;
+	int nMapHeight = 16;
+
+	sNode *nodeStart = nullptr;
+	sNode *nodeEnd = nullptr;
+	
+
+protected:
+	virtual bool OnUserCreate()
+	{
+		// Create a 2D array of nodes - this is for convenience of rendering and construction
+		// and is not required for the algorithm to work - the nodes could be placed anywhere
+		// in any space, in multiple dimensions...
+		nodes = new sNode[nMapWidth * nMapHeight];
+		for (int x = 0; x < nMapWidth; x++)
+			for (int y = 0; y < nMapHeight; y++)
+			{
+				nodes[y * nMapWidth + x].x = x; // ...because we give each node its own coordinates
+				nodes[y * nMapWidth + x].y = y;
+				nodes[y * nMapWidth + x].bObstacle = false;
+				nodes[y * nMapWidth + x].parent = nullptr;
+				nodes[y * nMapWidth + x].bVisited = false;
+			}
+
+		// Create connections - in this case nodes are on a regular grid
+		for (int x = 0; x < nMapWidth; x++)
+			for (int y = 0; y < nMapHeight; y++)
+			{
+				if(y>0)
+					nodes[y*nMapWidth + x].vecNeighbours.push_back(&nodes[(y - 1) * nMapWidth + (x + 0)]);
+				if(y<nMapHeight-1)
+					nodes[y*nMapWidth + x].vecNeighbours.push_back(&nodes[(y + 1) * nMapWidth + (x + 0)]);
+				if (x>0)
+					nodes[y*nMapWidth + x].vecNeighbours.push_back(&nodes[(y + 0) * nMapWidth + (x - 1)]);
+				if(x<nMapWidth-1)
+					nodes[y*nMapWidth + x].vecNeighbours.push_back(&nodes[(y + 0) * nMapWidth + (x + 1)]);
+
+				// We can also connect diagonally
+				/*if (y>0 && x>0)
+					nodes[y*nMapWidth + x].vecNeighbours.push_back(&nodes[(y - 1) * nMapWidth + (x - 1)]);
+				if (y<nMapHeight-1 && x>0)
+					nodes[y*nMapWidth + x].vecNeighbours.push_back(&nodes[(y + 1) * nMapWidth + (x - 1)]);
+				if (y>0 && x<nMapWidth-1)
+					nodes[y*nMapWidth + x].vecNeighbours.push_back(&nodes[(y - 1) * nMapWidth + (x + 1)]);
+				if (y<nMapHeight - 1 && x<nMapWidth-1)
+					nodes[y*nMapWidth + x].vecNeighbours.push_back(&nodes[(y + 1) * nMapWidth + (x + 1)]);
+				*/
+			}
+
+		// Manually positio the start and end markers so they are not nullptr
+		nodeStart = &nodes[(nMapHeight / 2) * nMapWidth + 1];
+		nodeEnd = &nodes[(nMapHeight / 2) * nMapWidth + nMapWidth-2];
+		return true;
+	}
+
+	bool Solve_AStar()
+	{
+		// Reset Navigation Graph - default all node states
+		for (int x = 0; x < nMapWidth; x++)
+			for (int y = 0; y < nMapHeight; y++)
+			{
+				nodes[y*nMapWidth + x].bVisited = false;
+				nodes[y*nMapWidth + x].fGlobalGoal = INFINITY;
+				nodes[y*nMapWidth + x].fLocalGoal = INFINITY;
+				nodes[y*nMapWidth + x].parent = nullptr;	// No parents
+			}
+
+		auto distance = [](sNode* a, sNode* b) // For convenience
+		{
+			return sqrtf((a->x - b->x)*(a->x - b->x) + (a->y - b->y)*(a->y - b->y));
+		};
+
+		auto heuristic = [distance](sNode* a, sNode* b) // So we can experiment with heuristic
+		{
+			return distance(a, b);
+		};
+
+		// Setup starting conditions
+		sNode *nodeCurrent = nodeStart;
+		nodeStart->fLocalGoal = 0.0f;
+		nodeStart->fGlobalGoal = heuristic(nodeStart, nodeEnd);
+
+		// Add start node to not tested list - this will ensure it gets tested.
+		// As the algorithm progresses, newly discovered nodes get added to this
+		// list, and will themselves be tested later
+		list<sNode*> listNotTestedNodes;
+		listNotTestedNodes.push_back(nodeStart);
+
+		// if the not tested list contains nodes, there may be better paths
+		// which have not yet been explored. However, we will also stop 
+		// searching when we reach the target - there may well be better
+		// paths but this one will do - it wont be the longest.
+		while (!listNotTestedNodes.empty() && nodeCurrent != nodeEnd)// Find absolutely shortest path // && nodeCurrent != nodeEnd)
+		{
+			// Sort Untested nodes by global goal, so lowest is first
+			listNotTestedNodes.sort([](const sNode* lhs, const sNode* rhs){ return lhs->fGlobalGoal < rhs->fGlobalGoal; } );
+			
+			// Front of listNotTestedNodes is potentially the lowest distance node. Our
+			// list may also contain nodes that have been visited, so ditch these...
+			while(!listNotTestedNodes.empty() && listNotTestedNodes.front()->bVisited)
+				listNotTestedNodes.pop_front();
+
+			// ...or abort because there are no valid nodes left to test
+			if (listNotTestedNodes.empty())
+				break;
+
+			nodeCurrent = listNotTestedNodes.front();
+			nodeCurrent->bVisited = true; // We only explore a node once
+			
+					
+			// Check each of this node's neighbours...
+			for (auto nodeNeighbour : nodeCurrent->vecNeighbours)
+			{
+				// ... and only if the neighbour is not visited and is 
+				// not an obstacle, add it to NotTested List
+				if (!nodeNeighbour->bVisited && nodeNeighbour->bObstacle == 0)
+					listNotTestedNodes.push_back(nodeNeighbour);
+
+				// Calculate the neighbours potential lowest parent distance
+				float fPossiblyLowerGoal = nodeCurrent->fLocalGoal + distance(nodeCurrent, nodeNeighbour);
+
+				// If choosing to path through this node is a lower distance than what 
+				// the neighbour currently has set, update the neighbour to use this node
+				// as the path source, and set its distance scores as necessary
+				if (fPossiblyLowerGoal < nodeNeighbour->fLocalGoal)
+				{
+					nodeNeighbour->parent = nodeCurrent;
+					nodeNeighbour->fLocalGoal = fPossiblyLowerGoal;
+
+					// The best path length to the neighbour being tested has changed, so
+					// update the neighbour's score. The heuristic is used to globally bias
+					// the path algorithm, so it knows if its getting better or worse. At some
+					// point the algo will realise this path is worse and abandon it, and then go
+					// and search along the next best path.
+					nodeNeighbour->fGlobalGoal = nodeNeighbour->fLocalGoal + heuristic(nodeNeighbour, nodeEnd);
+				}
+			}	
+		}
+
+		return true;
+	}
+
+	virtual bool OnUserUpdate(float fElapsedTime)
+	{
+		int nNodeSize = 9;
+		int nNodeBorder = 2;
+
+		// Use integer division to nicely get cursor position in node space
+		int nSelectedNodeX = m_mousePosX / nNodeSize;
+		int nSelectedNodeY = m_mousePosY / nNodeSize;
+
+		if (m_mouse[0].bReleased) // Use mouse to draw maze, shift and ctrl to place start and end
+		{
+			if(nSelectedNodeX >=0 && nSelectedNodeX < nMapWidth)
+				if (nSelectedNodeY >= 0 && nSelectedNodeY < nMapHeight)
+				{
+					if (m_keys[VK_SHIFT].bHeld)
+						nodeStart = &nodes[nSelectedNodeY * nMapWidth + nSelectedNodeX];
+					else if (m_keys[VK_CONTROL].bHeld)
+						nodeEnd = &nodes[nSelectedNodeY * nMapWidth + nSelectedNodeX];
+					else
+						nodes[nSelectedNodeY * nMapWidth + nSelectedNodeX].bObstacle = !nodes[nSelectedNodeY * nMapWidth + nSelectedNodeX].bObstacle;
+
+					Solve_AStar(); // Solve in "real-time" gives a nice effect
+				}
+		}
+
+		// Draw Connections First - lines from this nodes position to its
+		// connected neighbour node positions
+		Fill(0, 0, ScreenWidth(), ScreenHeight(), L' ');
+		for (int x = 0; x < nMapWidth; x++)
+			for (int y = 0; y < nMapHeight; y++)
+			{
+				for (auto n : nodes[y*nMapWidth + x].vecNeighbours)
+				{
+					DrawLine(x*nNodeSize + nNodeSize / 2, y*nNodeSize + nNodeSize / 2,
+						n->x*nNodeSize + nNodeSize / 2, n->y*nNodeSize + nNodeSize / 2, PIXEL_SOLID, FG_DARK_BLUE);
+				}
+			}
+
+		// Draw Nodes on top
+		for (int x = 0; x < nMapWidth; x++)
+			for (int y = 0; y < nMapHeight; y++)
+			{
+	
+				Fill(x*nNodeSize + nNodeBorder, y*nNodeSize + nNodeBorder, 
+					(x + 1)*nNodeSize - nNodeBorder, (y + 1)*nNodeSize - nNodeBorder, 
+					PIXEL_HALF, nodes[y * nMapWidth + x].bObstacle ? FG_WHITE : FG_BLUE);
+
+				if (nodes[y * nMapWidth + x].bVisited)
+					Fill(x*nNodeSize + nNodeBorder, y*nNodeSize + nNodeBorder, (x + 1)*nNodeSize - nNodeBorder, (y + 1)*nNodeSize - nNodeBorder, PIXEL_SOLID, FG_BLUE);
+
+				if(&nodes[y * nMapWidth + x] == nodeStart)
+					Fill(x*nNodeSize + nNodeBorder, y*nNodeSize + nNodeBorder, (x + 1)*nNodeSize - nNodeBorder, (y + 1)*nNodeSize - nNodeBorder, PIXEL_SOLID, FG_GREEN);
+
+				if(&nodes[y * nMapWidth + x] == nodeEnd)
+					Fill(x*nNodeSize + nNodeBorder, y*nNodeSize + nNodeBorder, (x + 1)*nNodeSize - nNodeBorder, (y + 1)*nNodeSize - nNodeBorder, PIXEL_SOLID, FG_RED);						
+				
+			}
+
+
+		// Draw Path by starting ath the end, and following the parent node trail
+		// back to the start - the start node will not have a parent path to follow
+		if (nodeEnd != nullptr)
+		{
+			sNode *p = nodeEnd;
+			while (p->parent != nullptr)
+			{
+				DrawLine(p->x*nNodeSize + nNodeSize / 2, p->y*nNodeSize + nNodeSize / 2,
+					p->parent->x*nNodeSize + nNodeSize / 2, p->parent->y*nNodeSize + nNodeSize / 2, PIXEL_SOLID, FG_YELLOW);
+				
+				// Set next node to this node's parent
+				p = p->parent;
+			}
+		}
+
+		return true;
+	}
+
+};
+
+int main()
+{
+	OneLoneCoder_PathFinding game;
+	game.ConstructConsole(160, 160, 6, 6);
+	game.Start();
+	return 0;
+}