| 1 | package dk.deepthought.sidious.planner; |
| 2 | |
| 3 | import java.util.ArrayList; |
| 4 | import java.util.Collection; |
| 5 | import java.util.PriorityQueue; |
| 6 | import java.util.Queue; |
| 7 | |
| 8 | import net.jcip.annotations.ThreadSafe; |
| 9 | |
| 10 | import org.apache.commons.logging.Log; |
| 11 | import org.apache.commons.logging.LogFactory; |
| 12 | |
| 13 | import dk.deepthought.sidious.gui.SidiousOutput; |
| 14 | import dk.deepthought.sidious.planner.graph.Edge; |
| 15 | import dk.deepthought.sidious.planner.graph.Graph; |
| 16 | import dk.deepthought.sidious.planner.graph.Vertex; |
| 17 | |
| 18 | /** |
| 19 | * Implements the A* search algorithm. |
| 20 | * |
| 21 | * @author Deepthought |
| 22 | * |
| 23 | */ |
| 24 | @ThreadSafe |
| 25 | public class AStarAlgorithm implements Pathfinder { |
| 26 | |
| 27 | /** |
| 28 | * Logger for this class. |
| 29 | */ |
| 30 | private static final Log logger = LogFactory.getLog(AStarAlgorithm.class); |
| 31 | |
| 32 | /** |
| 33 | * Flag to stop the calculation. |
| 34 | */ |
| 35 | private volatile boolean cancelled = false; |
| 36 | |
| 37 | /* |
| 38 | * (non-Javadoc) |
| 39 | * |
| 40 | * @see dk.deepthought.sidious.planner.Pathfinder#search(dk.deepthought.sidious.planner.graph.Graph) |
| 41 | */ |
| 42 | public void search(Graph graph) { |
| 43 | jAStar(graph); |
| 44 | } |
| 45 | |
| 46 | /** |
| 47 | * This method implements the A* algorithm. |
| 48 | * <p> |
| 49 | * The resulting shortest path is represented in the traversed graph, by |
| 50 | * following the predecessors from end vertex to start vertex. |
| 51 | * |
| 52 | * @param graph |
| 53 | * the graph to be searched |
| 54 | */ |
| 55 | private void jAStar(Graph graph) { |
| 56 | if (logger.isDebugEnabled()) { |
| 57 | logger.debug("jAStar() - start"); |
| 58 | } |
| 59 | Collection<Vertex> closedList = new ArrayList<Vertex>(); |
| 60 | Queue<Vertex> openList = new PriorityQueue<Vertex>(); |
| 61 | Vertex currentVertex; |
| 62 | Vertex goalVertex = graph.getGoalVertex(); |
| 63 | Collection<Edge> edges = null; |
| 64 | openList.offer(graph.getSourceVertex()); |
| 65 | int vertexCount = 0; |
| 66 | while (!openList.isEmpty() && !cancelled) { |
| 67 | currentVertex = openList.poll(); |
| 68 | SidiousOutput.getInstance().addVertex(currentVertex); |
| 69 | if (logger.isDebugEnabled()) { |
| 70 | logger.debug("Polled: " + currentVertex); |
| 71 | } |
| 72 | if (currentVertex.partiallyEquals(goalVertex)) { |
| 73 | if (logger.isDebugEnabled()) { |
| 74 | logger.debug("jAStar() - goal reached - currentVertex=" |
| 75 | + currentVertex + ", goalVertex=" + goalVertex); |
| 76 | } |
| 77 | graph.setApproximateGoal(currentVertex); |
| 78 | break; |
| 79 | } |
| 80 | edges = graph.getEdges(currentVertex); |
| 81 | vertexCount += edges.size(); |
| 82 | for (Edge edge : edges) { |
| 83 | Vertex endVertex = edge.getEndVertex(); |
| 84 | double sum = edge.getCost() + currentVertex.g(); |
| 85 | endVertex.update(edge, sum); |
| 86 | if (endVertex.g() >= Double.MAX_VALUE) { |
| 87 | // FIXME Possibly remove |
| 88 | throw new IllegalStateException( |
| 89 | "Cost exceeded Double.MAX_VALUE value"); |
| 90 | } |
| 91 | if (!openList.contains(endVertex) |
| 92 | && !closedList.contains(endVertex)) { |
| 93 | openList.offer(endVertex); |
| 94 | } |
| 95 | } |
| 96 | closedList.add(currentVertex); |
| 97 | } |
| 98 | if (logger.isDebugEnabled()) { |
| 99 | logger.debug("jAStar() - end - looked at: " + vertexCount |
| 100 | + " vertices"); |
| 101 | } |
| 102 | } |
| 103 | |
| 104 | /* |
| 105 | * (non-Javadoc) |
| 106 | * |
| 107 | * @see dk.deepthought.sidious.planner.Pathfinder#cancel() |
| 108 | */ |
| 109 | public void cancel() { |
| 110 | this.cancelled = true; |
| 111 | } |
| 112 | |
| 113 | } |