private void Loop() {
while (!H.isEmpty()) {
Cell v = H.poll();
if (Log.ON) Log.write("popped: " + potentialField[v.col][v.row]);
frozenDjogurt[v.col][v.row] = true;
for (Cell vn : map.doSomeMagic(v)) {
if (Log.ON) {
Log.write("\n" + toString(v));
Log.write("\n" + printStack());
}
if (!(map.cells[vn.col][vn.row].type == CellType.OBSTACLE)) {
if (!frozenDjogurt[vn.col][vn.row]) {
double d = computeArrivalTime(vn);
if (d > maxElement) maxElement = d;
if (!H.contains(vn)) {
potentialField[vn.col][vn.row] = d;
H.add(vn);
} else {
// if(potentialField[vn.col][vn.row] > d)
// {
H.remove(vn);
potentialField[vn.col][vn.row] = d;
H.add(vn);
// }
}
}
}
}
}
}
private static Job getShortestJob(List<Job> jobList, float currentTime) {
for (Job j : jobList) {
if (!pq.contains(j) && (float) j.requestTime <= currentTime) {
pq.add(j);
}
}
return pq.poll();
}
@Test
public void testHostManafer() {
HashTaskCache.HostManager m = new HashTaskCache.HostManager(11, "baidu.com");
HashTaskCache.HostManager m2 = new HashTaskCache.HostManager(11, "baidu.com");
PriorityQueue<HashTaskCache.HostManager> queue = new PriorityQueue<HashTaskCache.HostManager>();
queue.add(m);
System.out.print(queue.contains(m2));
}
private Path aStar() {
init();
// While there are still node to explore
while (!openSet.isEmpty()) {
// Get the node with the lowest f_score value
Node currentNode = openSet.poll();
Square currentSquare = currentNode.getSquare();
// System.out.println(currentSquare.toString() + " openSet");
// If current square is the destination square, return the reconstructed path
if (currentSquare.equals(endPosition)) {
return reconstructPath(currentSquare);
}
closedSet.add(currentSquare);
piece.move(currentSquare);
// for (Square neighbourSquare : piece.getValidMoves()) {
// System.out.print(neighbourSquare + " ");
// }
// System.out.println();
// Explore the neighbours of the current square
for (Square neighbourSquare : piece.getValidMoves()) {
// System.out.println(neighbourSquare.toString() + " neighbour");
// If the current neighbour square has already been explored, pass
if (closedSet.contains(neighbourSquare)) {
continue;
}
// Get a tentative gScore, default the distance from 'piece' to 'neighbourSquare' to 1
// (step).
int tentativeGScore = gScore.get(currentSquare) + 1;
if ((!openSet.contains(neighbourSquare)) || tentativeGScore < gScore.get(neighbourSquare)) {
cameFrom.put(neighbourSquare, currentSquare);
gScore.put(neighbourSquare, tentativeGScore);
fScore.put(
neighbourSquare,
(tentativeGScore
+ ((AStarPiece) piece).getHeuristicMoves(neighbourSquare, endPosition)));
// Add this neighbour to the open set
Node newNode = new Node(neighbourSquare, fScore.get(neighbourSquare));
openSet.add(newNode);
}
}
}
// If no path is found, return null
return null;
}
private boolean undirectedAdd(UndirectedWeightedEdge e, UndirectedNode s) {
HashMap<Node, Node> parent = parents.get(s);
HashMap<Node, Integer> height = heights.get(s);
IntWeight eWeight = (IntWeight) e.getWeight();
UndirectedNode n1 = e.getNode1();
UndirectedNode n2 = e.getNode2();
if (height.get(n1) > height.get(n2)) {
n1 = e.getNode2();
n2 = e.getNode1();
}
if (height.get(n1) + (int) eWeight.getWeight() >= height.get(n2)
|| height.get(n1) + (int) eWeight.getWeight() < 0) {
return true;
}
if (height.get(n2) != Integer.MAX_VALUE) apsp.decr(height.get(n2));
apsp.incr(height.get(n1) + (int) eWeight.getWeight());
height.put(n2, height.get(n1) + (int) eWeight.getWeight());
parent.put(n2, n1);
PriorityQueue<Node> q = new PriorityQueue<>();
q.add(n2);
while (!q.isEmpty()) {
Node current = q.poll();
if (height.get(current) == Integer.MAX_VALUE) {
break;
}
for (IElement edge : current.getEdges()) {
UndirectedWeightedEdge d = (UndirectedWeightedEdge) edge;
Node neighbor = d.getDifferingNode(current);
IntWeight dWeight = (IntWeight) d.getWeight();
int alt = height.get(current) + (int) dWeight.getWeight();
if (alt < height.get(neighbor)) {
if (height.get(neighbor) != Integer.MAX_VALUE) apsp.decr(height.get(neighbor));
apsp.incr(alt);
height.put(neighbor, alt);
parent.put(neighbor, current);
if (q.contains(neighbor)) {
q.remove(neighbor);
}
q.add(neighbor);
}
}
}
return true;
}
/*
* A* Algo from psuedo code from wikipedia
* http://en.wikipedia.org/wiki/A*_search_algorithm
*/
private int[] aStar(Node startNode, Node goalNode, int maxLength) {
ArrayList<Node> closedSet = new ArrayList<Node>();
PriorityQueue<Node> openSet = new PriorityQueue<Node>();
// When handling SPNodes
if (goalNode instanceof SPNode) {
goalNode = node[startNode.col()];
}
int tentativeG;
Node current;
boolean tentativeBetter = false;
// set details of start node
startNode.g = 0;
startNode.h = this.heuristicDistance(startNode, goalNode);
startNode.f = startNode.h;
// add startNode to openset
openSet.add(startNode);
while (!openSet.isEmpty()) {
current = openSet.poll();
if (this.reachedGoal(current)) {
reconstructPath(current);
return this.toIntArray(this.path);
}
closedSet.add(current);
if (current.g >= maxLength) continue;
for (Node attached : getDirectlyAttachedNodes(current)) {
if (closedSet.contains(attached)) continue;
tentativeG = current.g + 1;
if (!(openSet.contains(attached))) {
tentativeBetter = true;
} else if (tentativeG < attached.g) {
tentativeBetter = true;
} else {
tentativeBetter = false;
}
if (tentativeBetter) {
attached.parent = current;
attached.g = tentativeG;
attached.h = heuristicDistance(attached, goalNode);
attached.f = attached.g + attached.h;
openSet.add(attached);
}
}
}
return null;
}
public void fetchAnchorLinks(File file, URL parentUrl) throws IOException {
Document doc = Jsoup.parse(file, "UTF-8", parentUrl.toString());
Elements links = doc.select("a[href]");
for (Element link : links) {
String linkHref = link.attr("href");
String anchorText = link.text();
URL childUrl = new URL(parentUrl, linkHref);
Link newLink = new Link(childUrl, anchorText);
int score = computeScore(file, link, query, parentUrl);
if (knownUrls.size() == maxPages) {
break;
}
if (!knownUrls.containsKey(childUrl)) {
if (!urlQueue.contains(newLink)) {
newLink.setScore(score);
urlQueue.add(newLink);
// allLinks.put(childUrl, newLink);
if (debug) {
System.out.println(
"Adding to queue: " + newLink.getURL() + " with score = " + newLink.getScore());
}
}
} else {
Link retrievedLink = knownUrls.get(childUrl);
if (urlQueue.contains(retrievedLink)) {
int newScore = score + retrievedLink.getScore();
retrievedLink.setScore(newScore);
// knownUrls.put(childUrl,retrievedLink);
if (debug) {
System.out.println("Adding " + score + " to score of " + retrievedLink.getURL());
}
}
}
}
System.out.println("\n");
}
private void initialization() {
for (Cell v : map.goals) {
frozenDjogurt[v.col][v.row] = true;
for (Cell vn : map.doSomeMagic(v)) {
double d = computeArrivalTime(vn);
if (!frozenDjogurt[vn.col][vn.row])
if (!H.contains(vn)) {
potentialField[vn.col][vn.row] = d;
H.add(vn);
} else {
H.remove(vn);
potentialField[vn.col][vn.row] = d;
H.add(vn);
}
}
}
}
public void printProofTree(Clause finalClause, LinkedList<Clause> clauseList) {
PriorityQueue<Integer> proofTree =
new PriorityQueue<
Integer>(); // Will be used to order the ancestors of the finalClause for output
LinkedList<Clause> treeQueue =
new LinkedList<
Clause>(); // Will take in the ancestors of the finalClause. Dequeue each element, add
// it to the proofTree, then add the parents to the queue
int[] parentIDs;
treeQueue.add(finalClause);
while (!treeQueue.isEmpty()) {
Clause polledClause = treeQueue.poll();
if (proofTree.contains(
polledClause
.getClauseID())) // Skip this iteration if the clause has already been added to the
// proofTree
{
continue;
}
proofTree.add(polledClause.getClauseID());
parentIDs = polledClause.getParentIDs();
if (parentIDs[0]
!= -1) // if one parent exists, the other must exist and we add the parents to the queue
{
treeQueue.add(clauseList.get(parentIDs[0] - 1)); // add the first parent to the queue
treeQueue.add(clauseList.get(parentIDs[1] - 1)); // add the second parent to the queue
}
}
// output all the clauses in the proof tree
while (proofTree.peek() != null) {
clauseList.get(proofTree.poll() - 1).outputClause();
}
}
public Route start(DateTime time) {
this.startTime = time;
ArrayList<FakeNode> neighbouringNodes;
hScore(startNode);
openSet.add(startNode);
while (!openSet.isEmpty()) {
currentNode = openSet.poll();
neighbouringNodes = getNeighbours(currentNode.id);
for (FakeNode neighbour : neighbouringNodes) {
if (neighbour.visited == true) {
continue;
} else if (openSet.contains(neighbour)) {
if (neighbour.g_scores > gScore(neighbour)) {
neighbour.cameFrom = currentNode;
calcScores(neighbour);
}
continue;
} else {
neighbour.cameFrom = currentNode;
}
if (endNode == neighbour) {
calcScores(neighbour);
return reconstructPath();
}
calcScores(neighbour);
openSet.add(neighbour);
}
currentNode.visited = true;
}
return null;
}
public void StartRace() throws IOException {
// Get start point and finish points
RoadCell start = new RoadCell();
for (int h = 0; h < height; h++) {
for (int w = 0; w < width; w++) {
if (RoadGrid[w][h].type.equalsIgnoreCase("S")) {
int[] arr = {0, 0};
start.set(w, h);
start.g = 1;
start.m = arr;
start.type = RoadGrid[w][h].type;
} else if (RoadGrid[w][h].type.equalsIgnoreCase("F")) {
RoadCell rc = new RoadCell();
rc.set(w, h);
rc.f = w; // Sort by x coord value
rc.g = 0;
rc.h = 0;
rc.type = RoadGrid[w][h].type;
FinishPts.add(rc);
}
}
}
// Get furthest left finish point
HeuristicFinishPt = FinishPts.peek();
RoadGrid[HeuristicFinishPt.x][HeuristicFinishPt.y].type = "H";
start.h = CalcH(start.x, start.y);
start.f = start.h + 0;
OpenSet.add(start);
fstream = new FileWriter(outputpath);
out = new BufferedWriter(fstream);
double startTime = System.currentTimeMillis();
while (!isAtGoal) {
/*
1. For each moving vector in S do the following
1.1. If the speed of move is over the top speed ignore this moving vector.
1.2. Apply the breakTheMovementVactor function to get a sequence of breakdown moves (right, left, up, or down).
1.3. Start from the current position follow the segments. After each single segment, check the cell to see if it is off-the-road or not. If yes, ignore the whole moving vector.
1.4. If there are still some valid moves in S, select one of them according to the f (g+h) value.
1.5. If all the moves are invalid, go back to your fringe and select another candidate to expand.
*/
Vector<int[]> MovingVectors = new Vector<int[]>();
Vector<int[]> BreakVectors = new Vector<int[]>();
// PriorityQueue<RoadCell> ValidPositions = new PriorityQueue<RoadCell>();
/*Begin movement calculations
* Calculate movement vector based on PrevM
* Then get the breakdown of the path for that movement vector
* Check if the path is valid (doesn't go off road)
* If valid and put in the min heap sort by F
*/
CurPos = OpenSet.poll();
// Save Step's M value
PrevM = CurPos.m;
// Check if at/crossed the finish line
if (RoadGrid[CurPos.x][CurPos.y].type.equalsIgnoreCase("F")) {
isAtGoal = true;
Finish.set(CurPos.x, CurPos.y);
Finish.h = CalcH(Finish.x, Finish.y);
Finish.g = CurPos.g + 1;
Finish.f = Finish.g + Finish.h;
Finish.parent = CurPos;
break;
}
ClosedSet.add(CurPos);
MovingVectors = CalcMovingVectors();
for (int i = 0; i < MovingVectors.size(); i++) {
boolean isValid = true;
int x = MovingVectors.get(i)[0];
int y = MovingVectors.get(i)[1];
if (CalcSpeed(x, y) > TopSpeed) // Greater than top speed
continue;
BreakVectors = breakTheMovementVector(x, y);
// For each of the break down vectors check if resulting position is not off road and store
// it if on road
int movX = CurPos.x;
int movY = CurPos.y;
for (int j = 0; j < BreakVectors.size(); j++) {
movX += BreakVectors.get(j)[0];
movY += BreakVectors.get(j)[1];
if (movY < 1
|| movY >= height
|| movX < 1
|| movX >= width
|| RoadGrid[movX][movY].type.equals("#")) {
isValid = false;
continue;
}
}
// If there are valid positions sort them in a min heap/priority queue
if (isValid) {
RoadCell rc =
new RoadCell(CurPos.x + x, CurPos.y + y, CurPos.g + 1, MovingVectors.get(i));
rc.type = RoadGrid[CurPos.x + x][CurPos.y + y].type;
if (!ClosedSet.contains(rc)) {
if (rc.type.equals("#")) {
// System.out.print("poop");
// out.close();
// System.exit(0);
}
rc.g = CurPos.g + 1;
rc.h = CalcH(rc.x, rc.y);
rc.f = rc.g + rc.h;
rc.parent = CurPos;
rc.m = MovingVectors.get(i);
if (!OpenSet.contains(rc)) {
OpenSet.add(rc);
NumExpanded++;
} else {
Object[] openArr = OpenSet.toArray();
for (int n = 0; n < openArr.length; n++) {
if (openArr[n].equals(rc)) {
if (rc.f < ((RoadCell) openArr[n]).f) {
OpenSet.remove(openArr[n]);
OpenSet.add(rc);
NumExpanded++;
}
break;
}
}
}
}
}
}
} // End While
double finishTime = System.currentTimeMillis();
System.out.println("");
System.out.println("Goal Reached");
System.out.println("Top Speed: 30");
out.write("" + "\n");
out.write("Goal Reached" + "\n");
out.write("Top Speed: 30" + "\n");
int numSteps = CurPos.g;
// RoadGrid[CurPos.x][CurPos.y].type = "G";
// for(int i=0;i<height;i++)
// {
// int dy = Math.abs(i-CurPos.y);
// for(int j=0;j<width;j++)
// {
// int dx = Math.abs(j-CurPos.x);
//
// if(dx <= 50.0 && dy <= 10)
// //System.out.print(RoadGrid[j][i].type);
// out.write(RoadGrid[j][i].type);
// }
// if(dy <= 10)
// //System.out.print("\n");
// out.write("\n");
// }
while (CurPos.parent != null) {
System.out.println(
"Step "
+ CurPos.g
+ " {P: "
+ "("
+ CurPos.x
+ ","
+ CurPos.y
+ ") "
+ " M: "
+ "("
+ CurPos.m[0]
+ ","
+ CurPos.m[1]
+ ") "
+ " g="
+ CurPos.g
+ " h="
+ CurPos.h
+ " f="
+ CurPos.f
+ " Speed="
+ CalcSpeed(CurPos.m[0], CurPos.m[1]));
out.write(
"Step "
+ CurPos.g
+ " {P: "
+ "("
+ CurPos.x
+ ","
+ CurPos.y
+ ") "
+ " M: "
+ "("
+ CurPos.m[0]
+ ","
+ CurPos.m[1]
+ ") "
+ " g="
+ CurPos.g
+ " h="
+ CurPos.h
+ " f="
+ CurPos.f
+ " Speed="
+ CalcSpeed(CurPos.m[0], CurPos.m[1])
+ "\n");
SpeedsList.add(CalcSpeed(CurPos.m[0], CurPos.m[1]));
RoadGrid[CurPos.x][CurPos.y].type = "X";
CurPos = CurPos.parent;
}
// for(int i=0;i<height;i++)
// {
// int dy = Math.abs(i-CurPos.y);
// for(int j=0;j<width;j++)
// {
// out.write(RoadGrid[j][i].type);
// }
//
// out.write("\n");
// }
System.out.println(
"Step "
+ CurPos.g
+ " {P: "
+ "("
+ CurPos.x
+ ","
+ CurPos.y
+ ") "
+ " M: "
+ "("
+ CurPos.m[0]
+ ","
+ CurPos.m[1]
+ ") "
+ " g="
+ CurPos.g
+ " h="
+ CurPos.h
+ " f="
+ CurPos.f
+ " Speed="
+ CalcSpeed(CurPos.m[0], CurPos.m[1]));
System.out.println(
"Step "
+ 0
+ " {P: "
+ "("
+ start.x
+ ","
+ start.y
+ ") "
+ " M: "
+ "("
+ start.m[0]
+ ","
+ start.m[1]
+ ") "
+ " g="
+ 0
+ " h="
+ start.h
+ " f="
+ start.f
+ " Speed="
+ CalcSpeed(start.m[0], start.m[1]));
out.write(
"Step "
+ CurPos.g
+ " {P: "
+ "("
+ CurPos.x
+ ","
+ CurPos.y
+ ") "
+ " M: "
+ "("
+ CurPos.m[0]
+ ","
+ CurPos.m[1]
+ ") "
+ " g="
+ CurPos.g
+ " h="
+ CurPos.h
+ " f="
+ CurPos.f
+ " Speed="
+ CalcSpeed(CurPos.m[0], CurPos.m[1])
+ "\n");
out.write(
"Step "
+ 0
+ " {P: "
+ "("
+ start.x
+ ","
+ start.y
+ ") "
+ " M: "
+ "("
+ start.m[0]
+ ","
+ start.m[1]
+ ") "
+ " g="
+ 0
+ " h="
+ start.h
+ " f="
+ start.f
+ " Speed="
+ CalcSpeed(start.m[0], start.m[1])
+ "\n");
System.out.println("Number of Steps: " + numSteps);
System.out.println("Number of Expanded nodes: " + ClosedSet.size());
double avgSpeed = 0;
for (int i = 0; i < SpeedsList.size(); i++) {
avgSpeed += SpeedsList.get(i);
}
avgSpeed = avgSpeed / SpeedsList.size();
System.out.println("Average Speed: " + avgSpeed);
System.out.println("Runtime: " + (finishTime - startTime) / 1000 + " sec");
out.write("Number of Steps: " + numSteps + "\n");
out.write("Number of Expanded nodes: " + ClosedSet.size() + "\n");
out.write("Average Speed: " + avgSpeed + "\n");
out.write("Runtime: " + (finishTime - startTime) / 1000 + " sec" + "\n");
out.close();
} // End StartRace
/**
* Finds path based on starting and ending points
*
* @param oldPath Zombie's existing path
* @param startX Starting tile x coordinate
* @param startY Starting tile y coordinate
* @param targetX Target tile x coordinate
* @param targetY Target tile y coordinate
* @return
*/
public Stack<Node> findPath(
Stack<Node> oldPath, int startX, int startY, int targetX, int targetY) {
// If path has the same target do not recalculate path
if (!oldPath.isEmpty()
&& oldPath.get(0).locationX == targetX
&& oldPath.get(0).locationY == targetY) {
return oldPath;
} else {
// Reset path
this.path = new Stack<Node>();
openList.clear();
closedList.clear();
path.clear();
// In case zombie get pushed out of map and has not been updated
if (startY < 0 || startX < 0 || startY > tiles[0].length || startX > tiles.length)
return path;
// Set starting point
Node start = graph[startY][startX];
Node currentNode = start;
openList.add(start);
int depthOfSearch = 0;
// If target is unreachable return a empty path
if (tiles[targetY][targetX] == false)
// While the frontier is not empty and pathfinder has not
// exceeded the max number of checks
while (!openList.isEmpty() && depthOfSearch < MAX_DEPTH) {
depthOfSearch++;
// Remove next node from the queue and place into closed
// list;
currentNode = openList.peek();
openList.remove(currentNode);
closedList.add(currentNode);
// If current node is the destination create the path
if (currentNode.locationX == targetX && currentNode.locationY == targetY) {
while (currentNode != start) {
path.add(currentNode);
currentNode = currentNode.prev;
}
break;
}
// Add adjacent nodes to the frontier
for (int i = -1; i < 2; i++)
for (int j = -1; j < 2; j++) {
// Ignore if out of bounds
if (currentNode.locationY + i < 0
|| currentNode.locationX + j < 0
|| currentNode.locationY + i > tiles.length - 1
|| currentNode.locationX + j > tiles[0].length - 1) continue;
// Ignore if tile is solid
if (tiles[currentNode.locationY + i][currentNode.locationX + j] == true) continue;
// Ignore if current node
if (i == 0 && j == 0) continue;
Node nextNode = graph[currentNode.locationY + i][currentNode.locationX + j];
int add = 10;
// Diagonal case
if (i != 0 && j != 0) {
add = 14;
// If any of the two nodes are solid diagonal
// not possible (zombie would walk through a
// tile0
if (tiles[currentNode.locationY + i][currentNode.locationX] == true
|| tiles[currentNode.locationY][currentNode.locationX + j] == true) continue;
}
// If already on the closed list ignore
if (closedList.contains(nextNode)) continue;
int g = 0;
if (currentNode.prev != null) {
g = currentNode.g;
}
g += add;
// If in the open list movement cost if needed
if (openList.contains(nextNode)) {
// If old g is greater than old g, swap it out
if (nextNode.g > g) {
nextNode.g = g;
nextNode.setParent(currentNode);
;
}
// Else add to open list
} else {
// Set parent to current node
nextNode.setParent(currentNode);
nextNode.g = g;
// Calculate heuristic
nextNode.h =
Math.abs(nextNode.locationX - targetX) + Math.abs(nextNode.locationY - targetY);
openList.add(nextNode);
}
}
}
}
return path;
}
public boolean containsResource(T resource) {
return resources.contains(resource);
}
@Override
public boolean elementExists(Node node) {
return queue.contains(node);
}
@Override
public boolean contains(Object arg0) {
return data.containsKey(arg0) || queue.contains(arg0);
}
public static void main(String[] args) throws Exception {
// Creates the grid and prints it.
System.out.println("Enter the nn value");
nn = keyboard.nextInt();
System.out.println("Enter the mm value");
mm = keyboard.nextInt();
System.out.println("Enter the max cost range value");
maxCostRange = keyboard.nextInt();
System.out.println("Enter the p value");
p = keyboard.nextDouble();
// Ask the user which heuristic function they want to use.
do {
System.out.println("\nMake a choice");
System.out.println("-------------");
System.out.println("1-Heuristic Function (h1) = Straight-line distance");
System.out.println("2-Heuristic Function (h2) = Sum of displacement in x and y axes");
System.out.println("3-Heuristic Function (h3) = 0.5h1+0.5h2");
heuristicChoice = keyboard.nextInt();
} while (heuristicChoice != 1 && heuristicChoice != 2 && heuristicChoice != 3);
// Print the random maze generated.
System.out.print("\nRandom Maze");
System.out.println("\n-----------");
g = new GridProblem(nn, mm, maxCostRange, p);
g.print();
// Set the start and goal states.
startState = g.startState;
goalState = g.goalState;
startNode = new SearchNode(startState, null);
goalNode = new SearchNode(goalState, null);
startNode.setFscore(0 + getHCost(heuristicChoice, startNode));
open.offer(startNode);
stateToNode.put(startState, startNode);
// The while loop executes until we haven't reach the goal state and
// there are still search nodes in the priority queue.
while (!open.isEmpty()) {
// Get the first element from the priority queue.
crntNode = open.peek();
// We've reached the destination. We print the path leading to the goal node
// and return.
if (g.isGoal(crntNode.getGridState())) {
System.out.println("\nOutput");
System.out.println("------");
constructPath(crntNode);
return;
}
// Get the node that is visited next. The neighbors (those that can be reached with legal
// operations) of this node will be added to the queue as well.
crntNode = open.poll();
crntState = crntNode.getGridState();
closed.add(crntNode);
a = g.getLegalOps(crntNode.getGridState());
// Goes through all of the neighbors of the current node. The neighbors get added to the queue
// if they may potentially be on the optimal path to the goal node.
for (int i = 0; i < a.size(); i++) {
// Gets an operation to perform on the current state and gets the next state
// after this operation is performed.
op = a.get(i);
nxtState = g.nextState((State) crntState, op);
// If the next state returned is equal to the current state, then it must be a wall
// so we ignore this operation.
if (nxtState == crntState) {
continue;
}
// Check to see whether a search node has already been created for the current state.
nxtNode = stateToNode.get(nxtState);
if (nxtNode == null) {
nxtNode = new SearchNode(nxtState, crntNode);
stateToNode.put(nxtState, nxtNode);
}
// Compute a temporary gScore for the neighbor.
tmpGScore = crntNode.getgScore() + g.cost(crntState, op);
// Check to see whether the gScore for the neighbor is greater than
// the gScore calculated in the above operation. If it is, any search nodes
// present in the priority queue or list will have to be removed as it may
// potentially be on the optimal path.
if (tmpGScore < nxtNode.getgScore()) {
if (open.contains(nxtNode)) {
open.remove(nxtNode); // The given node can be reached from a more optimal path.
}
if (closed.contains(nxtNode)) {
closed.remove(
nxtNode); // A node already considered can be reached from a more optimal path.
}
}
// Adjust/set the cost of the neighbor and add it to the priority queue.
if (!open.contains(nxtNode) && !closed.contains(nxtNode)) {
nxtNode.setParent(crntNode); // Set the parent for the given node.
nxtNode.setOperator(op);
nxtNode.setgScore(tmpGScore); // Set the new cost to get to this node from the start node.
nxtNode.setFscore(
nxtNode.getgScore()
+ getHCost(
heuristicChoice,
nxtNode)); // Set the estimated cost to get from this node to the goal node.
open.offer(nxtNode); // Add the node to the priority queue.
}
}
}
// There are no more search nodes in the priority queue and we haven't reached the goal
// node yet. Thus, there is no valid path from the start node to the goal node.
System.out.println("No path exists!");
}
@SuppressWarnings("unchecked")
public static <NodeType extends AStarNode, GoalType extends Goal> List<NodeType> getBestPath(
NodeType begin, GoalType end, Heuristic<NodeType, GoalType> heuristic) {
Set<NodeType> closedSet;
PriorityQueue<NodeType> openSet;
if (log.isLoggable(Level.FINE)) {
log.fine("Starting Path Planning from " + begin + " for " + end);
log.fine("Hash: " + begin.hashCode());
}
// empty set for already explored cells
closedSet = new HashSet<>(100000);
openSet = new PriorityQueue<>(100000);
// in the "to explore" set there is in the beginning just the cell with which we begin
openSet.add(begin);
begin.g = 0;
begin.f = heuristic.getHeuristicValue(begin, end, null);
int loopCount = 0;
while (!openSet.isEmpty()) {
// if (openSet.size() > 10000) {
// Iterator<NodeType> it = openSet.iterator();
// int i = 0;
// while (it.hasNext() && (i++) < 6000)
// it.next();
// while (it.hasNext()){
// it.next();
// it.remove();
// }
//
// }
if (log.isLoggable(Level.INFO)) {
loopCount++;
if ((loopCount % 5000) == 0)
log.info("Expanded " + loopCount + " states. OpenSet size: " + openSet.size());
}
NodeType current = openSet.peek();
if (current.satisfiesGoal(end)) return AStarSearch.<NodeType>computePath(current);
closedSet.add(current);
openSet.remove(current);
for (AStarNode _entity : current.getNeighbours()) {
NodeType entity = (NodeType) _entity;
float tentativeScore = current.g + DISTANCE_ONE;
if (closedSet.contains(entity)) {
if (tentativeScore >= entity.g) {
continue;
}
}
if (!openSet.contains(entity) || tentativeScore < entity.g) {
// update partial score from start to neighbor cell
entity.g = tentativeScore;
// update estimated score till goal for neighbor cell
entity.f = entity.g + heuristic.getHeuristicValue(entity, end, current);
if (!openSet.contains(entity)) {
openSet.add(entity);
}
}
// TODO - re-think how to return path. Maybe using a HashMap(with Cell and parent)
}
}
return null;
}
/**
* Accessor for whether an element is contained in the Collection.
*
* @param element The element
* @return Whether the element is contained here
*/
public synchronized boolean contains(Object element) {
return delegate.contains(element);
}