protected double getG() {
if (g_score == 0) {
final JPSNode parent = came_from;
g_score = parent != null ? parent.getG() + parent.getMoveCost(this) : 0.0D;
}
return g_score;
}
@Override
public JPSNode clone() {
// TODO to override object clone() this should implement Cloneable.
// Otherwise it's better to call it something else. like copy().
JPSNode clone = new JPSNode(x, y);
clone.came_from = this;
return clone;
}
private boolean walkable(JPSNode node) {
if (node.equals(goal)) {
return !AbstractObstacle.isInsideObstacleList(obsList, node, 0)
&& shape.isInside(simulationDim, node);
} else {
return !AbstractObstacle.isInsideObstacleList(obsList, node, safetyDistance)
&& shape.isInside(simulationDim, node);
}
}
private JPSNode retrieveInstance(HashSet<JPSNode> openSet, JPSNode node) {
if (node == null) return null;
final Iterator<JPSNode> nI = openSet.iterator();
while (nI.hasNext()) {
final JPSNode n = nI.next();
if (node.equals(n)) return n;
}
return null;
}
public double getMoveCost(JPSNode node) {
if (node == null) return 0;
double x = node.getX(), y = node.getY(), px = getX(), py = getY();
double steps = getSteps(x, y, px, py);
return x == px || y == px ? steps : (double) steps * 1;
}
private JPSNode jump(JPSNode node, JPSNode parent, JPSNode goal) {
double x = node.getX(), y = node.getY(), px = parent.getX(), py = parent.getY();
double dx = (x - px);
double dy = (y - py);
// Log.e("Coordinatescaling: " + coordinateScaling);
// Log.e("dy: " + dy + " dx: " + dx);
// Log.e("JUMP! node: " + node.toString() + " parent: " + parent.toString() + " dx: " + dx + "
// dy: " + dy);
if (!walkable(node)) // check blocked
return null;
if (node.equals(goal)) // reached goal
return new JPSNode(node);
// resolve forced neighbors
JPSNode temp = new JPSNode(node);
if (((int) dx & (int) dy) != 0) { // diagonal
if ((walkable(temp.setPosition(x - dx, y + dy)) && !walkable(temp.setPosition(x - dx, y)))
|| (walkable(temp.setPosition(x + dx, y - dy))
&& !walkable(temp.setPosition(x, y - dy)))) {
return new JPSNode(node);
}
// recurse
JPSNode h = jump(node.derive(dx, 0), node, goal);
if (h != null) return new JPSNode(node);
JPSNode v = jump(node.derive(0, dy), node, goal);
if (v != null) return new JPSNode(node);
} else if (dx == 0) { // vertical, dx = 0, dy = 1 or -1
if ((walkable(temp.setPosition(x + coordinateScaling, y + dy))
&& !walkable(temp.setPosition(x + coordinateScaling, y)))
|| (walkable(temp.setPosition(x - coordinateScaling, y + dy))
&& !walkable(temp.setPosition(x - coordinateScaling, y)))) {
return new JPSNode(node);
}
} else { // horizontal, dx = 1 or -1, dy = 0
if ((walkable(temp.setPosition(x + dx, y + coordinateScaling))
&& !walkable(temp.setPosition(x, y + coordinateScaling)))
|| (walkable(temp.setPosition(x + dx, y - coordinateScaling))
&& !walkable(temp.setPosition(x, y - coordinateScaling)))) {
return new JPSNode(node);
}
}
return jump(node.derive(dx, dy), node, goal);
}
private JPSNode[] getJPSNeighbours(JPSNode current, List<IObstacle> obsList) {
double x = current.getX();
double y = current.getY();
LinkedList<JPSNode> nodes = new LinkedList<JPSNode>();
JPSNode parent = current.came_from;
if (parent != null) { // determines whether to prune neighbors
JPSNode norm = normalizeDirection(x, y, parent.getX(), parent.getY());
double dx = norm.getX() * coordinateScaling;
double dy = norm.getY() * coordinateScaling;
JPSNode temp = new JPSNode(current);
if (((int) dx & (int) dy) != 0) { // diagonal direction
// check straight directions in the direction of the diagonal move
if (walkable(temp.setPosition(x, y + dy))) nodes.add(new JPSNode(temp));
if (walkable(temp.setPosition(x + dx, y))) nodes.add(new JPSNode(temp));
if (walkable(temp.setPosition(x + dx, y + dy))) nodes.add(new JPSNode(temp));
// forced neighbor checks
if (!walkable(temp.setPosition(x - dx, y))) nodes.add(new JPSNode(temp.shift(0, dy)));
if (!walkable(temp.setPosition(x, y - dy))) nodes.add(new JPSNode(temp.shift(dx, 0)));
} else { // straight direction
if (walkable(temp.setPosition(x + dx, y + dy))) {
nodes.add(new JPSNode(temp));
// forced neighbor checks
if (!walkable(temp.setPosition(x + dy, y + dx)))
nodes.add(new JPSNode(temp.shift(dx, dy)));
if (!walkable(temp.setPosition(x - dy, y - dx)))
nodes.add(new JPSNode(temp.shift(dx, dy)));
}
}
} else {
// no parent, return all that aren't blocked
JPSNode[] ns =
new JPSNode[] {
new JPSNode(x, y - coordinateScaling),
new JPSNode(x + coordinateScaling, y - coordinateScaling),
new JPSNode(x + coordinateScaling, y),
new JPSNode(x + coordinateScaling, y + coordinateScaling),
new JPSNode(x, y + coordinateScaling),
new JPSNode(x - coordinateScaling, y + coordinateScaling),
new JPSNode(x - coordinateScaling, y),
new JPSNode(x - coordinateScaling, y - coordinateScaling)
};
for (int i = 0; i < ns.length; i++) {
if (walkable(ns[i])) nodes.add(ns[i]);
}
}
return nodes.toArray(new JPSNode[nodes.size()]);
}
public List<Position> getShortestPathJumpPointsSearch(
Position startPos, Position endPos, double safetyDistance) {
this.safetyDistance = safetyDistance;
JPSNode start = new JPSNode(startPos.getX(), startPos.getY());
goal = new JPSNode(endPos.getX(), endPos.getY());
HashSet<JPSNode> closedSet = new HashSet<JPSNode>();
HashSet<JPSNode> openSet = new HashSet<JPSNode>();
openSet.add(start);
JPSNode current = start;
double lowScore = Integer.MAX_VALUE;
while (!openSet.isEmpty()) {
lowScore = Integer.MAX_VALUE;
for (JPSNode n : openSet) {
double score = n.g_score;
if (score < lowScore) {
current = n;
lowScore = score;
} else if (score == lowScore) {
// TODO (==) is less good for float equality
// if(Math.abs(score - lowScore) < epsilon) // epsilon should be 0.000001 something
if (n != current) {
current = n;
}
}
}
// Log.v("openset" + openSet);
//// Log.v("closedSet" + closedSet);
//// Log.v("g_score" + current.g_score);
//// Log.v("f_score" + current.f_score);
// Log.v("current: " + current);
// Log.v("--------");
if (current.equals(goal)) {
return reconstructPath(current);
}
openSet.remove(current);
closedSet.add(current);
for (JPSNode neighbour : getJPSNeighbours(current, obsList)) {
JPSNode jumpPoint = jump(neighbour, current, goal);
if (jumpPoint != null) {
if (closedSet.contains(jumpPoint)) continue;
if (!openSet.contains(jumpPoint)) {
jumpPoint.came_from = current;
openSet.add(jumpPoint);
} else if ((current.getG() + current.getMoveCost(jumpPoint))
< jumpPoint.getG()) { // G score of node with current node as it's parent
JPSNode instanceNode = retrieveInstance(openSet, jumpPoint);
if (instanceNode != null) instanceNode.came_from = current;
}
}
}
}
Log.e(
"Failed to find path to target! Start: "
+ start
+ " End: "
+ endPos
+ " obsList: "
+ obsList
+ " Dimension: "
+ simulationDim
+ " Shape: "
+ shape);
return null;
}
