Is the A* path finding algorithm guaranteed to find the shortest path 100% or the time, if implemented correctly?
int Graph::FindPath(Node *start, Node *finish, list< vec2f > &path)
{
list<NodeRecord*> open;
list<NodeRecord*> closed;
list<NodeRecord*>::iterator openIt;
list<NodeRecord*>::iterator closedIt;
// add the starting node to the open list
open.push_back( new NodeRecord(start, NULL, 0.0f, 0.0f + start->pos.DistanceSq(finish->pos) ) );
// NodeRecord(Node *node, Node *from, float cost, float totalCost)
while(!open.empty())
{
// find the node record with the lowest cost
NodeRecord *currentRecord = open.front();
openIt = ++open.begin();
while(openIt != open.end())
{
if((*openIt)->total < currentRecord->total)
currentRecord = (*openIt);
openIt++;
}
// get a pointer to the current node
Node *currentNode = currentRecord->node;
// if the current node is the finish point
if(currentNode == finish)
{
// add the finish node
path.push_front(currentNode->pos);
// add all the from nodes
Node *from = currentRecord->from;
while(!closed.empty())
{
// if this node record is where the path came from,
if(closed.back()->node == from) //&& closed.back()->from != NULL
{
// add it to the path
path.push_front( from->pos );
// get the next 'from' node
from = closed.back()->from;
}
// delete the node record
delete closed.back();
closed.pop_back();
}
while(! open.empty() )
{
delete open.back();
open.pop_back();
}
// a path was found
return 0;
}
// cycle through all neighbours of the current node
bool isClosed, isOpen;
for(int i = 0; i < (int)currentNode->neighbours.size(); i++)
{
// check if neigbour is on the closed list
isClosed = false;
closedIt = closed.begin();
while(closedIt != closed.end())
{
if(currentNode->neighbours[i] == (*closedIt)->node)
{
isClosed = true;
break;
}
closedIt++;
}
// skip if already on the closed list
if(isClosed == true)
continue;
float cost = currentRecord->cost + currentNode->distance[i];
float totalCost = cost + currentNode->neighbours[i]->pos.DistanceSq(finish->pos);
// check if this neighbour is already on the open list
isOpen = false;
openIt = open.begin();
while(openIt != open.end())
{
if(currentNode->neighbours[i] == (*openIt)->node)
{
// node was found on the open list
if(totalCost < (*openIt)->total)
{
// node on open list was updated
(*openIt)->cost = cost;
(*openIt)->total = totalCost;
(*openIt)->from = currentNode;
}
isOpen = true;
break;
}
openIt++;
}
// skip if already on the open list
if(isOpen == true)
continue;
// add to the open list
open.push_back( new NodeRecord(currentNode->neighbours[i], currentNode, cost, totalCost) );
}
// move the current node to the closed list after it has been evaluated
closed.push_back( currentRecord );
open.remove( currentRecord );
}
// free any nodes left on the closed list
while(! closed.empty() )
{
delete closed.back();
closed.pop_back();
}
// no path was found
return -1;
}
Yes (but I haven't looked deeply at your implementation).
The thing that most people miss is that the heuristic algorithm MUST underestimate the cost of traversal to the final solution (this is called "admissible"). It is also good (but not absolutely required) for the heuristic to monotonically approach the solution (this is called "consistent")
Anyway, at my glance at your code, you probably should use std::set for your closed list and std::deque for your open one so that your searches and insertion in these two lists aren't O(n). You also shouldn't make new NodeRecords, since it gives you a level of indirection with no benefit (and your algorithm will leak memory if an exception is thrown).
According to Wikipedia, A* uses heuristics for faster finding shortest path, but actually it is a modification of Dijkstra's shortest path algorithm, and if the heuristics is not good enough, A* does practically the same as Dijkstra.
So yes, it is guaranteed that A* finds the shortest path.
Interestingly, while admissible heuristics provide the optimal solution 100% of the time, they can be slow in certain situations. If there are several paths which are roughly the same total distance, an inadmissible heuristic will provide faster "decision-making" between the relatively equivalent paths. Note that you must use a closed list (which you did) for this to work.
In fact, Pearl in his book "Heuristics" proves that if your heuristic overestimates by a small amount, the solution provided will only be longer than the optimal by that same amount (at most)!
For certain fast/real-time applications, this can be a real help to boost speed, at a small cost to the solution quality.
Related
I'm implementing a Dijkstra's algorithm. The code I've written works in terms of finding the shortest path. However, the problem is when I try to save the path.
Just a heads up: I'm using Stanford's library , so some things might be different (e.g. Set has contains method) but functionality is mostly identical to standard libraries.
Vector<Loc> shortestPath(Loc start, Loc end, Grid<double>& world,
double costFn(Loc from, Loc to, Grid<double>& world)) {
Vector<Loc> vec;
Map<Loc, double> distanceMap;
TrailblazerPQueue<Loc> queue; // priority queue with decrease key method
Set<Loc> greenNodes;
Set<Loc> yellowNodes;
colorCell(world, start, YELLOW);
queue.enqueue(start, 0);
while(!queue.isEmpty()) {
Loc currNode = queue.dequeueMin(); // here is the problem
colorCell(world, currNode, GREEN);
greenNodes.add(currNode);
if (currNode == end) {
break;
}
Vector<Loc> neighbourNodes = getNeighbourNodes(currNode, world);
foreach (Loc node in neighbourNodes) {
if (greenNodes.contains(node)) {
continue;
}
if (!yellowNodes.contains(node)) {
colorCell(world, node, YELLOW);
yellowNodes.add(node);
double distance = distanceMap.get(currNode) + costFn(currNode, node, world);
distanceMap.put(node, distance);
node.parent = &currNode; // here is the problem
queue.enqueue(node, distance);
} else {
double distance = distanceMap.get(node);
double currDistance = distanceMap.get(currNode) + costFn(currNode, node, world);
if (distance > currDistance) {
distanceMap.put(node, currDistance);
node.parent = &currNode;
queue.decreaseKey(node, distance);
}
}
}
}
return vec;
}
Here Grid is just a 2D array and costFn just calculates the cost from one location to the next. The way I try to save the path is assign the location of the previous node to current one. Here's the problem: when I traverse the neighbouring nodes of currNode and set the parent of each one as the &currNode, after setting currNode to the new element from the queue, it now refers to other node, so the node's parent now refers to itself, infinitely.
I've tried many different ways of overcoming this problem, creating new nodes in heap and other stuff, but they didn't really work. I've also tried to create other struct and use that to save locations, but I feel like there should be a fairly simple way to resolve this issue.
I hope my explanation isn't too confusing, I've added markers as comments to which lines cause the problem. Also, I say "node", but the struct is called Loc:
struct Loc {
int row;
int col;
Loc* parent;
};
I'm currently working on a problem of finding cycles consisted of selected nodes in a directed graph.
For the instance described here:
there's a cycle within node 1, 2, 3, and no cycle is found within 1, 2, 4.
I've tried to implement the algorithm myself with the following operation:
Start with the first node within the selected nodes.
Mark current node as "visited".
Check if adjacent nodes are within selected nodes.
Recursive call if the node hasn't been visited, return true if it's visited.
At the end of the function: return false.
My implementation is as following(the function is called for each selected nodes, and the array storing visited nodes is initialized every time)
bool hasLoop(const int startNode, const bool directions[][MAX_DOT_NUM], const int nodesLen, bool nodesVisited[], const int selectedNodes[], const int selectedNum){
nodesVisited[startNode] = true;
for(int i = 0; i < nodesLen; i++){ //loop through all nodes
if(withinSelected(i, selectedNodes, selectedNum) == false) continue; //check loop only for selected nodes
if(directions[startNode][i] == 1){ //connected and is within selected nodes
if(nodesVisited[i] == true){
return true;
}else{
if(hasLoop(i, directions, nodesLen, nodesVisited, selectedNodes, selectedNum)){
return true;
}
}
}
}
return false;
}
However, this implementation doesn't work for all testing data from the online judge I'm using.
I found that my algorithm is different from Depth First Search, which uses White, Grey, Black arrays to store nodes that are not visited, being visited, or not needed to be visited, I wonder if that's the reason causing problems.
Hopefully, I can find the bug causing this implementation not to work for all circumstances with your help!
Thank you so much for reading this!
Edited: it's a directed graph! sorry for that.
Edited: Thanks for your help so much! I revised my implementation to have the function return true only when finding a node pointing to the node where the function started.
Here's the final implementation accepted by the online judge I use:
bool hasLoop(const int currentNode, const bool directions[][MAX_DOT_NUM], const int nodesLen, bool nodesVisited[], const int selectedNodes[], const int selectedNum, const int startNode){
// cout << currentNode << " -> ";
nodesVisited[currentNode] = true;
for(int i = 0; i < nodesLen; i++){
if(withinSelected(i, selectedNodes, selectedNum) == false) continue;
if(directions[currentNode][i] == 1){ //connected and is within selected nodes
if(nodesVisited[i] == true){
if(i == startNode) return true;
}else{
if(hasLoop(i, directions, nodesLen, nodesVisited, selectedNodes, selectedNum, startNode)){
return true;
}
}
}
}
return false;
}
Your implementation is a DFS, but will fail for "side nodes" that do not create a cycle:
Consider the graph with 3 nodes (A,B,C):
A
/ \
/ \
V V
B <---- C
Your algorithm will tell that the graph has a cycle, while in fact - it does not!
You can solve it by finding Strongly Connected Components, and seeing if there are non trivial (size>1) components.
Another solution would be to use Topological Sort - which returns an error if and only if the graph has a cycle.
In both solutions, you apply the algorithm only on the subgraph containing the "selected nodes". Both solutions are O(|V|+|E|) time, and O(|V|) space.
I get a segmentation fault in the call to
auto n1=std::make_shared<Node>(n,n->x+i,n->y+j);
after a few recursive calls. Strange thing is that it's always at the same point in time. Can anyone spot the problem?
This is an implementation for a dynamic programming problem and here I'm accumulating the costs of a path. I have simplified the cost function but in this example the problem still occurs.
void HorizonLineDetector::dp(std::shared_ptr<Node> n)
{
n->cost= 1 + n->prev->cost;
//Check if we reached the last column(done!)
if (n->x==current_edges.cols-1)
{
//Save the info in the last node if it's the cheapest path
if (last_node->cost > n->cost)
{
last_node->cost=n->cost;
last_node->prev=n;
}
}
else
{
//Check for neighboring pixels to see if they are edges, launch dp with all the ones that are
for (int i=0;i<2;i++)
{
for (int j=-1;j<2;j++)
{
if (i==0 && j==0) continue;
if (n->x+i >= current_edges.cols || n->x+i < 0 ||
n->y+j >= current_edges.rows || n->y+j < 0) continue;
if (current_edges.at<char>(n->y+j,n->x+i)!=0)
{
auto n1=std::make_shared<Node>(n,n->x+i,n->y+j);
//n->next.push_back(n1);
nlist.push_back(n1);
dp(n1);
}
}
}
}
}
class Node
{
public:
Node(){}
Node(std::shared_ptr<Node> p,int x_,int y_){prev=p;x=x_;y=y_;lost=0;}
Node(Node &n1){x=n1.x;y=n1.y;cost=n1.cost;lost=n1.lost;prev=n1.prev;}//next=n1.next;}
std::shared_ptr<Node> prev; //Previous and next nodes
int cost; //Total cost until now
int lost; //Number of steps taken without a clear path
int x,y;
Node& operator=(const Node &n1){x=n1.x;y=n1.y;cost=n1.cost;lost=n1.lost;prev=n1.prev;}//next=n1.next;}
Node& operator=(Node &&n1){x=n1.x;y=n1.y;cost=n1.cost;lost=n1.lost;prev=n1.prev;n1.prev=nullptr;}//next=n1.next;n1.next.clear();}
};
Your code looks like a pathological path search, in that it checks almost every path and doesn't keep track of paths it has already checked you can get to more than one way.
This will build recursive depth equal to the length of the longest path, and then the next longest path, and ... down to the shortest one. Ie, something like O(# of pixels) depth.
This is bad. And, as call stack depth is limited, will crash you.
The easy solution is to modify dp into dp_internal, and have dp_internal return a vector of nodes to process next. Then write dp, which calls dp_internal and repeats on its return value.
std::vector<std::shared_ptr<Node>>
HorizonLineDetector::dp_internal(std::shared_ptr<Node> n)
{
std::vector<std::shared_ptr<Node>> retval;
...
if (current_edges.at<char>(n->y+j,n->x+i)!=0)
{
auto n1=std::make_shared<Node>(n,n->x+i,n->y+j);
//n->next.push_back(n1);
nlist.push_back(n1);
retval.push_back(n1);
}
...
return retval;
}
then dp becomes:
void HorizonLineDetector::dp(std::shared_ptr<Node> n)
{
std::vector<std::shared_ptr<Node>> nodes={n};
while (!nodes.empty()) {
auto node = nodes.back();
nodes.pop_back();
auto new_nodes = dp_internal(node);
nodes.insert(nodes.end(), new_nodes.begin(), new_nodes.end());
}
}
but (A) this will probably just crash when the number of queued-up nodes gets ridiculously large, and (B) this just patches over the recursion-causes-crash, doesn't make your algorithm suck less.
Use A*.
This involves keeping track of which nodes you have visited and what nodes to process next with their current path cost.
You then use heuristics to figure out which of the ones to process next you should check first. If you are on a grid of some sort, the heuristic is to use the shortest possible distance if nothing was in the way.
Add the cost to get to the node-to-process, plus the heuristic distance from that node to the destination. Find the node-to-process that has the least total. Process that one: you mark it as visited, and add all of its adjacent nodes to the list of nodes to process.
Never add a node to the list of nodes to process that you have already visited (as that is redundant work).
Once you have a solution, prune the list of nodes to process against any node whose current path value is greater than or equal to your solution. If you know your heuristic is a strong one (that it is impossible to get to the destination faster), you can even prune based off of the total of heuristic and current cost. Similarly, don't add to the list of nodes to process if it would be pruned by this paragraph.
The result is that your algorithm searches in a relatively strait line towards the target, and then expands outwards trying to find a way around any barriers. If there is a relatively direct route, it is used and the rest of the universe isn't even touched.
There are many optimizations on A* you can do, and even alternative solutions that don't rely on heuristics. But start with A*.
So, I made the following code for DFS:
void dfs (graph * mygraph, int foo, bool arr[]) // here, foo is the source vertex
{
if (arr[foo] == true)
return;
else
{
cout<<foo<<"\t";
arr[foo] = true;
auto it = mygraph->edges[foo].begin();
while (it != mygraph->edges[foo].end())
{
int k = *it;
if (arr[k] == false)
{
//cout<<k<<"\n";
dfs(mygraph,k,arr);
//cout<<k<<"\t";
}
it++;
}
}
//cout<<"\n";
}
Now, I read up that in an undirected graph, if while DFS, it returns to the same vertex again, there is a cycle. Therefore, what I did was this,
bool checkcycle( graph * mygraph, int foo, bool arr[] )
{
bool result = false;
if (arr[foo] == true)
{
result = true;
}
else
{
arr[foo] = true;
auto it = mygraph->edges[foo].begin();
while (it != mygraph->edges[foo].end())
{
int k = *it;
result = checkcycle(mygraph,k,arr);
it++;
}
}
return result;
}
But, my checkcycle function returns true even if their is no cycle. Why is that? Is there something wrong with my function? There is no execution problem, otherwise I would have debugged, but their seems to be something wrong in my logic.
Notice that your function doesn't quite do what you think it does. Let me try to step through what's happening here. Assume the following relationships: (1,2), (1,3), (2,3). I'm not assuming reflexibility (that is, (1,2) does not imply (2,1)). Relationships are directed.
Start with node 1. Flag it as visited
Iterate its children (2 and 3)
When in node 2, recursively call check cycle. At this point 2 is also flagged as visited.
The recursive call now visits 3 (DEPTH search). 3 is also flagged as visited
Call for step 4 dies returning false
Call for step 3 dies returning false
We're back at step 2. Now we'll iterate node 3, which has already been flagged in step 4. It just returns true.
You need a stack of visited nodes or you ONLY search for the original node. The stack will detect sub-cycles as well (cycles that do not include the original node), but it also takes more memory.
Edit: the stack of nodes is not just a bunch of true/false values, but instead a stack of node numbers. A node has been visited in the current stack trace if it's present in the stack.
However, there's a more memory-friendly way: set arr[foo] = false; as the calls die. Something like this:
bool checkcycle( graph * mygraph, int foo, bool arr[], int previousFoo=-1 )
{
bool result = false;
if (arr[foo] == true)
{
result = true;
}
else
{
arr[foo] = true;
auto it = mygraph->edges[foo].begin();
while (it != mygraph->edges[foo].end())
{
int k = *it;
// This should prevent going back to the previous node
if (k != previousFoo) {
result = checkcycle(mygraph,k,arr, foo);
}
it++;
}
// Add this
arr[foo] = false;
}
return result;
}
I think it should be enough.
Edit: should now support undirected graphs.
Node: this code is not tested
Edit: for more elaborate solutions see Strongly Connected Components
Edit: this answer is market as accepted although the concrete solution was given in the comments. Read the comments for details.
are all of the bools in arr[] set to false before checkcycle begins?
are you sure your iterator for the nodes isn't doubling back on edges it has already traversed (and thus seeing the starting node multiple times regardless of cycles)?
I need to use (not implement) an array based version of Dijkstras algo .The task is that given a set of line segments(obstacles) and start/end points I have to find and draw the shortest path from start/end point.I have done the calculating part etc..but dont know how to use dijkstras with my code.My code is as follows
class Point
{
public:
int x;
int y;
Point()
{
}
void CopyPoint(Point p)
{
this->x=p.x;
this->y=p.y;
}
};
class NeighbourInfo
{
public:
Point x;
Point y;
double distance;
NeighbourInfo()
{
distance=0.0;
}
};
class LineSegment
{
public:
Point Point1;
Point Point2;
NeighbourInfo neighbours[100];
LineSegment()
{
}
void main()//in this I use my classes and some code to fill out the datastructure
{
int NoOfSegments=i;
for(int j=0;j<NoOfSegments;j++)
{
for(int k=0;k<NoOfSegments;k++)
{
if( SimpleIntersect(segments[j],segments[k]) )
{
segments[j].neighbours[k].distance=INFINITY;
segments[j].neighbours[k].x.CopyPoint(segments[k].Point1);
segments[j].neighbours[k].y.CopyPoint(segments[k].Point2);
cout<<"Intersect"<<endl;
cout<<segments[j].neighbours[k].distance<<endl;
}
else
{
segments[j].neighbours[k].distance=
EuclidianDistance(segments[j].Point1.x,segments[j].Point1.y,segments[k].Point2.x,segments[k ].Point2.y);
segments[j].neighbours[k].x.CopyPoint(segments[k].Point1);
segments[j].neighbours[k].y.CopyPoint(segments[k].Point2);
}
}
}
}
Now I have the distances from each segmnets to all other segments, amd using this data(in neighbourinfo) I want to use array based Dijkstras(restriction ) to trace out the shortest path from start/end points.There is more code , but have shortened the problem for the ease of the reader
Please Help!!Thanks and plz no .net lib/code as I am using core C++ only..Thanks in advance
But I need the array based version(strictly..) I am not suppose to use any other implemntation.
Dijkstras
This is how Dijkstra's works:
Its not a simple algorithm. So you will have to map this algorithm to your own code.
But good luck.
List<Nodes> found; // All processed nodes;
List<Nodes> front; // All nodes that have been reached (but not processed)
// This list is sorted by the cost of getting to this node.
List<Nodes> remaining; // All nodes that have not been explored.
remaining.remove(startNode);
front.add(startNode);
startNode.setCost(0); // Cost nothing to get to start.
while(!front.empty())
{
Node current = front.getFirstNode();
front.remove(current);
found.add(current);
if (current == endNode)
{ return current.cost(); // we found the end
}
List<Edge> edges = current.getEdges();
for(loop = edges.begin(); loop != edges.end(); ++loop)
{
Node dst = edge.getDst();
if (found.find(dst) != found.end())
{ continue; // If we have already processed this node ignore it.
}
// The cost to get here. Is the cost to get to the last node.
// Plus the cost to traverse the edge.
int cost = current.cost() + loop.cost();
Node f = front.find(dst);
if (f != front.end())
{
f.setCost(std::min(f.cost(), cost));
continue; // If the node is on the front line just update the cost
// Then continue with the next node.
}
// Its a new node.
// remove it from the remaining and add it to the front (setting the cost).
remaining.remove(dst);
front.add(dst);
dst.setCost(cost);
}
}