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;
}
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);
}
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;
}