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