public void visitNeighbour(AStarNode origin, int x, int y, int z) { int costFromStart = origin.getCostFromStart() + goal.getNodeCost(origin, x, y, z); int costToGoal = goal.distanceToGoal(x, y, z); int totalCost = costFromStart + costToGoal; // Check if on open/closed list // If yes, check if new path is more efficient (lower f value) // and update path if so. AStarNode foundNode = openList.findOpenNode(x, y, z); boolean foundCheaperPath = false; if (foundNode != null) { if (totalCost < foundNode.getTotalCost()) { foundCheaperPath = true; openList.removeNodeFromOpenList(foundNode); } } else { foundNode = closedList.findClosedNode(x, y, z); if (foundNode != null) { if (totalCost < foundNode.getTotalCost()) { foundCheaperPath = true; closedList.removeNodeFromClosedList(foundNode); } } else { // Node not found in open and closed list, thus put a // new one on the open list. foundNode = map.nodeAt(x, y, z); foundCheaperPath = true; } } if (foundCheaperPath) { foundNode.initialize(origin, costFromStart, costToGoal, totalCost, x, y, z); openList.addNodeToOpenList(foundNode); } }
/** * Returns if all regions are currently not occupied by another search. * * @return */ public boolean isBoundingBoxFree() { for (int x = westernBound; x <= easternBound; x++) for (int y = northernBound; y <= southernBound; y++) for (int z = upperBound; z <= lowerBound; z++) if (!isOutsideMap(x, y, z)) if (regionGrid.isTileBlocked(x, y, z)) return false; return true; }
/* * (non-Javadoc) * * @see machine.AStarMachine#findPath() */ public List<AStarNode> findPath( int startX, int startY, int startZ, int endX, int endY, int endZ) { if (map == null) throw new IllegalStateException( "The searched map must be specified before path search can be done."); if (this.goal == null) throw new IllegalStateException( "The goal object must be specified before path search can be done."); AStarNode goalNode = map.nodeAt(endX, endY, endZ); int goalDistance = this.goal.getNodeCost(goalNode, startX, startY, startZ); goalNode.initialize(null, goalDistance, 0, goalDistance, endX, endY, endZ); this.goal.setGoalNode(goalNode); AStarNode startNode = map.nodeAt(startX, startY, startZ); startNode.initialize(null, 0, goalDistance, goalDistance, startX, startY, startZ); openList.addNodeToOpenList(startNode); return AStar.findPath(openList, closedList, map, this.goal, nodePool, this); }
private boolean isOutsideMap(int x, int y, int z) { boolean outsideX = false; boolean outsideY = false; boolean outsideZ = false; if (regionGrid.getMaxXNodes() != 0) outsideX = (x < 0) || (x >= regionGrid.getMaxXNodes()); if (regionGrid.getMaxYNodes() != 0) outsideY = (y < 0) || (y >= regionGrid.getMaxYNodes()); if (regionGrid.getMaxZNodes() != 0) outsideZ = (z < 0) || (z >= regionGrid.getMaxZNodes()); return (outsideX || outsideY || outsideZ); }
/** Marks all AStarRegionNodes inside this bounding box as not occupied. */ public void releaseBoundingBox() { for (int x = westernBound; x <= easternBound; x++) for (int y = northernBound; y <= southernBound; y++) for (int z = upperBound; z <= lowerBound; z++) if (!isOutsideMap(x, y, z)) regionGrid.setTileBlocked(x, y, z, false); }