diff --git a/OneLoneCoder_PathFinding_AStar.cpp b/OneLoneCoder_PathFinding_AStar.cpp new file mode 100644 index 0000000..df756cd --- /dev/null +++ b/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 +#include +#include +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 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(y0) + nodes[y*nMapWidth + x].vecNeighbours.push_back(&nodes[(y + 0) * nMapWidth + (x - 1)]); + if(x0 && x>0) + nodes[y*nMapWidth + x].vecNeighbours.push_back(&nodes[(y - 1) * nMapWidth + (x - 1)]); + if (y0) + nodes[y*nMapWidth + x].vecNeighbours.push_back(&nodes[(y + 1) * nMapWidth + (x - 1)]); + if (y>0 && xx - 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 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; +} \ No newline at end of file