/*
* Initialise Method
* @params start and goal coordinates
*/
public void init(int sX, int sY, int gX, int gY) {
cellHash.clear();
path.clear();
openHash.clear();
while (!openList.isEmpty()) openList.poll();
k_m = 0;
s_start.x = sX;
s_start.y = sY;
s_goal.x = gX;
s_goal.y = gY;
CellInfo tmp = new CellInfo();
tmp.g = 0;
tmp.rhs = 0;
tmp.cost = C1;
cellHash.put(s_goal, tmp);
tmp = new CellInfo();
tmp.g = tmp.rhs = heuristic(s_start, s_goal);
tmp.cost = C1;
cellHash.put(s_start, tmp);
s_start = calculateKey(s_start);
s_last = s_start;
}
/*
* Checks if a cell is in the hash table, if not it adds it in.
*/
private void makeNewCell(State u) {
if (cellHash.get(u) != null) return;
CellInfo tmp = new CellInfo();
tmp.g = tmp.rhs = heuristic(u, s_goal);
tmp.cost = C1;
cellHash.put(u, tmp);
}
/*
* This is somewhat of a hack, to change the position of the goal we
* first save all of the non-empty nodes on the map, clear the map, move the
* goal and add re-add all of the non-empty cells. Since most of these cells
* are not between the start and goal this does not seem to hurt performance
* too much. Also, it frees up a good deal of memory we are probably not
* going to use.
*/
public void updateGoal(int x, int y) {
List<Pair<ipoint2, Double>> toAdd = new ArrayList<Pair<ipoint2, Double>>();
Pair<ipoint2, Double> tempPoint;
for (Map.Entry<State, CellInfo> entry : cellHash.entrySet()) {
if (!close(entry.getValue().cost, C1)) {
tempPoint =
new Pair(new ipoint2(entry.getKey().x, entry.getKey().y), entry.getValue().cost);
toAdd.add(tempPoint);
}
}
cellHash.clear();
openHash.clear();
while (!openList.isEmpty()) openList.poll();
k_m = 0;
s_goal.x = x;
s_goal.y = y;
CellInfo tmp = new CellInfo();
tmp.g = tmp.rhs = 0;
tmp.cost = C1;
cellHash.put(s_goal, tmp);
tmp = new CellInfo();
tmp.g = tmp.rhs = heuristic(s_start, s_goal);
tmp.cost = C1;
cellHash.put(s_start, tmp);
s_start = calculateKey(s_start);
s_last = s_start;
Iterator<Pair<ipoint2, Double>> iterator = toAdd.iterator();
while (iterator.hasNext()) {
tempPoint = iterator.next();
updateCell(tempPoint.first().x, tempPoint.first().y, tempPoint.second());
}
}