I am writing a small section of a program in which i have to write up a pathfinding algorithm. The function takes in what will be known as 'routes' that each define a start and end point in 2D space. The algorithm is required to find the shortest and most efficient (to a degree) path (from the origin) to take through these routes, minimising the total distance traveled overall.
I did a bit of research and started down a path that i thought might work. So far i have converted the routes into a directed graph which is all linked up as if it were an idealised road map. I then attempted to perform an A* search on this graph. The heuristic i used calculates the total distance of the 'routes' left to travel and the distance from start (G) value i used was just the cumulative distance traveled to get to the current point. This works for some input but others return no path at all and i cant seem to figure out why.
Is is possible that my heuristic is wrong and this is causing a miscalculation somewhere or is it more likely that the A* procedure itself is wrong? or am i just on completely the wrong track here?
I'll put the getPath function below (written in Java) just in case that helps.
Thanks in advance.
public ArrayList<Vector2> getPath()
{
PriorityQueue<SearchNode> openList = new PriorityQueue<SearchNode>(10, new SearchNodeComparator());
ArrayList<SearchNode> closedList = new ArrayList<SearchNode>();
map.startJobs();
searchDepth = 0;
SearchNode start = searchableGraph.getNode(new Vector2(0, 0));
int goalsLeft = map.getJobCount();
start.setDistanceTraveled(0);
openList.add(start);
while (openList.size() > 0)
{
SearchNode current = openList.peek();
searchDepth++;
if (map.isJobEndPoint(current.getValue()))
{
map.completeJob(current.getValue());
goalsLeft--;
}
if (reachedGoalState(current, searchableGraph.getNodes().size()))
{
return getFinalPath(current);
}
else
{
ArrayList<SearchNode> neighbours = getNeighbours(current);
for (int i = 0; i < neighbours.size(); i++)
{
SearchNode node = neighbours.get(i);
System.out.print("Inspecting node" + node.getValue().toString());
float distanceTraveled = current.getDistanceTraveled() + getDistance(current.getValue(), node.getValue());
float heuristic = heuristic(node);
if (!openList.contains(node) && !closedList.contains(node))
{
node.setDistanceTraveled(distanceTraveled);
node.setDistanceToGoal(distanceTraveled + heuristic);
node.setParent(current);
openList.add(node);
}
else if(openList.contains(node))
{
if (node.getDistanceTraveled() <= distanceTraveled)
{
node.setDistanceToGoal(distanceTraveled + heuristic);
node.setParent(current);
}
}
}
openList.remove(current);
closedList.add(current);
}
}
return new ArrayList<Vector2>();
}
The heuristic i used calculates the total distance of the 'routes' left to travel
The heuristic used by A* must be admissible; that is, it must never overestimate the distance to the end.
If I understand your description correctly, your heuristic is not admissible, so is not guaranteed to work.
Related
I was tasked to perform the Dijkstra Algorithm on big graphs (25 million nodes). These are represented as a 2D array: -each node as a double[] with latitude, longitude and offset (offset meaning index of the first outgoing edge of that node)
-each edge as a int[] with sourceNodeId,targetNodeId and weight of that edge
Below is the code, I used int[] as a tupel for the comparison in the priority queue.
The algorithm is working and gets the right results HOWEVER it is required to be finished in 15s but takes like 8min on my laptop. Is my algorithm fundamentally slow? Am I using the wrong data structures? Am I missing something? I tried my best optimizing as far as I saw fit.
Any help or any ideas would be greatly appreciated <3
public static int[] oneToAllArray(double[][]nodeList, int[][]edgeList,int sourceNodeId) {
int[] distance = new int[nodeList[0].length]; //the array that will be returned
//the priorityQueue will use arrays with the length 2, representing [index, weight] for each node and order them by their weight
PriorityQueue<int[]> prioQueue = new PriorityQueue<>((a, b) -> ((int[])a)[1] - ((int[])b)[1]);
int offset1; //used for determining the amount of outgoing edges
int offset2;
int newWeight; //declared here so we dont need to declare it a lot of times later (not sure if that makes a difference)
//currentSourceNode here means the node that will be looked at for OUTGOING edges
int[] currentSourceNode= {sourceNodeId,0};
prioQueue.add(currentSourceNode);
//at the start we only add the sourceNode, then we start the actual algorithm
while(!prioQueue.isEmpty()) {
if(prioQueue.size() % 55 == 2) {
System.out.println(prioQueue.size());
}
currentSourceNode=prioQueue.poll();
int sourceIndex = currentSourceNode[0];
if(sourceIndex == nodeList[0].length-1) {
offset1= (int) nodeList[2][sourceIndex];
offset2= edgeList[0].length;
} else {
offset1= (int) nodeList[2][sourceIndex];
offset2= (int) nodeList[2][sourceIndex+1];
}
//checking every outgoing edge for the currentNode
for(int i=offset1;i<offset2;i++) {
int targetIndex = edgeList[1][i];
//if the node hasnt been looked at yet, the weight is just the weight of this edge + distance to sourceNode
if(distance[targetIndex]==0&&targetIndex!=sourceNodeId) {
distance[targetIndex] = distance[sourceIndex] + edgeList[2][i];
int[]targetArray = {targetIndex, distance[targetIndex]};
prioQueue.add(targetArray);
} else if(prioQueue.stream().anyMatch(e -> e[0]==targetIndex)) {
//above else if checks if this index is already in the prioQueue
newWeight=distance[sourceIndex]+edgeList[2][i];
//if new weight is better, we have to update the distance + the prio queue
if(newWeight<distance[targetIndex]) {
distance[targetIndex]=newWeight;
int[] targetArray;
targetArray=prioQueue.stream().filter(e->e[0]==targetIndex).toList().get(0);
prioQueue.remove(targetArray);
targetArray[1]=newWeight;
prioQueue.add(targetArray);
}
}
}
}
return distance;
}
For each node that you process, you are doing a linear scan of the priority queue to see if something is already queued, and a second scan to find all the things that are queued if you have to update the distance. Instead, keep a separate multi-set of things that are in the queue.
This is not a proper Dijkstra's implementation.
One of the key elements of Dijkstra is that you mark nodes as "visited" when they have been evaluated and prevent looking at them again because you can't do any better. You are not doing that, so your algorithm is doing many many more computations than necessary. The only place where a priority queue or sort is required is to pick the next node to visit, from amongst the unvisited. You should re-read the algorithm, implement the "visitation tracking" and re-formulate.
I implemented Held-Karp in Java following Wikipedia and it gives the correct solution for total distance of a cycle, however I need it to give me the path (it doesn't end on the same vertex where is started). I can get path if I take out the edge with largest weight from the cycle, but there is a possibility that 2 different cycles have same total distance, but different maximum weight, therefore one of the cycles is wrong.
Here is my implementation:
//recursion is called with tspSet = [0, {set of all other vertices}]
private static TSPSet recursion (TSPSet tspSet) {
int end = tspSet.endVertex;
HashSet<Integer> set = tspSet.verticesBefore;
if (set.isEmpty()) {
TSPSet ret = new TSPSet(end, new HashSet<>());
ret.secondVertex = -1;
ret.totalDistance = matrix[end][0];
return ret;
}
int min = Integer.MAX_VALUE;
int minVertex = -1;
HashSet<Integer> copy;
for (int current: set) {
copy = new HashSet<>(set);
copy.remove(current);
TSPSet candidate = new TSPSet(current, copy);
int distance = matrix[end][current] + recursion(candidate).totalDistance;
if (distance < min) {
min = distance;
minVertex = current;
}
}
tspSet.secondVertex = minVertex;
tspSet.totalDistance = min;
return tspSet;
}
class TSPSet {
int endVertex;
int secondVertex;
int totalDistance;
HashSet<Integer> verticesBefore;
public TSPSet(int endVertex, HashSet<Integer> vertices) {
this.endVertex = endVertex;
this.secondVertex = -1;
this.verticesBefore = vertices;
}
}
You can slightly alter the dynamic programming state.
Let the path start in a node S. Let f(subset, end) be the optimal cost of the path that goes through all the vertices in the subset and ends in the end vertex (S and end must always be in the subset). A transition is just adding a new vertex V not the subset by using the end->V edge.
If you need a path that ends T, the answer is f(all vertices, T).
A side note: what you're doing now is not a dynamic programming. It's an exhaustive search as you do not memoize answers for subsets and end up checking all possibilities (which results in O(N! * Poly(N)) time complexity).
Problem with current approach
Consider this graph:
The shortest path visiting all vertices (exactly once each) is of length 3, but the shortest cycle is 1+100+200+300, which is 301 even if you remove the maximum weight edge.
In other words, it is not correct to construct the shortest path by deleting an edge from the shortest cycle.
Suggested approach
An alternative approach to convert your cycle algorithm into a path algorithm is to add a new node to the graph which has a zero cost edge to all of the other nodes.
Any path in the original graph corresponds to a cycle in this graph (the start and end points of the path are the nodes that the extra node connects to.
I am trying to build an app implementing A*, and I am having trouble working on the logic. The method here takes in 4 ints (startX/Y, goalX/Y) and then using the A* algorithm, it will build an ArrayList and return it, so the main method can take iterate through and display the path A* builds. But what I am getting is a jumpy path that eventually builds a very thick path to the goal node. Can anybody pinpoint where my mistake is.
Note: open and closed are priority queues and Tile implements comparable.
public ArrayList<Tile> findPath(int sX, int sY, int gX, int gY)
{
ArrayList<Tile> path = new ArrayList<Tile>();
open.offer(gameMap[sX][sY]);
Tile currentNode = gameMap[sX][sY];
Tile goalNode = gameMap[gX][gY];
int cX;
int cY;
while(open.size() > 0){
currentNode = open.poll();
closed.offer(currentNode);
path.add(currentNode);
cX = currentNode.getX();
cY = currentNode.getY();
if(currentNode == goalNode){
break;
}
if((cX > 0 && cX < gameMap.length - 1) && (cY > 0 && cY < gameMap.length -1)){
for(int i = -1; i < 2; i++){
for(int j = 1; j > -2; j--){
if(i == 0 && j == 0){}
else{
if((gameMap[cX + i][cX + j].type != 1) && !closed.contains(gameMap[cX + i][cX + j])){
if(!open.contains(gameMap[cX + i][cX + j])){
open.offer(gameMap[cX + i][cX + j]);
gameMap[cX + i][cX + j].parent = currentNode;
}
}
}
}
}
}
}
// while(currentNode != gameMap[sX][sY]){
// path.push(currentNode);
// currentNode = currentNode.parent;
// }
return path;
}
First off, I don't think your closed set needs to be a priority queue. It's just a set of nodes that have been looked at.
You seem to be missing the core part of how A* works, which is why I think this path finder is not working to well for you.
Here's the main idea:
Have a heuristic function that guesses how far away the destination is. Ideally, that function will be admissible, meaning that it will never overestimate the distance.
For tile grids, this can be done using manhattan distance (x difference + y difference) since that is the minimum distance, so it will always be admissible.
Whenever you take a tile out of your open list and add it to the closed set, you need to update the known value of how far away the neighboring tiles are (keeping the lowest known value). Since you have the known value for the tile you are putting in the closed set, you just add 1 to all the neighbors' known values.
By updating these values, the open list may shift order (which is why a priority queue is a good choice here). The heuristic value will probably remain the same, but the known value will get more refined.
Once you reach the destination, you will have a set of closed nodes that all have a known distance. You then backtrack from the destination, looking at each neighbor that is also in the closed set and choosing the one with the lowest known distance.
In terms of how to implement this, you may want to consider having your Tile class be wrapped in another class called SearchTile or something like that. It could look like this:
//You may not want to use public variables, depending on your needs
public class SearchTile implements Comparable<SearchTile> {
public final Tile tile;
//These need to be updated
public int knownDistance = 0;
public int heuristicDistance = 0;
public SearchTile(final Tile tile) {
this.tile = tile;
}
#Override
public int compareTo(final SearchTile other) {
if (knownDistance + heuristicDistance > other.knownDistance + other.heuristicDistance) {
return 1;
} else if (knownDistance + heuristicDistance < other.knownDistance + other.heuristicDistance) {
return -1;
} else {
return 0;
}
}
}
The cool thing about A* is that in the ideal case, it should go straight to the destination. In cases with walls, it will take the best guess and as long as the heuristic is admissible, it will come up with the optimal solution.
I've not completely entered in the details of your implementation, but it comes to my mind that the way in which you are inserting the nodes in OPEN might be a cause of trouble:
if(!open.contains(gameMap[cX + i][cX + j])){
open.offer(gameMap[cX + i][cX + j]);
gameMap[cX + i][cX + j].parent = currentNode;
}
Your goal here is to manage avoiding repeated elementes in your OPEN list, but it might happen that sometimes you have to replace the element because you have encountered a way in which you reach it with a better cost. In this case you need to remove the node already inserted in OPEN and reintroduce it with a lower cost (and thus with highest priority). If you do not allow this, you might be generating suboptimal paths as it seems to be your case.
Additionaly, some logic of the algorithm is missing. You should store the accumulated cost from the start, G, and the estimated cost to goal, H, for each node you create. The OPEN list is ordered according to the value of G+H, which I didn't notice in your code to be done this way. Anyway, I recommend you to take a look of some existing implementation of A* like one of the Hipster4j library to have more details on how this works.
Hope my answer helped!
I'm working on an A* algorithm. This is the code for the pathfinding method. For reference, this is the board I am working with: http://i.imgur.com/xaAzNSw.png?1 Each color tile represents a different heuristic value. For some unknown reason, it finds a path every single time, just not always the correct path. Here is the code for the pathfinding method. If anyone needs any clarifications, I'd be happy to provide them.
public List<Point> findPath(Point start, Point end) {
//declarations and instantiations
List<PathState> closedList = new ArrayList<PathState>(); //the nodes already considered
List<PathState> openList = new ArrayList<PathState>(); //nodes to be considered
openList.add(new PathState(start, end, tm)); //add starting point
PathState current = openList.get(0);
while(!current.isGoal()){
//sort open list to find the pathstate with the best hscore(sorts by hscore)
Collections.sort(openList);
current = openList.get(openList.size() - 1);
closedList.add(current);
openList.remove(current);
//get the valid children of current node
List<PathState> children = current.getChildren();;
if(!current.isGoal()){
for(int i = 0; i < children.size(); i++){
if(!closedList.contains(children.get(i))){
if(openList.contains(children.get(i))){
if(openList.get(openList.indexOf(children.get(i))).getgScore() > children.get(i).getgScore()){
//child is already on the open list, but this node has lower g value
//change g value and parent of node on open list
openList.get(openList.indexOf(children.get(i))).setG(children.get(i).getgScore());
openList.get(openList.indexOf(children.get(i))).changeParent(current);
}
}else{
//child not in closed list
openList.add(children.get(i));
//repaint the terrain panel with shades
tp.addAstarState(children.get(i));
tp.repaint();
try {
Thread.sleep(25);
} catch(Exception e) {
e.printStackTrace();
}
}
}
}
}
}
//returns the path from winning node to start for output
return current.getPath();
}
A* is basically Djikstra's algorithm but you direct your search to the destination by combining your distance function with an estimate of the remaining distance.
At node x, the cost or "score" is (distance_so_far) for djikstra
at A*, it is (distance_so_far + estimate_of_remaining_distance)
To make sure that A* finds the shortest path, the estimate_of_remaining_distance must be a true lower bound. That means it must always be less than the actual remaining distance. That means, you can never overestimate this distance otherwise it becomes an inexact heuristic in which case it won't necessarily find the shortest path.
This is a good reference: http://theory.stanford.edu/~amitp/GameProgramming/AStarComparison.html
It links to this reference which explains more about heuristic functions: http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html
I'm a student and me and my team have to make a simulation of student's behaviour in a campus (like making "groups of friends") walking etc. For finding path that student has to go, I used A* algorithm (as I found out that its one of fastest path-finding algorithms). Unfortunately our simulation doesn't run fluently (it takes like 1-2 sec between successive iterations). I wanted to optimize the algorithm but I don't have any idea what I can do more. Can you guys help me out and share with me information if its possible to optimize my A* algorithm? Here goes code:
public LinkedList<Field> getPath(Field start, Field exit) {
LinkedList<Field> foundPath = new LinkedList<Field>();
LinkedList<Field> opensList= new LinkedList<Field>();
LinkedList<Field> closedList= new LinkedList<Field>();
Hashtable<Field, Integer> gscore = new Hashtable<Field, Integer>();
Hashtable<Field, Field> cameFrom = new Hashtable<Field, Field>();
Field x = new Field();
gscore.put(start, 0);
opensList.add(start);
while(!opensList.isEmpty()){
int min = -1;
//searching for minimal F score
for(Field f : opensList){
if(min==-1){
min = gscore.get(f)+getH(f,exit);
x = f;
}else{
int currf = gscore.get(f)+getH(f,exit);
if(min > currf){
min = currf;
x = f;
}
}
}
if(x == exit){
//path reconstruction
Field curr = exit;
while(curr != start){
foundPath.addFirst(curr);
curr = cameFrom.get(curr);
}
return foundPath;
}
opensList.remove(x);
closedList.add(x);
for(Field y : x.getNeighbourhood()){
if(!(y.getType()==FieldTypes.PAVEMENT ||y.getType() == FieldTypes.GRASS) || closedList.contains(y) || !(y.getStudent()==null))
{
continue;
}
int tentGScore = gscore.get(x) + getDist(x,y);
boolean distIsBetter = false;
if(!opensList.contains(y)){
opensList.add(y);
distIsBetter = true;
}else if(tentGScore < gscore.get(y)){
distIsBetter = true;
}
if(distIsBetter){
cameFrom.put(y, x);
gscore.put(y, tentGScore);
}
}
}
return foundPath;
}
private int getH(Field start, Field end){
int x;
int y;
x = start.getX()-end.getX();
y = start.getY() - end.getY();
if(x<0){
x = x* (-1);
}
if(y<0){
y = y * (-1);
}
return x+y;
}
private int getDist(Field start, Field end){
int ret = 0;
if(end.getType() == FieldTypes.PAVEMENT){
ret = 8;
}else if(start.getX() == end.getX() || start.getY() == end.getY()){
ret = 10;
}else{
ret = 14;
}
return ret;
}
//EDIT
This is what i got from jProfiler:
So getH is a bottlneck yes? Maybe remembering H score of field would be a good idea?
A linked list is not a good data structure for the open set. You have to find the node with the smallest F from it, you can either search through the list in O(n) or insert in sorted position in O(n), either way it's O(n). With a heap it's only O(log n). Updating the G score would remain O(n) (since you have to find the node first), unless you also added a HashTable from nodes to indexes in the heap.
A linked list is also not a good data structure for the closed set, where you need fast "Contains", which is O(n) in a linked list. You should use a HashSet for that.
You can optimize the problem by using a different algorithm, the following page illustrates and compares many different aglorihms and heuristics:
A*
IDA*
Djikstra
JumpPoint
...
http://qiao.github.io/PathFinding.js/visual/
From your implementation it seems that you are using naive A* algorithm. Use following way:-
A* is algorithm which is implemented using priority queue similar to BFS.
Heuristic function is evaluated at each node to define its fitness to be selected as next node to be visited.
As new node is visited its neighboring unvisited nodes are added into queue with its heuristic values as keys.
Do this till every heuristic value in the queue is less than(or greater) calculated value of goal state.
Find bottlenecks of your implementation using profiler . ex. jprofiler is easy to use
Use threads in areas where algorithm can run simultaneously.
Profile your JavaVM to run faster.
Allocate more RAM
a) As mentioned, you should use a heap in A* - either a basic binary heap or a pairing heap which should be theoretically faster.
b) In larger maps, it always happens that you need some time for the algorithm to run (i.e., when you request a path, it will simply have to take some time). What can be done is to use some local navigation algorithm (e.g., "run directly to the target") while the path computes.
c) If you have reasonable amount of locations (e.g., in a navmesh) and some time at the start of your code, why not to use Floyd-Warshall's algorithm? Using that, you can the information where to go next in O(1).
I built a new pathfinding algorithm. called Fast* or Fastaer, It is a BFS like A* but is faster and efficient than A*, the accuracy is 90% A*. please see this link for info and demo.
https://drbendanilloportfolio.wordpress.com/2015/08/14/fastaer-pathfinder/
It has a fast greedy line tracer, to make path straighter.
The demo file has it all. Check Task manager when using the demo for performance metrics. So far upon building this the profiler results of this has maximum surviving generation of 4 and low to nil GC time.