/*
* Returns the length of the longest edge within the specified graph.
*/
private double maxEdgeLength(Graph graph) {
double maxLength = -1;
for (int i = 0; i < graph.numberOfNodes(); i++) {
for (int neighbour : graph.neighboursOf(i)) {
if (graph.getEdge(i, neighbour) > maxLength) {
maxLength = graph.getEdge(i, neighbour);
}
}
}
return Math.max(1, maxLength);
}
/** {@inheritDoc} */
@Override
public Route calculateRoute(Point from, Point to, Graph mapGraph) {
if (from == null || to == null || mapGraph == null) {
throw new NullPointerException();
}
int fromIndex = mapGraph.getNodeIndex(from);
if (fromIndex == -1) {
throw new IllegalArgumentException("from has to be a node within mapGraph");
}
AddressableRadixHeap<Node> nodeQueue =
new AddressableRadixHeap<Node>((int) maxEdgeLength(mapGraph));
GenericArray<AddressableRadixHeap<Node>.Handle> handles =
new GenericArray<AddressableRadixHeap<Node>.Handle>(mapGraph.numberOfNodes());
handles.set(fromIndex, nodeQueue.insert(new Node(fromIndex, null), 0));
while (!nodeQueue.isEmpty()
&& !mapGraph.getNode(nodeQueue.min().getElement().index).equals(to)) {
AddressableRadixHeap<Node>.Handle activeHandle = nodeQueue.min();
for (int neighbour : mapGraph.neighboursOf(activeHandle.getElement().index)) {
int distance =
activeHandle.getKey()
+ (int) (mapGraph.getEdge(activeHandle.getElement().index, neighbour));
if (handles.get(neighbour) == null) {
handles.set(
neighbour,
nodeQueue.insert(new Node(neighbour, activeHandle.getElement()), distance));
} else if (handles.get(neighbour).getKey() > distance) {
handles.get(neighbour).getElement().parent = activeHandle.getElement();
nodeQueue.decreaseKey(handles.get(neighbour), distance);
}
}
nodeQueue.deleteMin();
}
LinkedList<Point> route = new LinkedList<Point>();
if (!nodeQueue.isEmpty() && mapGraph.getNode(nodeQueue.min().getElement().index).equals(to)) {
Node routeNode = nodeQueue.min().getElement();
while (routeNode != null) {
route.addFirst(mapGraph.getNode(routeNode.index));
routeNode = routeNode.parent;
}
}
return new Route(route);
}