Path Planning - A* (A-Star)

hello some of the more keen and eagle-eyed viewers may have noticed some not too subtle hints about a secret project that I'm working on and this is a project that's going to involve pathfinding and I thought it would be useful time to talk about an algorithm often use for path finding the a-star algorithm and to demonstrate the a-star algorithm I've created an application in the console game engine to explore and show how the algorithm works and it's going to draw a yellow line which is supposed to be the shortest path between the green and red elements of this array now I've got these in a tile style but they don't have to be these nodes could be in any kind of space you want it's just it's convenient to work with them in a 2d grid and I can choose a new start point and a new end point by holding down Shift + ctrl and clicking I've implemented the mouse features of the console game engine to do this is quite nice and what we see is some of these cells go to a lighter shade of blue we'll discuss that in a minute and but it comes up with a path and I can then place individual blockages around the maze and we can see the path adapts to try and find always the shortest path and if there is no solution at all such as if I box in here the output then no path can be found and this is really quite fun to play with the lighter shaded squares show these cells that have been tested for pathing so you can see up here somewhere it's darker some of these cells have just not been tested at all that's because the a-star algorithm can reject certain paths earlier on and it doesn't matter how convoluted the path is so long as there is a solution the algorithm should be able to find it and it's guaranteed to be the shortest under normal circumstances now normally in my videos I like to take an algorithm and build it up step by step and so we can see what changes do what but that's a bit tricky with the a-star algorithm because it doesn't work until it's complete so to be a bit different I'm going to do a manual walkthrough of a simple a-star algorithm and then we'll implement the code so let's go through a worked example here I've got a graph but I'm going to try and leave the graph theory out of it well we've got some nodes and each node is labeled a b c d e and f and in between the nodes i've got some lines which represents the distance between nodes I've started off by marking my starting point which is node a and I've marked my ending point which is node E and for the algorithm we're going to visit each node and we're going to run a little routine and through a combination of goals we can work out the shortest path now this may look a little complicated to start with but it actually isn't that hard at all and once we get going you'll see the rules are quite simple I've initialized the graph to show the local and global goals for each node now the global goal is really a measure of how well is the algorithm performing how close to the end result is that node and this will be given by heuristic which is effectively the line of sight the as the crow flies distance between that node and the target node and we'll be updating this and biasing it as we go through the algorithm the local goal marked in blue is used to compare nodes so nodes can exchange information with each other about whether they're potentially the shortest path or not the final structure is a list and this will be lists of nodes that have been discovered by the algorithm that need to be tested and right now at the start we've only got one it's our starting node so that's listed as a and I've also included the global goal because we'll want to sort this list later on so let's get stuck in the algorithm goes as follows for each neighbor of a we want to run some code and in this case it's the first neighbor is going to be B so we've discovered a new node we need to add that to the list which is B and currently it's global goal is infinity and the calculation is as follows I take my current node which is a I take the local goal value I added to the distance to my neighbor and if that sum is less than my neighbors local goal value then I get to update my neighbor so in this case I've got zero plus two and two I believe is less than infinity so we definitely want to update node B and we'll update it in the following manner first thing we'll do is give node B a parent which in this case is a and then we want to update node B's local goal with the zero plus two which becomes two and then we want to update the global goal which is the local goal to plus the heuristic and in this case our heuristic is this direct line-of-sight value this could be calculated with Pythagoras's theorem or any other equation you want that represents the space of your environment through which you're planning a path but in this case I'm just going to take the obvious route which is about three so I'll update the global goal to two plus the three becomes a five and we mustn't forget to also update that in our list don't forget these nodes will be objects in coats when you update them in one place they're updated in all places we're done with B for the moment so now it's time to check age next neighbor which let's say is e and we do exactly the same again we've discovered a new node so I'll add that to the list well at its current global goal which is infinity and we'll do the following check we'll take a local goal zero add it to the distance between the two and if it's less than the neighboring nodes local goal then we update our neighboring node so 0 plus 1 is less than infinity we're going to see this pattern for the start of the algorithm so we get to update e we set its parent I'm going to use green if you haven't noticed already to represent the parents and I want to update the local metric 0 plus 1 and we write that into neighboring nodes local goal and then for the heuristic we take the local goal value which in this case is 1 for E and we add it to our heuristic value which I'm going to say is 5 in this case so that gives us a total of 6 I'm going to get rid of the dotted yellow lines because I'm already confused and I'm sure this won't help we've updated e now it's time to move on to F which is the final neighbor of a so we take the 0 we add the 3 to it is that less than the infinity yes it is so we get to update it but we've discovered a new node as well so let's also add that to the list F with infinity I've already forgotten to update the e infinity that's now changed to a 6 this is quite important that we do this so we know that we've got a new parent node for F this becomes a as well and we can update the local and global score so we update the local by taking the zero plus the three that gives us three and we can update our global by taking the local and adding the heuristic to it so in this case it's three plus this one we'll assume is the distance to the end goal which of course becomes four and we'll also change that in our list we've run out of neighbors for a now so we can remove that from the list and we'll mark a as being visited we're not going to want to process it again so we won't add it to the list again we may still update it but we won't be adding it to the list and once we've removed a we also then want to sort the list so here we can see we've got B E and F but actually the order should be four five and six so we'll move the F to the front because we want to sort in ascending global goals because this gives us the best chance of finding the shortest path because what our algorithm has learnt so far is that B is potentially five away from the end result is potentially six and F is potentially four so we may as well start with the most promising node so we'll take F and then we repeat the procedure we first check its neighbors well its first neighbor let's assume is a so we'll do the routine again we take FS local value we add it to the distance between them six is six less than zero well no it's not so F doesn't get to update node a this time let's move on to its next neighbor which is e so we take the local three plus one which is four is that less than E's local one no it's not so we don't get to update a this time we'll check FS final neighbor now which is D so we found a new node so we'll just add that to the list and we know protect the local three plus the distance one well three plus one is less than infinity so yes we do get to update D this time so we'll set the parent node to fi this is the node that updated it last the local goal we can set to four that's the local goal of F plus the one and we can update the heuristic which is the local goal of D plus well the distance to the end which in this case zero so that also gets set to four well update that in the list well so the final step is we've now visited F we've done processing it we've checked its neighbors we can tick that off now and we want to restore the list again well in this case it's just moving the D to the front now it's clearly obvious we've found a path we've got from node a to node e we go through F but we don't know if it's the shortest path it's not bad but there could be better ones so we don't want to stop the algorithm prematurely it's found a path which is great but it's not necessarily the shortest path now because D is our target path we don't really want to update any nodes from it it doesn't really make much sense that the path would be going backwards and forming loops so we don't actually check D we can remove that from the list which leaves us with B and E well B has two neighbors so let's check against a first and we'll check if the local of B 2 plus the distance 2 is less than a local well it isn't so B doesn't get to update a and that that's good that makes some sort of sense P's only other neighbor is C so we've discovered a new node which is C and that's infinity so we've now got B's local 2 plus the distance 1 gives us a 3 is 3 less than infinity yes it is so B gets to update node C and so we'll set the parent of node C to node B and we can update the local score as being B's local Plus that distance that becomes a 3 and we'll set the global score as being C's local goal which is 3 plus the distance to the end this gives us a 5 and we'll update the global goal in our list B has no more neighbors and we've now visited it so we can tick be off we can cross it off our list will sort the list which means we need to flip these around so that's becomes C and then E and we'll check node C well node C has three neighbors the first one is B so C's local is 3 plus the 1 well that's not less than 2 and you can kind of tell that this is stopping the algorithm from going backwards so see doesn't get to update be let's try neighbor e so we've got three plus the one well four isn't less than ease local so she doesn't get to update e either and this is quite good because so far the algorithm has learnt that the path to C is three steps going through B but the path to see through E is only two steps so the algorithm is stopping itself from going back the only neighbor left to check then is D so we'll take the three local so three plus the distance two is five which isn't less than these local so C doesn't get to update D in fact C is pretty boring it's it's it's established that it's not part of the shortest path the shortest path has already been found so we can now finish with C we visited it so we can cross that off our list and we've only got one node left to check which is e and so one last time let's check the neighbors so we'll check a first we've got he's local one plus one isn't less than zero so he doesn't get to update a we'll try C one plus one well that is less than three so II get Stu update C so the first thing will happen is C gets a new owner e the local a of one plus the distance one is two so we change this three to or two and we'll modify the global goal with the heuristic which becomes the local plus the distance to the end in this case it's a for the next neighbor let's assume is D so we take E's local one plus five well that's six six isn't less than four so e doesn't get to update D which leaves only one neighbor left and we've got these local one plus the distance to F 1 is 2 2 is less than 3 so we get to update F so we'll change the owner and we can change the local which is E's local plus the distance this just becomes a 2 and we can change the global which is 2 plus the distance to the target so in this case the global becomes 3 and we're now done with a and that means our nodes to test list is empty so now what well if we take our end node we can look at who owns it in this case F owns it so let's draw a path backwards to F well if we look at who owns f e owns it so let's go backwards towards E and if we look at who owns e well a owns it so we'll go all the way back and we're back at our starting point and this path will be the shortest path and we can see that it is because a to e is 1 e to F is 1 and F to D is 1 so the total number of steps is 3 all of the other paths have more steps and that ladies and gentlemen is the a-star algorithm let's now code it up as usual the first thing we'll need to do is set up the console game engine by creating a derived class and I've done that and I've set the console to be 160 by 160 and each character is to be 6 by 6 pixels as we've just seen the whole algorithm revolves around the concept of nodes so I'm going to add in a node structure and the node contains is the node an obstacle or not now we didn't cover that in the walkthrough but it's actually a very simple addition to the algorithm we'll come back to it has the node been visited we saw that with the ticks and whether it goes in the list it's global goal score it's local golf score which are floats its position in space now we're going to need that to calculate the heuristic amongst other things we have a vector of pointers to other node structures to represent the neighbors and we have a final pointer to another node which is its parent now because each node does have its own X&Y coordinate the nodes can exist anywhere in space but I'm going to arrange them in a grid because it's convenient to draw and it's convenient to demonstrate however there is no real concept of north east west and south in this arrangement now for the demonstration program we're going to need some nodes so I'm creating a pointer which will hold an array of nodes and I'm going to have 16 by 16 of them in a grid but don't forget the facts in a grid is not important and in the unused accrete function I want to define my nodes so the first thing I do is allocate the memory and then I iterate through each of the nodes and initialize it so I set the X&Y coordinate and I set none of them to obstacles because right now I just want them all to be passable nodes none of them have any parents and none of them have been visited and this is where things may get a bit confusing because the node structure contains information that will change during the path planning routines but also information about its state in this demonstration combining two different descriptions into a single structure is not good programming practice however to keep this demo simple I think you'll be okay now we've got some nodes it's time to draw them and we'll do that in the on user update function the first thing I'll add is the node size and the node boarder this is just to make it aesthetically pleasing and the node size is the pitch in between individual nodes and the node boarder is how many pixels not to draw around the edge so it makes them look like the floating islands as usual first thing we want to do is clear the screen so I'm going to draw a space to all locations and then using a simple nested for loop I'm going to draw a rectangle to represent the note and it looks a little bit long-winded but that's only because I'm calculating the coordinates all in one go so I'm taking the top left coordinates and the bottom right coordinates of a node including all of its node border and everything else and I'm just drawing it as a blue rectangle so let's have a look see see if that looks okay excellent some of the nodes I want to specify as being obstacles are not so I'm going to use the mouse to get this information so to get the nodes coordinates I'm taking the mouse position which is in pixel space across the console and I'm using an integer division to give me a nicely rounded number of which node has been selected the integer division will get rid of the remainder for me and I can check if the left mouse button has been clicked by a looking at the released flag because this is only set for one frame and if it has been clicked I then check to see if my coordinates are valid are they within the dimensions of my node array so I live between 0 and 16 and if they are I want to set the obstacle flag of the node to be the inverse of what it already is so it toggles on and off and I'm accessing the know this is I was convenient to put it into a grid because I can take the Y coordinates and the x coordinate to access the node I don't need to check all of my nodes individually to see if they've been hit and we can see here that I just take the obstacle flag and I set it to not what ever it was before so this will toggle it on and off so now we've got two states of the node we should draw both States and I'm going to do that by modifying my draw a rectangle function into something the same again it's got the same coordinate system so it looks a little bit complicated but I'm choosing the color based on whether the obstacle flag has been set so if it's an obstacle I'm drawing it as white and if it isn't I'm drawing it as blue I'm using a half shaded pixel here because it gives me a few more shades if you not sure what that really means check out the webcam at the command line video let's take a look now so we can see everything has been shaded and as I click on individual squares they get set to gray now at the moment my nodes are just floating in space we can't path through them because they're not connected to each other so we'll establish a routine now to set north south east and west connections between the nodes they could have arranged them in a grid this is quite easy to do well though isn't it clear and all I'm going to need are two for loops which are going to iterate through all of the cells for example let's say I wanted to add a connection between the current node and its northern neighbor I can find the current nodes address by Y times the width of my node array plus X we've seen that in many previous videos and I can get the vector of neighbors for that node and pushed to the back of it the address of the node above it and I use the same coordinate indexing strategy to work out what the address is in this case it's just the y coordinate minus one ie the node above me but I need to be careful because if I'm on the top row I'll be attempting to connect two nodes that don't exist so I want to check for that and therefore it's a similar process for the southern connections I don't want to check beyond the bottom row of the array similarly we can do things in the x-axis so we're linking up the West and Eastern neighbors let's now update our drawing routines to draw the connections and we're going to draw them behind so I'll draw them before the nodes and this is because our connections are going to be drawn from the center of each node location to the center of each neighboring location so I go through each node individually and then I also scroll through its vector of neighbors let's take a look very nice we can see the neighbors are connected to their immediate north west south and east except for around the boundary we'll need to record us start and end locations so I'm going to add two more variables which are just pointers to the node structure node start a node end and in the on user create function I'm going to give these a default value and I've just chosen something along the half way down and a little bit in from the edges and to identify these as being different from the other nodes I'm going to color them red and green which will do by modifying our draw node routine so now it draws the node as normal and in the special cases of it being a starter and end node it just simply over writes whatever already there with a green and a red color now at this point I'm also going to add some more drawing because I'm going to get it to draw whether the node has been visited or not and we'll use this to evaluate the performance of the algorithm so if a node has its visited flag set when it's drawn I'm going to draw it in blue but I'm going to use this solid pixel type as opposed to the half type before and this gives us that nice shading between the two drawing the path if you remember involves taking the end location and drawing it backwards following the parents so if the end location is not null I'm going to create a temporary pointer to the node and I'm going to use this pointer to follow the parent pointers back through the chain so if the node does have a parent I can effectively find the parents parent and so forth so I follow it on parent to parent to parent to parent and in a similar way that we drew the connections between the node centers I'm going to use exactly the same code except I'm going to draw a yellow line and this time it's between the current nodes location and the parents nodes location and this loop will repeatedly go through all of the parents the end all the way to the starts because the start location doesn't have a parent it is no pointer and that's when this routine will end so what to check everything still working we can see the end I can draw blocks but now I want to reposition the start and end points and I want to do that with these shift and control keys so we'll modify our mouse released function under normal circumstances this is currently toggling on or off whether the node is an obstacle or not but if we hold the shift key that's when we'll position the start node and if we hold the control key that's when we'll position the end node otherwise we'll just toggle an obstacle just see if that works so I'm holding down shift and I'm clicking and we can see we're moving the start node around and hold down ctrl and if I'm not holding anything down I'm drawing blocks now the only problem with the a-star algorithm is that can't really construct it in stages like this and then demonstrate how it works I've really just got to put all of the code in at once so it's in my class I'm going to add another function solve a star and this will update the graph and update all of the parents and the global goals and local goals accordingly but before we can do any solving the first thing we need to do is reset all of the nodes to a known state and if you remember through the manual walkthrough before we set our global goals and local goals to infinity and we set all the visited flags to false and none of the nodes have any parents and here again is the convenience of representing the nodes as a grid we can access them quite easily we don't need to scroll through all of the nodes in the list now in the manual version demonstrated we already had the distances between the nodes they were they're drawn on the screen we don't know them in this case so we're going to use Pythagoras's theorem to work them out and we're going to do this a lot so I'm going to create a lambda function to do just that that takes two nodes and gives me the distance between their center points we also need to calculate a heuristic and for bog-standard a star the heuristic is simply just the distance again so even though I've created another lambda function to do this right now all it does is return the distance I'm doing this so we can play around with the heuristic and see what effects it has on the algorithm once all our nodes are established to set up the start node so I'm creating a temporary variable called node current and this is the node that we're currently exploring the one that we're working with so it starts off at the beginning and as before we set the local goal to zero and we set the global goal to be the heuristic which in this case is just really the distance between the start and end locations and this is the distance as the crow flies it's this that biases the algorithm towards success we then need to maintain a list of nodes that have not been tested or nodes that are to be tested so I'm just going to use a standard list of node pointers and to this list I'm going to add the starting node and now we start implementing the algorithm I'm going to sit in a while loop making sure that I've got nodes to test so as soon as my list is empty I can assume the algorithm has completed and then the first step of the algorithm is to sort this list into ascending order of global goals well to begin with the list only has one element in so the sort is rather redundant but I've used the list sort he can't use the standard sort here because it doesn't work on lists that's because lists don't have any random access iterators which is unlike a vector because you can access a vector is any location with a list you have to scroll through the list in order to get to the next item the sort function can take a lambda function to implement the sort so I'm going to be sorting it based on the global goal this then implies that the element at the start of the list has the lowest global goal so it's probably the best solution to take however our list may also contain nodes we've already visited and we don't want to operate on them again so if the front of the list contains a node that we've already visited one we've already processed then just get rid of it and move on to the next one but it's always risky removing things from lists because we may end up with an empty list and if we do we want the algorithm to terminate so now we know the node at the front of the list is potentially the best candidate to finding the shortest path let's set our current node to the front of the list and set it to visited because we're going to explore it now we now want to iterate through each of the nodes neighbors I'm going to use the auto keyword in a for loop to do this conveniently because the vector here contains all of the connections to our neighbors now if we've not visited the neighbor we want to add it to our list but there's also one of the condition and this is slightly different to the manual example in that if our node neighbor is an obstacle we also don't want to add it to the list and so simply put if the neighbor has been visited we don't care we don't want to add it to our testing list because we've already done it and if the neighbor who is an obstacle there's still no point in processing it because we can't path through it after updating the neighbor list the next step is to calculate our local goals and we want to check if the current local goal plus the distance to the neighbor is less than the neighbors local goal and here we're using the distance lambda function again and if it is then we update our neighbor so here I'm setting the neighbors parents to the current node I'm setting the local goal to the possibly local goal variable which was the current nodes local plus the distance and then I set the neighbors heuristic which calls our heuristic lambda function added to the local goal so this is exactly what we were working out by hand before and so this chunk of code implements our a-star algorithm it's not very much for the two crucial parts are whether we're checking if our neighbor is an obstacle and updating the heuristic now when we work through it manually you might think that the global goal doesn't contribute very much we didn't seem to do very much with it other than setting but it's really important because it's based on the heuristic overall how well are we doing on that path and that that value is used to sort the list we know that we're trying the best possible paths first so it's trying to optimize the speed of finding the shortest path from start to finish there's one final thing to do and that's is if we make any changes to the scene we want to run our solve a star function let's have a play with it so if I draw a block in straightaway we can see it's found a path and if I enter up to that path it keeps trying to find the shortest path and this is guaranteed to be the shortest path because what we can see is all of the nodes have highlighted to blue and if I make some rather aggressive blockages so if I block off half of the array we can see it's never bothered to test those because it can't however this still seems a little redundant in fact if we're not bothered about finding the absolute shortest path and just want to find a path which is going to get there and is going to be reasonably short we can add another term to the while loop and that is to simply stop searching when we've reached the end it might not be the shortest path but it is a valid path and it might be good enough and so now we can see it doesn't search the whole space it stops once it's satisfied its objective and so it's searching much fewer cells instead of just connecting to our north south east and west neighbors why not connect to all of our immediate neighbors so we'll connect diagonally but we have to make the same checks as we did before to make sure we don't go out of bounds let's take a look so now we can see all of the connections and this gives us diagonal passing opportunities and we see we get a nice smooth transition as it exploits being able to travel via diagonals of course it can go through our path now because our connections aren't obstacles it's only the nodes that are obstacles I think it's quite interesting to see how it develops and explores the space and so after much playing around we can see the path is also not afraid to go back on itself if there's a solution this algorithm will find it another thing to play with is to change the heuristic so if we set our heuristic to one ie we don't bias the algorithm in any particular direction towards the goal we can see that it explores a lot more space before it comes to a solution in fact you get a wavefront I put this in the top corner and put this one in the bottom corner it has to explore the whole array and this makes a star behave in exactly the same way as Cheeks does out with them or Dijkstra's algorithm depending on what you throw but having the heuristic certainly makes it a little more optimal so we'll leave it in for now and so there you have it a rather rushed look at the a-star algorithm being used for path planning there are other path planning algorithms out there but a star is a pretty good one to start with and most of the path buying algorithms are simply variations of a theme as always the codes going to be available on get if you've enjoyed this video give me a big thumbs up have a think about subscribing I'll see you next time it's okay

Channel: javidx9

Views: 101,848

Rating: 4.959322 out of 5

Keywords: onelonecoder, one lone coder, a-star, a*, a star, c++, programming, coding, tutorial, learning, command prompt, ascii, simple, path planning, path finding, algorithm

Id: icZj67PTFhc

Length: 31min 17sec (1877 seconds)

Published: Mon Oct 09 2017