001 package dk.deepthought.sidious.planner;
002
003 import java.util.ArrayList;
004 import java.util.Collection;
005 import java.util.PriorityQueue;
006 import java.util.Queue;
007
008 import net.jcip.annotations.ThreadSafe;
009
010 import org.apache.commons.logging.Log;
011 import org.apache.commons.logging.LogFactory;
012
013 import dk.deepthought.sidious.gui.SidiousOutput;
014 import dk.deepthought.sidious.planner.graph.Edge;
015 import dk.deepthought.sidious.planner.graph.Graph;
016 import dk.deepthought.sidious.planner.graph.Vertex;
017
018 /**
019 * Implements the A* search algorithm.
020 *
021 * @author Deepthought
022 *
023 */
024 @ThreadSafe
025 public class AStarAlgorithm implements Pathfinder {
026
027 /**
028 * Logger for this class.
029 */
030 private static final Log logger = LogFactory.getLog(AStarAlgorithm.class);
031
032 /**
033 * Flag to stop the calculation.
034 */
035 private volatile boolean cancelled = false;
036
037 /*
038 * (non-Javadoc)
039 *
040 * @see dk.deepthought.sidious.planner.Pathfinder#search(dk.deepthought.sidious.planner.graph.Graph)
041 */
042 public void search(Graph graph) {
043 jAStar(graph);
044 }
045
046 /**
047 * This method implements the A* algorithm.
048 * <p>
049 * The resulting shortest path is represented in the traversed graph, by
050 * following the predecessors from end vertex to start vertex.
051 *
052 * @param graph
053 * the graph to be searched
054 */
055 private void jAStar(Graph graph) {
056 if (logger.isDebugEnabled()) {
057 logger.debug("jAStar() - start");
058 }
059 Collection<Vertex> closedList = new ArrayList<Vertex>();
060 Queue<Vertex> openList = new PriorityQueue<Vertex>();
061 Vertex currentVertex;
062 Vertex goalVertex = graph.getGoalVertex();
063 Collection<Edge> edges = null;
064 openList.offer(graph.getSourceVertex());
065 int vertexCount = 0;
066 while (!openList.isEmpty() && !cancelled) {
067 currentVertex = openList.poll();
068 SidiousOutput.getInstance().addVertex(currentVertex);
069 if (logger.isDebugEnabled()) {
070 logger.debug("Polled: " + currentVertex);
071 }
072 if (currentVertex.partiallyEquals(goalVertex)) {
073 if (logger.isDebugEnabled()) {
074 logger.debug("jAStar() - goal reached - currentVertex="
075 + currentVertex + ", goalVertex=" + goalVertex);
076 }
077 graph.setApproximateGoal(currentVertex);
078 break;
079 }
080 edges = graph.getEdges(currentVertex);
081 vertexCount += edges.size();
082 for (Edge edge : edges) {
083 Vertex endVertex = edge.getEndVertex();
084 double sum = edge.getCost() + currentVertex.g();
085 endVertex.update(edge, sum);
086 if (endVertex.g() >= Double.MAX_VALUE) {
087 // FIXME Possibly remove
088 throw new IllegalStateException(
089 "Cost exceeded Double.MAX_VALUE value");
090 }
091 if (!openList.contains(endVertex)
092 && !closedList.contains(endVertex)) {
093 openList.offer(endVertex);
094 }
095 }
096 closedList.add(currentVertex);
097 }
098 if (logger.isDebugEnabled()) {
099 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 }
|