| 1 | package dk.deepthought.sidious.planner.graph; |
| 2 | |
| 3 | import java.util.ArrayList; |
| 4 | |
| 5 | import net.jcip.annotations.ThreadSafe; |
| 6 | |
| 7 | import org.apache.commons.logging.Log; |
| 8 | import org.apache.commons.logging.LogFactory; |
| 9 | |
| 10 | import dk.deepthought.sidious.supportsystem.Adjustable; |
| 11 | import dk.deepthought.sidious.supportsystem.State; |
| 12 | import dk.deepthought.sidious.supportsystem.Step; |
| 13 | |
| 14 | /** |
| 15 | * This class implements a vertex with respect to the A*-algorithm. |
| 16 | * |
| 17 | * @author Deepthought |
| 18 | * |
| 19 | */ |
| 20 | @ThreadSafe |
| 21 | public final class AStarVertex implements Vertex, Comparable<AStarVertex> { |
| 22 | |
| 23 | /** |
| 24 | * Logger of this class. |
| 25 | */ |
| 26 | private static final Log logger = LogFactory.getLog(AStarVertex.class); |
| 27 | |
| 28 | /** |
| 29 | * The state this represents. |
| 30 | */ |
| 31 | private final State state; |
| 32 | |
| 33 | /** |
| 34 | * The calculated heuristic for this. |
| 35 | */ |
| 36 | private final double heuristic; |
| 37 | |
| 38 | /** |
| 39 | * The edge to the predecessor of this. |
| 40 | */ |
| 41 | private Edge edgeToPredecessor; |
| 42 | |
| 43 | /** |
| 44 | * Flag to indicate this being a source vertex. |
| 45 | */ |
| 46 | private final boolean source; |
| 47 | |
| 48 | /** |
| 49 | * Flag to indicate this being a goal vertex. |
| 50 | */ |
| 51 | private final boolean goal; |
| 52 | |
| 53 | /** |
| 54 | * The cost-candidate of getting to this vertex from the source vertex. Is |
| 55 | * initialized to an "arbitrary" maximum value. |
| 56 | */ |
| 57 | private double costCandidate = Double.MAX_VALUE; |
| 58 | |
| 59 | /** |
| 60 | * Creates a <code>AStarVertex</code> with the specified |
| 61 | * <code>State</code> and heuristic value. |
| 62 | * <p> |
| 63 | * According to the A*-algorithm an estimated cost from any vertex to the |
| 64 | * goal vertex <i>cannot</i> be negative. |
| 65 | * |
| 66 | * @param state |
| 67 | * the <code>State</code> this vertex represent |
| 68 | * @param heuristic |
| 69 | * the heuristic |
| 70 | */ |
| 71 | public AStarVertex(final State state, final double heuristic) { |
| 72 | if (heuristic < 0) { |
| 73 | throw new IllegalArgumentException( |
| 74 | "Heuristic value less than nil: " + heuristic); |
| 75 | } |
| 76 | this.state = state; |
| 77 | this.heuristic = heuristic; |
| 78 | goal = false; |
| 79 | source = false; |
| 80 | } |
| 81 | |
| 82 | /** |
| 83 | * Private utility constructor. |
| 84 | * |
| 85 | * @param state |
| 86 | * the state |
| 87 | * @param heuristic |
| 88 | * the heuristic |
| 89 | * @param edgeToPredecessor |
| 90 | * the edge |
| 91 | * @param source |
| 92 | * is source vertex flag |
| 93 | * @param goal |
| 94 | * is goal vertex flag |
| 95 | * @param costCandidate |
| 96 | * the cost candidate |
| 97 | */ |
| 98 | private AStarVertex(State state, double heuristic, Edge edgeToPredecessor, |
| 99 | boolean source, boolean goal, double costCandidate) { |
| 100 | if (state == null) { |
| 101 | throw new IllegalArgumentException("null not valid state."); |
| 102 | } |
| 103 | this.state = state; |
| 104 | this.heuristic = heuristic; |
| 105 | this.edgeToPredecessor = edgeToPredecessor; |
| 106 | this.source = source; |
| 107 | this.goal = goal; |
| 108 | this.costCandidate = costCandidate; |
| 109 | } |
| 110 | |
| 111 | /** |
| 112 | * This method returns an estimated cost of getting to the goal from this |
| 113 | * vertex. This estimate is calculated as the Euclidian distance to the |
| 114 | * goal. |
| 115 | * |
| 116 | * @return an estimated cost of getting to the goal |
| 117 | */ |
| 118 | public double h() { |
| 119 | return heuristic; |
| 120 | } |
| 121 | |
| 122 | /* |
| 123 | * (non-Javadoc) |
| 124 | * |
| 125 | * @see dk.deepthought.sidious.planner.Vertex#g() |
| 126 | */ |
| 127 | public synchronized double g() { |
| 128 | return costCandidate; |
| 129 | } |
| 130 | |
| 131 | /** |
| 132 | * This is the implementation of the <code>update</code> method. |
| 133 | * <p> |
| 134 | * In accordance with the A*-algorithm the following will throw |
| 135 | * {@link AssertionError}: |
| 136 | * <li>Negative costs, which imply negative edge weights. |
| 137 | * <li>An edgeToPredecessor which is <code>null</code>, because it will break the |
| 138 | * shortest path tree. |
| 139 | * |
| 140 | * @see dk.deepthought.sidious.planner.graph.Vertex#update(dk.deepthought.sidious.planner.graph.Edge, |
| 141 | * double) |
| 142 | */ |
| 143 | public synchronized void update(Edge edgeToPredecessor, double cost) { |
| 144 | if (logger.isDebugEnabled()) { |
| 145 | logger.debug("update(Edge edgeToPredecessor=" + edgeToPredecessor |
| 146 | + ", double cost=" + cost + ") - start"); |
| 147 | } |
| 148 | |
| 149 | assert cost >= 0 : "The cost was less than nil: cost = " + cost; |
| 150 | assert edgeToPredecessor != null : "The edgeToPredecessor was null"; |
| 151 | if (cost < costCandidate) { |
| 152 | this.edgeToPredecessor = edgeToPredecessor; |
| 153 | costCandidate = cost; |
| 154 | logger.debug(this); |
| 155 | } |
| 156 | |
| 157 | if (logger.isDebugEnabled()) { |
| 158 | logger.debug("update(Edge edgeToPredecessor=" + edgeToPredecessor |
| 159 | + ", double cost=" + cost + ") - end"); |
| 160 | } |
| 161 | } |
| 162 | |
| 163 | /* |
| 164 | * (non-Javadoc) |
| 165 | * |
| 166 | * @see dk.deepthought.sidious.planner.Vertex#toGoalVertex() |
| 167 | */ |
| 168 | public Vertex toGoalVertex() { |
| 169 | if (logger.isDebugEnabled()) { |
| 170 | logger.debug("toGoalVertex() - start"); |
| 171 | } |
| 172 | |
| 173 | Vertex goalVertex = new AStarVertex(state, 0, edgeToPredecessor, false, |
| 174 | true, g()); |
| 175 | logger.debug("Goal vertex created: " + goalVertex); |
| 176 | return goalVertex; |
| 177 | } |
| 178 | |
| 179 | /* |
| 180 | * (non-Javadoc) |
| 181 | * |
| 182 | * @see dk.deepthought.sidious.planner.Vertex#toSourceVertex(dk.deepthought.sidious.supportsystem.Step) |
| 183 | */ |
| 184 | public Vertex toSourceVertex(Step initialStep) { |
| 185 | if (logger.isDebugEnabled()) { |
| 186 | logger.debug("toSourceVertex(Step initialStep=" + initialStep |
| 187 | + ") - start"); |
| 188 | } |
| 189 | |
| 190 | Edge edge = new AStarEdge(null, this, initialStep, 0); |
| 191 | AStarVertex sourceVertex = new AStarVertex(state, heuristic, edge, |
| 192 | true, false, 0); |
| 193 | logger.debug("Source vertex created: " + sourceVertex); |
| 194 | return sourceVertex; |
| 195 | } |
| 196 | |
| 197 | /* |
| 198 | * (non-Javadoc) |
| 199 | * |
| 200 | * @see dk.deepthought.sidious.planner.Vertex#isGoal() |
| 201 | */ |
| 202 | public boolean isGoal() { |
| 203 | return goal; |
| 204 | } |
| 205 | |
| 206 | /* |
| 207 | * (non-Javadoc) |
| 208 | * |
| 209 | * @see dk.deepthought.sidious.planner.Vertex#isSource() |
| 210 | */ |
| 211 | public boolean isSource() { |
| 212 | return source; |
| 213 | } |
| 214 | |
| 215 | /* |
| 216 | * (non-Javadoc) |
| 217 | * |
| 218 | * @see dk.deepthought.sidious.planner.Vertex#getPredecessorVertex() |
| 219 | */ |
| 220 | public Vertex getPredecessorVertex() { |
| 221 | if (logger.isDebugEnabled()) { |
| 222 | logger.debug("getPredecessorVertex() - start"); |
| 223 | } |
| 224 | |
| 225 | Vertex returnVertex = ((edgeToPredecessor == null) ? null |
| 226 | : edgeToPredecessor.getStartVertex()); |
| 227 | if (logger.isDebugEnabled()) { |
| 228 | logger.debug("getPredecessorVertex() - end - return value=" |
| 229 | + returnVertex); |
| 230 | } |
| 231 | return returnVertex; |
| 232 | |
| 233 | } |
| 234 | |
| 235 | /* |
| 236 | * (non-Javadoc) |
| 237 | * |
| 238 | * @see dk.deepthought.sidious.planner.Vertex#getStepToThis() |
| 239 | */ |
| 240 | public Step getStepToThis() { |
| 241 | if (logger.isDebugEnabled()) { |
| 242 | logger.debug("getStepToThis() - start"); |
| 243 | } |
| 244 | Step returnStep = (edgeToPredecessor == null) ? |
| 245 | new Step(new ArrayList<Adjustable>()) : |
| 246 | edgeToPredecessor.getStep(); |
| 247 | if (returnStep == null) { |
| 248 | // logger.error(""); |
| 249 | throw new IllegalStateException( |
| 250 | "null not valid return value. For: " + this); |
| 251 | } |
| 252 | if (logger.isDebugEnabled()) { |
| 253 | logger.debug("getStepToThis() - end - return value=" + returnStep); |
| 254 | } |
| 255 | return returnStep; |
| 256 | } |
| 257 | |
| 258 | /* |
| 259 | * (non-Javadoc) |
| 260 | * |
| 261 | * @see dk.deepthought.sidious.planner.Vertex#getState() |
| 262 | */ |
| 263 | public State getState() { |
| 264 | return state; |
| 265 | } |
| 266 | |
| 267 | /* |
| 268 | * (non-Javadoc) |
| 269 | * |
| 270 | * @see dk.deepthought.sidious.planner.Vertex#partiallyEquals(dk.deepthought.sidious.planner.Vertex) |
| 271 | */ |
| 272 | public boolean partiallyEquals(Vertex otherVertex) { |
| 273 | return state.partiallyEquals(otherVertex.getState()); |
| 274 | } |
| 275 | |
| 276 | /** |
| 277 | * This method implements the <code>compareTo</code> method of the |
| 278 | * <code>Comparable</code> interface. |
| 279 | * <p> |
| 280 | * The method is needed to specify the ordering of the |
| 281 | * <code>AStarVertex</code> objects. This ordering is imperative for the |
| 282 | * functionality of the pathfinding algorithm. |
| 283 | * |
| 284 | * @see java.lang.Comparable#compareTo(java.lang.Object) |
| 285 | * @see dk.deepthought.sidious.planner.Pathfinder#search() |
| 286 | */ |
| 287 | public int compareTo(AStarVertex v) { |
| 288 | if (logger.isDebugEnabled()) { |
| 289 | logger.debug("compareTo(AStarVertex v=" + v + ") - start"); |
| 290 | } |
| 291 | |
| 292 | // assert g() < g() + 1 : "Select IS broken"; |
| 293 | // assert v.g() < v.g() + 1 : "Select IS broken"; |
| 294 | int returnint = Double.compare(g() + h(), v.g() + v.h()); |
| 295 | if (logger.isDebugEnabled()) { |
| 296 | logger.debug("compareTo(AStarVertex v=" + v |
| 297 | + ") - end - return value=" + returnint); |
| 298 | } |
| 299 | return returnint; |
| 300 | } |
| 301 | |
| 302 | /* |
| 303 | * (non-Javadoc) |
| 304 | * |
| 305 | * @see java.lang.Object#hashCode() |
| 306 | */ |
| 307 | @Override |
| 308 | public int hashCode() { |
| 309 | final int PRIME = 31; |
| 310 | int result = 1; |
| 311 | result = PRIME * result + ((state == null) ? 0 : state.hashCode()); |
| 312 | return result; |
| 313 | } |
| 314 | |
| 315 | /* |
| 316 | * (non-Javadoc) |
| 317 | * |
| 318 | * @see java.lang.Object#equals(java.lang.Object) |
| 319 | */ |
| 320 | @Override |
| 321 | public boolean equals(Object obj) { |
| 322 | if (this == obj) { |
| 323 | return true; |
| 324 | } |
| 325 | if (obj == null) { |
| 326 | return false; |
| 327 | } |
| 328 | if (getClass() != obj.getClass()) { |
| 329 | return false; |
| 330 | } |
| 331 | final AStarVertex other = (AStarVertex) obj; |
| 332 | if (state == null) { |
| 333 | if (other.state != null) { |
| 334 | return false; |
| 335 | } |
| 336 | } else if (!state.equals(other.state)) { |
| 337 | return false; |
| 338 | } |
| 339 | return true; |
| 340 | } |
| 341 | |
| 342 | /* |
| 343 | * (non-Javadoc) |
| 344 | * |
| 345 | * @see java.lang.Object#toString() |
| 346 | */ |
| 347 | @Override |
| 348 | public String toString() { |
| 349 | return getClass().getSimpleName() + "[cost=" + costCandidate |
| 350 | + ", heuristic=" + heuristic + "]"; |
| 351 | } |
| 352 | |
| 353 | } |