com.badlogic.gdx.ai.pfa.indexed.IndexedAStarPathFinder Maven / Gradle / Ivy
/*******************************************************************************
* Copyright 2014 See AUTHORS file.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
******************************************************************************/
package com.badlogic.gdx.ai.pfa.indexed;
import com.badlogic.gdx.ai.pfa.Connection;
import com.badlogic.gdx.ai.pfa.GraphPath;
import com.badlogic.gdx.ai.pfa.Heuristic;
import com.badlogic.gdx.ai.pfa.PathFinder;
import com.badlogic.gdx.ai.pfa.PathFinderQueue;
import com.badlogic.gdx.ai.pfa.PathFinderRequest;
import org.mini2Dx.gdx.utils.Array;
import org.mini2Dx.gdx.utils.TimeUtils;
/** A fully implemented {@link PathFinder} that can perform both interruptible and non-interruptible pathfinding.
*
* This implementation is a common variation of the A* algorithm that is faster than the general A*.
*
* In the general A* implementation, data are held for each node in the open or closed lists, and these data are held as a
* NodeRecord instance. Records are created when a node is first considered and then moved between the open and closed lists, as
* required. There is a key step in the algorithm where the lists are searched for a node record corresponding to a particular
* node. This operation is something time-consuming.
*
* The indexed A* algorithm improves execution speed by using an array of all the node records for every node in the graph. Nodes
* must be numbered using sequential integers (see {@link IndexedGraph#getIndex(Object)}), so we don't need to search for a node in the
* two lists at all. We can simply use the node index to look up its record in the array (creating it if it is missing). This
* means that the close list is no longer needed. To know whether a node is open or closed, we use the {@link NodeRecord#category
* category} of the node record. This makes the search step very fast indeed (in fact, there is no search, and we can go straight
* to the information we need). Unfortunately, we can't get rid of the open list because we still need to be able to retrieve the
* element with the lowest cost. However, we use a {@link BinaryHeap} for the open list in order to keep performance as high as
* possible.
*
* @param Type of node
*
* @author davebaol */
public class IndexedAStarPathFinder implements PathFinder {
IndexedGraph graph;
NodeRecord[] nodeRecords;
BinaryHeap> openList;
NodeRecord current;
int resumeIndex = -1;
public Metrics metrics;
/** The unique ID for each search run. Used to mark nodes. */
private int searchId;
static final int UNVISITED = 0;
static final int OPEN = 1;
static final int CLOSED = 2;
private final NodeRecordPool nodeRecordPool;
public IndexedAStarPathFinder (IndexedGraph graph) {
this(graph, false);
}
@SuppressWarnings("unchecked")
public IndexedAStarPathFinder (IndexedGraph graph, boolean calculateMetrics) {
this(graph, null, calculateMetrics);
}
public IndexedAStarPathFinder (IndexedGraph graph, NodeRecordPool nodeRecordPool, boolean calculateMetrics) {
this.graph = graph;
this.nodeRecords = (NodeRecord[])new NodeRecord[graph.getNodeCount()];
this.openList = new BinaryHeap>();
this.nodeRecordPool = nodeRecordPool;
if (calculateMetrics) this.metrics = new Metrics();
}
private void resetNodeRecords() {
for(int i = 0; i < nodeRecords.length; i++) {
if(nodeRecords[i] == null) {
continue;
}
if(nodeRecordPool != null) {
nodeRecordPool.release(nodeRecords[i]);
}
nodeRecords[i] = null;
}
}
public void dispose() {
resetNodeRecords();
graph = null;
nodeRecords = null;
openList = null;
current = null;
metrics = null;
}
@Override
public boolean searchConnectionPath (N startNode, N endNode, Heuristic heuristic, GraphPath> outPath) {
// Perform AStar
boolean found = search(startNode, endNode, heuristic);
if (found) {
// Create a path made of connections
generateConnectionPath(startNode, outPath);
}
return found;
}
@Override
public boolean searchNodePath (N startNode, N endNode, Heuristic heuristic, GraphPath outPath) {
// Perform AStar
boolean found = search(startNode, endNode, heuristic);
if (found) {
// Create a path made of nodes
generateNodePath(startNode, outPath);
}
return found;
}
protected boolean search (N startNode, N endNode, Heuristic heuristic) {
initSearch(startNode, endNode, heuristic);
// Iterate through processing each node
do {
// Retrieve the node with smallest estimated total cost from the open list
current = openList.pop();
current.category = CLOSED;
// Terminate if we reached the goal node
if (current.node == endNode) return true;
visitChildren(endNode, heuristic, 0, Long.MAX_VALUE);
} while (openList.size > 0);
// We've run out of nodes without finding the goal, so there's no solution
return false;
}
@Override
public boolean search (PathFinderRequest request, long timeToRun) {
long startTime = TimeUtils.nanoTime();
// We have to initialize the search if the status has just changed
if (request.statusChanged) {
initSearch(request.startNode, request.endNode, request.heuristic);
request.statusChanged = false;
resumeIndex = -1;
}
// Iterate through processing each node
do {
// Check the available time
if (timeToRun <= PathFinderQueue.TIME_TOLERANCE) {
return false;
}
if(resumeIndex > -1) {
if(visitChildren(request.endNode, request.heuristic, resumeIndex, timeToRun)) {
resumeIndex = -1;
} else {
return false;
}
} else {
// Retrieve the node with smallest estimated total cost from the open list
current = openList.pop();
current.category = CLOSED;
// Terminate if we reached the goal node; we've found a path.
if (current.node == request.endNode) {
request.pathFound = true;
generateNodePath(request.startNode, request.resultPath);
resumeIndex = -1;
return true;
}
// Visit current node's children
if(visitChildren(request.endNode, request.heuristic, resumeIndex, timeToRun)) {
resumeIndex = -1;
} else {
return false;
}
}
long currentTime = TimeUtils.nanoTime();
timeToRun -= currentTime - startTime;
// Store the current time
startTime = currentTime;
} while (openList.size > 0);
// The open list is empty and we've not found a path.
request.pathFound = false;
return true;
}
protected void initSearch (N startNode, N endNode, Heuristic heuristic) {
if (metrics != null) metrics.reset();
// Increment the search id
if (++searchId < 0) searchId = 1;
resetNodeRecords();
// Initialize the open list
openList.clear();
// Initialize the record for the start node and add it to the open list
NodeRecord startRecord = getNodeRecord(startNode);
startRecord.node = startNode;
startRecord.connection = null;
startRecord.costSoFar = 0;
addToOpenList(startRecord, heuristic.estimate(startNode, endNode));
current = null;
}
protected boolean visitChildren (N endNode, Heuristic heuristic, int resumeIndex, long timeToRun) {
// Get current node's outgoing connections
Array> connections = graph.getConnections(current.node);
long startTime = TimeUtils.nanoTime();
if(resumeIndex < 0) {
resumeIndex = 0;
}
// Loop through each connection in turn
for (; resumeIndex < connections.size; resumeIndex++) {
if (metrics != null) {
metrics.visitedNodes++;
}
if (timeToRun <= PathFinderQueue.TIME_TOLERANCE) {
return false;
}
Connection connection = connections.get(resumeIndex);
// Get the cost estimate for the node
N node = connection.getToNode();
float nodeCost = current.costSoFar + connection.getCost();
float nodeHeuristic;
NodeRecord nodeRecord = getNodeRecord(node);
if (nodeRecord.category == CLOSED) { // The node is closed
// If we didn't find a shorter route, skip
if (nodeRecord.costSoFar <= nodeCost) continue;
// We can use the node's old cost values to calculate its heuristic
// without calling the possibly expensive heuristic function
nodeHeuristic = nodeRecord.getEstimatedTotalCost() - nodeRecord.costSoFar;
} else if (nodeRecord.category == OPEN) { // The node is open
// If our route is no better, then skip
if (nodeRecord.costSoFar <= nodeCost) continue;
// Remove it from the open list (it will be re-added with the new cost)
openList.remove(nodeRecord);
// We can use the node's old cost values to calculate its heuristic
// without calling the possibly expensive heuristic function
nodeHeuristic = nodeRecord.getEstimatedTotalCost() - nodeRecord.costSoFar;
} else { // the node is unvisited
// We'll need to calculate the heuristic value using the function,
// since we don't have a node record with a previously calculated value
nodeHeuristic = heuristic.estimate(node, endNode);
}
// Update node record's cost and connection
nodeRecord.costSoFar = nodeCost;
nodeRecord.connection = connection;
// Add it to the open list with the estimated total cost
addToOpenList(nodeRecord, nodeCost + nodeHeuristic);
long currentTime = TimeUtils.nanoTime();
timeToRun -= currentTime - startTime;
// Store the current time
startTime = currentTime;
}
return true;
}
protected void generateConnectionPath (N startNode, GraphPath> outPath) {
// Work back along the path, accumulating connections
// outPath.clear();
while (current.node != startNode) {
outPath.add(current.connection);
current = nodeRecords[graph.getIndex(current.connection.getFromNode())];
}
// Reverse the path
outPath.reverse();
}
protected void generateNodePath (N startNode, GraphPath outPath) {
// Work back along the path, accumulating nodes
// outPath.clear();
while (current.connection != null) {
outPath.add(current.node);
current = nodeRecords[graph.getIndex(current.connection.getFromNode())];
}
outPath.add(startNode);
// Reverse the path
outPath.reverse();
}
protected void addToOpenList (NodeRecord nodeRecord, float estimatedTotalCost) {
openList.add(nodeRecord, estimatedTotalCost);
nodeRecord.category = OPEN;
if (metrics != null) {
metrics.openListAdditions++;
metrics.openListPeak = Math.max(metrics.openListPeak, openList.size);
}
}
protected NodeRecord getNodeRecord (N node) {
int index = graph.getIndex(node);
NodeRecord nr = nodeRecords[index];
if (nr != null) {
if (nr.searchId != searchId) {
nr.category = UNVISITED;
nr.searchId = searchId;
}
return nr;
}
final NodeRecord nodeRecord;
if(nodeRecordPool == null) {
nodeRecord = new NodeRecord();
} else {
nodeRecord = nodeRecordPool.allocate();
}
nr = nodeRecords[index] = nodeRecord;
nr.node = node;
nr.searchId = searchId;
return nr;
}
/** A class used by {@link IndexedAStarPathFinder} to collect search metrics.
*
* @author davebaol */
public static class Metrics {
public int visitedNodes;
public int openListAdditions;
public int openListPeak;
public Metrics () {
}
public void reset () {
visitedNodes = 0;
openListAdditions = 0;
openListPeak = 0;
}
}
}