/*
* Licensed to GraphHopper GmbH under one or more contributor
* license agreements. See the NOTICE file distributed with this work for
* additional information regarding copyright ownership.
*
* GraphHopper GmbH licenses this file to you 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.graphhopper.routing;
import com.carrotsearch.hppc.IntArrayList;
import com.carrotsearch.hppc.IntObjectMap;
import com.carrotsearch.hppc.predicates.IntObjectPredicate;
import com.carrotsearch.hppc.procedures.IntProcedure;
import com.graphhopper.coll.GHIntHashSet;
import com.graphhopper.coll.GHIntObjectHashMap;
import com.graphhopper.routing.util.AllEdgesIterator;
import com.graphhopper.routing.util.DefaultEdgeFilter;
import com.graphhopper.routing.util.EdgeFilter;
import com.graphhopper.storage.Graph;
import com.graphhopper.storage.GraphExtension;
import com.graphhopper.storage.NodeAccess;
import com.graphhopper.storage.TurnCostExtension;
import com.graphhopper.storage.index.QueryResult;
import com.graphhopper.util.*;
import com.graphhopper.util.shapes.BBox;
import com.graphhopper.util.shapes.GHPoint;
import com.graphhopper.util.shapes.GHPoint3D;
import java.util.*;
/**
* A class which is used to query the underlying graph with real GPS points. It does so by
* introducing virtual nodes and edges. It is lightweight in order to be created every time a new
* query comes in, which makes the behaviour thread safe.
* <p>
* Calling any <tt>lookup</tt> method creates virtual edges between the tower nodes of the existing
* graph and new virtual tower nodes. Every virtual node has two adjacent nodes and is connected
* to each adjacent nodes via 2 virtual edges with opposite base node / adjacent node encoding.
* However, the edge explorer returned by {@link #createEdgeExplorer()} only returns two
* virtual edges per virtual node (the ones with correct base node).
*
* @author Peter Karich
*/
public class QueryGraph implements Graph {
final static int VE_BASE = 0, VE_BASE_REV = 1, VE_ADJ = 2, VE_ADJ_REV = 3;
private static final AngleCalc AC = Helper.ANGLE_CALC;
private final Graph mainGraph;
private final NodeAccess mainNodeAccess;
private final int mainNodes;
private final int mainEdges;
private final QueryGraph baseGraph;
private final GraphExtension wrappedExtension;
// TODO when spreading it on different threads we need multiple independent explorers
private final Map<Integer, EdgeExplorer> cacheMap = new HashMap<Integer, EdgeExplorer>(4);
// For every virtual node there are 4 edges: base-snap, snap-base, snap-adj, adj-snap.
List<VirtualEdgeIteratorState> virtualEdges;
private List<QueryResult> queryResults;
/**
* Store lat,lon of virtual tower nodes.
*/
private PointList virtualNodes;
private final NodeAccess nodeAccess = new NodeAccess() {
@Override
public void ensureNode(int nodeId) {
mainNodeAccess.ensureNode(nodeId);
}
@Override
public boolean is3D() {
return mainNodeAccess.is3D();
}
@Override
public int getDimension() {
return mainNodeAccess.getDimension();
}
@Override
public double getLatitude(int nodeId) {
if (isVirtualNode(nodeId))
return virtualNodes.getLatitude(nodeId - mainNodes);
return mainNodeAccess.getLatitude(nodeId);
}
@Override
public double getLongitude(int nodeId) {
if (isVirtualNode(nodeId))
return virtualNodes.getLongitude(nodeId - mainNodes);
return mainNodeAccess.getLongitude(nodeId);
}
@Override
public double getElevation(int nodeId) {
if (isVirtualNode(nodeId))
return virtualNodes.getElevation(nodeId - mainNodes);
return mainNodeAccess.getElevation(nodeId);
}
@Override
public int getAdditionalNodeField(int nodeId) {
if (isVirtualNode(nodeId))
return 0;
return mainNodeAccess.getAdditionalNodeField(nodeId);
}
@Override
public void setNode(int nodeId, double lat, double lon) {
throw new UnsupportedOperationException("Not supported yet.");
}
@Override
public void setNode(int nodeId, double lat, double lon, double ele) {
throw new UnsupportedOperationException("Not supported yet.");
}
@Override
public void setAdditionalNodeField(int nodeId, int additionalValue) {
throw new UnsupportedOperationException("Not supported yet.");
}
@Override
public double getLat(int nodeId) {
return getLatitude(nodeId);
}
@Override
public double getLon(int nodeId) {
return getLongitude(nodeId);
}
@Override
public double getEle(int nodeId) {
return getElevation(nodeId);
}
};
// Use LinkedHashSet for predictable iteration order.
private final Set<VirtualEdgeIteratorState> unfavoredEdges = new LinkedHashSet<>(5);
private boolean useEdgeExplorerCache = false;
public QueryGraph(Graph graph) {
mainGraph = graph;
mainNodeAccess = graph.getNodeAccess();
mainNodes = graph.getNodes();
mainEdges = graph.getAllEdges().getMaxId();
if (mainGraph.getExtension() instanceof TurnCostExtension)
wrappedExtension = new QueryGraphTurnExt();
else
wrappedExtension = mainGraph.getExtension();
// create very lightweight QueryGraph which uses variables from this QueryGraph (same virtual edges)
baseGraph = new QueryGraph(graph.getBaseGraph(), this) {
// override method to avoid stackoverflow
@Override
public QueryGraph setUseEdgeExplorerCache(boolean useEECache) {
baseGraph.useEdgeExplorerCache = useEECache;
return baseGraph;
}
};
}
/**
* See 'lookup' for further variables that are initialized
*/
private QueryGraph(Graph graph, QueryGraph superQueryGraph) {
mainGraph = graph;
baseGraph = this;
wrappedExtension = superQueryGraph.wrappedExtension;
mainNodeAccess = graph.getNodeAccess();
mainNodes = superQueryGraph.mainNodes;
mainEdges = superQueryGraph.mainEdges;
}
/**
* Convenient method to initialize this QueryGraph with the two specified query results.
*
* @see #lookup(List)
*/
public QueryGraph lookup(QueryResult fromRes, QueryResult toRes) {
List<QueryResult> results = new ArrayList<QueryResult>(2);
results.add(fromRes);
results.add(toRes);
lookup(results);
return this;
}
/**
* For all specified query results calculate snapped point and if necessary set closest node
* to a virtual one and reverse closest edge. Additionally the wayIndex can change if an edge is
* swapped.
*
* @see QueryGraph
*/
public void lookup(List<QueryResult> resList) {
if (isInitialized())
throw new IllegalStateException("Call lookup only once. Otherwise you'll have problems for queries sharing the same edge.");
// initialize all none-final variables
virtualEdges = new ArrayList<VirtualEdgeIteratorState>(resList.size() * 2);
virtualNodes = new PointList(resList.size(), mainNodeAccess.is3D());
queryResults = new ArrayList<QueryResult>(resList.size());
baseGraph.virtualEdges = virtualEdges;
baseGraph.virtualNodes = virtualNodes;
baseGraph.queryResults = queryResults;
GHIntObjectHashMap<List<QueryResult>> edge2res = new GHIntObjectHashMap<List<QueryResult>>(resList.size());
// Phase 1
// calculate snapped point and swap direction of closest edge if necessary
for (QueryResult res : resList) {
// Do not create virtual node for a query result if it is directly on a tower node or not found
if (res.getSnappedPosition() == QueryResult.Position.TOWER)
continue;
EdgeIteratorState closestEdge = res.getClosestEdge();
if (closestEdge == null)
throw new IllegalStateException("Do not call QueryGraph.lookup with invalid QueryResult " + res);
int base = closestEdge.getBaseNode();
// Force the identical direction for all closest edges.
// It is important to sort multiple results for the same edge by its wayIndex
boolean doReverse = base > closestEdge.getAdjNode();
if (base == closestEdge.getAdjNode()) {
// check for special case #162 where adj == base and force direction via latitude comparison
PointList pl = closestEdge.fetchWayGeometry(0);
if (pl.size() > 1)
doReverse = pl.getLatitude(0) > pl.getLatitude(pl.size() - 1);
}
if (doReverse) {
closestEdge = closestEdge.detach(true);
PointList fullPL = closestEdge.fetchWayGeometry(3);
res.setClosestEdge(closestEdge);
if (res.getSnappedPosition() == QueryResult.Position.PILLAR)
// ON pillar node
res.setWayIndex(fullPL.getSize() - res.getWayIndex() - 1);
else
// for case "OFF pillar node"
res.setWayIndex(fullPL.getSize() - res.getWayIndex() - 2);
if (res.getWayIndex() < 0)
throw new IllegalStateException("Problem with wayIndex while reversing closest edge:" + closestEdge + ", " + res);
}
// find multiple results on same edge
int edgeId = closestEdge.getEdge();
List<QueryResult> list = edge2res.get(edgeId);
if (list == null) {
list = new ArrayList<QueryResult>(5);
edge2res.put(edgeId, list);
}
list.add(res);
}
// Phase 2 - now it is clear which points cut one edge
// 1. create point lists
// 2. create virtual edges between virtual nodes and its neighbor (virtual or normal nodes)
edge2res.forEach(new IntObjectPredicate<List<QueryResult>>() {
@Override
public boolean apply(int edgeId, List<QueryResult> results) {
// we can expect at least one entry in the results
EdgeIteratorState closestEdge = results.get(0).getClosestEdge();
final PointList fullPL = closestEdge.fetchWayGeometry(3);
int baseNode = closestEdge.getBaseNode();
// sort results on the same edge by the wayIndex and if equal by distance to pillar node
Collections.sort(results, new Comparator<QueryResult>() {
@Override
public int compare(QueryResult o1, QueryResult o2) {
int diff = o1.getWayIndex() - o2.getWayIndex();
if (diff == 0) {
// sort by distance from snappedPoint to fullPL.get(wayIndex) if wayIndex is identical
GHPoint p1 = o1.getSnappedPoint();
GHPoint p2 = o2.getSnappedPoint();
if (p1.equals(p2))
return 0;
double fromLat = fullPL.getLatitude(o1.getWayIndex());
double fromLon = fullPL.getLongitude(o1.getWayIndex());
if (Helper.DIST_PLANE.calcNormalizedDist(fromLat, fromLon, p1.lat, p1.lon)
> Helper.DIST_PLANE.calcNormalizedDist(fromLat, fromLon, p2.lat, p2.lon))
return 1;
return -1;
}
return diff;
}
});
GHPoint3D prevPoint = fullPL.toGHPoint(0);
int adjNode = closestEdge.getAdjNode();
int origTraversalKey = GHUtility.createEdgeKey(baseNode, adjNode, closestEdge.getEdge(), false);
int origRevTraversalKey = GHUtility.createEdgeKey(baseNode, adjNode, closestEdge.getEdge(), true);
long reverseFlags = closestEdge.detach(true).getFlags();
int prevWayIndex = 1;
int prevNodeId = baseNode;
int virtNodeId = virtualNodes.getSize() + mainNodes;
boolean addedEdges = false;
// Create base and adjacent PointLists for all none-equal virtual nodes.
// We do so via inserting them at the correct position of fullPL and cutting the
// fullPL into the right pieces.
for (int counter = 0; counter < results.size(); counter++) {
QueryResult res = results.get(counter);
if (res.getClosestEdge().getBaseNode() != baseNode)
throw new IllegalStateException("Base nodes have to be identical but were not: " + closestEdge + " vs " + res.getClosestEdge());
GHPoint3D currSnapped = res.getSnappedPoint();
// no new virtual nodes if exactly the same snapped point
if (prevPoint.equals(currSnapped)) {
res.setClosestNode(prevNodeId);
continue;
}
queryResults.add(res);
createEdges(origTraversalKey, origRevTraversalKey,
prevPoint, prevWayIndex,
res.getSnappedPoint(), res.getWayIndex(),
fullPL, closestEdge, prevNodeId, virtNodeId, reverseFlags);
virtualNodes.add(currSnapped.lat, currSnapped.lon, currSnapped.ele);
// add edges again to set adjacent edges for newVirtNodeId
if (addedEdges) {
virtualEdges.add(virtualEdges.get(virtualEdges.size() - 2));
virtualEdges.add(virtualEdges.get(virtualEdges.size() - 2));
}
addedEdges = true;
res.setClosestNode(virtNodeId);
prevNodeId = virtNodeId;
prevWayIndex = res.getWayIndex() + 1;
prevPoint = currSnapped;
virtNodeId++;
}
// two edges between last result and adjacent node are still missing if not all points skipped
if (addedEdges)
createEdges(origTraversalKey, origRevTraversalKey,
prevPoint, prevWayIndex,
fullPL.toGHPoint(fullPL.getSize() - 1), fullPL.getSize() - 2,
fullPL, closestEdge, virtNodeId - 1, adjNode, reverseFlags);
return true;
}
});
}
@Override
public Graph getBaseGraph() {
// Note: if the mainGraph of this QueryGraph is a CHGraph then ignoring the shortcuts will produce a
// huge gap of edgeIds between base and virtual edge ids. The only solution would be to move virtual edges
// directly after normal edge ids which is ugly as we limit virtual edges to N edges and waste memory or make everything more complex.
return baseGraph;
}
public EdgeIteratorState getOriginalEdgeFromVirtNode(int nodeId) {
return queryResults.get(nodeId - mainNodes).getClosestEdge();
}
public boolean isVirtualEdge(int edgeId) {
return edgeId >= mainEdges;
}
public boolean isVirtualNode(int nodeId) {
return nodeId >= mainNodes;
}
/**
* This method is an experimental feature to reduce memory and CPU resources if there are many
* locations ("hundreds") for one QueryGraph. It can make problems for custom or threaded
* algorithms or when using custom EdgeFilters for EdgeExplorer creation. Another limitation is
* that the same edge explorer is used even if a different vehicle/flagEncoder is chosen.
* Currently we can cache only the ALL_EDGES filter or instances of the DefaultEdgeFilter where
* three edge explorers will be created: forward OR backward OR both.
*/
public QueryGraph setUseEdgeExplorerCache(boolean useEECache) {
this.useEdgeExplorerCache = useEECache;
this.baseGraph.setUseEdgeExplorerCache(useEECache);
return this;
}
private void createEdges(int origTraversalKey, int origRevTraversalKey,
GHPoint3D prevSnapped, int prevWayIndex, GHPoint3D currSnapped, int wayIndex,
PointList fullPL, EdgeIteratorState closestEdge,
int prevNodeId, int nodeId, long reverseFlags) {
int max = wayIndex + 1;
// basePoints must have at least the size of 2 to make sure fetchWayGeometry(3) returns at least 2
PointList basePoints = new PointList(max - prevWayIndex + 1, mainNodeAccess.is3D());
basePoints.add(prevSnapped.lat, prevSnapped.lon, prevSnapped.ele);
for (int i = prevWayIndex; i < max; i++) {
basePoints.add(fullPL, i);
}
basePoints.add(currSnapped.lat, currSnapped.lon, currSnapped.ele);
PointList baseReversePoints = basePoints.clone(true);
double baseDistance = basePoints.calcDistance(Helper.DIST_PLANE);
int virtEdgeId = mainEdges + virtualEdges.size();
// edges between base and snapped point
VirtualEdgeIteratorState baseEdge = new VirtualEdgeIteratorState(origTraversalKey,
virtEdgeId, prevNodeId, nodeId, baseDistance, closestEdge.getFlags(), closestEdge.getName(), basePoints);
VirtualEdgeIteratorState baseReverseEdge = new VirtualEdgeIteratorState(origRevTraversalKey,
virtEdgeId, nodeId, prevNodeId, baseDistance, reverseFlags, closestEdge.getName(), baseReversePoints);
baseEdge.setReverseEdge(baseReverseEdge);
baseReverseEdge.setReverseEdge(baseEdge);
virtualEdges.add(baseEdge);
virtualEdges.add(baseReverseEdge);
}
/**
* Set those edges at the virtual node (nodeId) to 'unfavored' that require at least a turn of
* 100° from favoredHeading.
* <p>
*
* @param nodeId VirtualNode at which edges get unfavored
* @param favoredHeading north based azimuth of favored heading between 0 and 360
* @param incoming if true, incoming edges are unfavored, else outgoing edges
* @return boolean indicating if enforcement took place
*/
public boolean enforceHeading(int nodeId, double favoredHeading, boolean incoming) {
if (!isInitialized())
throw new IllegalStateException("QueryGraph.lookup has to be called in before heading enforcement");
if (Double.isNaN(favoredHeading))
return false;
if (!isVirtualNode(nodeId))
return false;
int virtNodeIDintern = nodeId - mainNodes;
favoredHeading = AC.convertAzimuth2xaxisAngle(favoredHeading);
// either penalize incoming or outgoing edges
List<Integer> edgePositions = incoming ? Arrays.asList(VE_BASE, VE_ADJ_REV) : Arrays.asList(VE_BASE_REV, VE_ADJ);
boolean enforcementOccurred = false;
for (int edgePos : edgePositions) {
VirtualEdgeIteratorState edge = virtualEdges.get(virtNodeIDintern * 4 + edgePos);
PointList wayGeo = edge.fetchWayGeometry(3);
double edgeOrientation;
if (incoming) {
int numWayPoints = wayGeo.getSize();
edgeOrientation = AC.calcOrientation(wayGeo.getLat(numWayPoints - 2), wayGeo.getLon(numWayPoints - 2),
wayGeo.getLat(numWayPoints - 1), wayGeo.getLon(numWayPoints - 1));
} else {
edgeOrientation = AC.calcOrientation(wayGeo.getLat(0), wayGeo.getLon(0),
wayGeo.getLat(1), wayGeo.getLon(1));
}
edgeOrientation = AC.alignOrientation(favoredHeading, edgeOrientation);
double delta = (edgeOrientation - favoredHeading);
if (Math.abs(delta) > 1.74) // penalize if a turn of more than 100°
{
edge.setUnfavored(true);
unfavoredEdges.add(edge);
//also apply to opposite edge for reverse routing
VirtualEdgeIteratorState reverseEdge = virtualEdges.get(virtNodeIDintern * 4 + getPosOfReverseEdge(edgePos));
reverseEdge.setUnfavored(true);
unfavoredEdges.add(reverseEdge);
enforcementOccurred = true;
}
}
return enforcementOccurred;
}
/**
* Sets the virtual edge with virtualEdgeId and its reverse edge to 'unfavored', which
* effectively penalizes both virtual edges towards an adjacent node of virtualNodeId.
* This makes it more likely (but does not guarantee) that the router chooses a route towards
* the other adjacent node of virtualNodeId.
* <p>
*
* @param virtualNodeId virtual node at which edges get unfavored
* @param virtualEdgeId this edge and the reverse virtual edge become unfavored
*/
public void unfavorVirtualEdgePair(int virtualNodeId, int virtualEdgeId) {
if (!isVirtualNode(virtualNodeId)) {
throw new IllegalArgumentException("Node id " + virtualNodeId
+ " must be a virtual node.");
}
VirtualEdgeIteratorState incomingEdge =
(VirtualEdgeIteratorState) getEdgeIteratorState(virtualEdgeId, virtualNodeId);
VirtualEdgeIteratorState reverseEdge = (VirtualEdgeIteratorState) getEdgeIteratorState(
virtualEdgeId, incomingEdge.getBaseNode());
incomingEdge.setUnfavored(true);
unfavoredEdges.add(incomingEdge);
reverseEdge.setUnfavored(true);
unfavoredEdges.add(reverseEdge);
}
/**
* Returns all virtual edges that have been unfavored via
* {@link #enforceHeading(int, double, boolean)} or {@link #unfavorVirtualEdgePair(int, int)}.
*/
public Set<EdgeIteratorState> getUnfavoredVirtualEdges() {
// Need to create a new set to convert Set<VirtualEdgeIteratorState> to
// Set<EdgeIteratorState>.
return new LinkedHashSet<EdgeIteratorState>(unfavoredEdges);
}
/**
* Removes the 'unfavored' status of all virtual edges.
*/
public void clearUnfavoredStatus() {
for (VirtualEdgeIteratorState edge : unfavoredEdges) {
edge.setUnfavored(false);
}
unfavoredEdges.clear();
}
@Override
public int getNodes() {
return virtualNodes.getSize() + mainNodes;
}
@Override
public NodeAccess getNodeAccess() {
return nodeAccess;
}
@Override
public BBox getBounds() {
return mainGraph.getBounds();
}
@Override
public EdgeIteratorState getEdgeIteratorState(int origEdgeId, int adjNode) {
if (!isVirtualEdge(origEdgeId))
return mainGraph.getEdgeIteratorState(origEdgeId, adjNode);
int edgeId = origEdgeId - mainEdges;
EdgeIteratorState eis = virtualEdges.get(edgeId);
if (eis.getAdjNode() == adjNode || adjNode == Integer.MIN_VALUE)
return eis;
edgeId = getPosOfReverseEdge(edgeId);
EdgeIteratorState eis2 = virtualEdges.get(edgeId);
if (eis2.getAdjNode() == adjNode)
return eis2;
throw new IllegalStateException("Edge " + origEdgeId + " not found with adjNode:" + adjNode
+ ". found edges were:" + eis + ", " + eis2);
}
private int getPosOfReverseEdge(int edgeId) {
// find reverse edge via convention. see virtualEdges comment above
if (edgeId % 2 == 0)
edgeId++;
else
edgeId--;
return edgeId;
}
@Override
public EdgeExplorer createEdgeExplorer(final EdgeFilter edgeFilter) {
if (!isInitialized())
throw new IllegalStateException("Call lookup before using this graph");
if (useEdgeExplorerCache) {
int counter = -1;
if (edgeFilter instanceof DefaultEdgeFilter) {
DefaultEdgeFilter dee = (DefaultEdgeFilter) edgeFilter;
counter = 0;
if (dee.acceptsBackward())
counter = 1;
if (dee.acceptsForward())
counter += 2;
if (counter == 0)
throw new IllegalStateException("You tried to use an edge filter blocking every access");
} else if (edgeFilter == EdgeFilter.ALL_EDGES) {
counter = 4;
}
if (counter >= 0) {
EdgeExplorer cached = cacheMap.get(counter);
if (cached == null) {
cached = createUncachedEdgeExplorer(edgeFilter);
cacheMap.put(counter, cached);
}
return cached;
}
}
return createUncachedEdgeExplorer(edgeFilter);
}
private EdgeExplorer createUncachedEdgeExplorer(EdgeFilter edgeFilter) {
// Iteration over virtual nodes needs to be thread safe if done from different explorer
// so we need to create the mapping on EVERY call!
// This needs to be a HashMap (and cannot be an array) as we also need to tweak edges for some mainNodes!
// The more query points we have the more inefficient this map could be. Hmmh.
final IntObjectMap<VirtualEdgeIterator> node2EdgeMap
= new GHIntObjectHashMap<VirtualEdgeIterator>(queryResults.size() * 3);
final EdgeExplorer mainExplorer = mainGraph.createEdgeExplorer(edgeFilter);
final GHIntHashSet towerNodesToChange = new GHIntHashSet(queryResults.size());
// 1. virtualEdges should also get fresh EdgeIterators on every createEdgeExplorer call!
for (int i = 0; i < queryResults.size(); i++) {
// create outgoing edges
VirtualEdgeIterator virtEdgeIter = new VirtualEdgeIterator(2);
EdgeIteratorState baseRevEdge = virtualEdges.get(i * 4 + VE_BASE_REV);
if (edgeFilter.accept(baseRevEdge))
virtEdgeIter.add(baseRevEdge);
EdgeIteratorState adjEdge = virtualEdges.get(i * 4 + VE_ADJ);
if (edgeFilter.accept(adjEdge))
virtEdgeIter.add(adjEdge);
int virtNode = mainNodes + i;
node2EdgeMap.put(virtNode, virtEdgeIter);
// replace edge list of neighboring tower nodes:
// add virtual edges only and collect tower nodes where real edges will be added in step 2.
//
// base node
int towerNode = baseRevEdge.getAdjNode();
if (!isVirtualNode(towerNode)) {
towerNodesToChange.add(towerNode);
addVirtualEdges(node2EdgeMap, edgeFilter, true, towerNode, i);
}
// adj node
towerNode = adjEdge.getAdjNode();
if (!isVirtualNode(towerNode)) {
towerNodesToChange.add(towerNode);
addVirtualEdges(node2EdgeMap, edgeFilter, false, towerNode, i);
}
}
// 2. the connected tower nodes from mainGraph need fresh EdgeIterators with possible fakes
// where 'fresh' means independent of previous call and respecting the edgeFilter
// -> setup fake iterators of detected tower nodes (virtual edges are already added)
towerNodesToChange.forEach(new IntProcedure() {
@Override
public void apply(int value) {
fillVirtualEdges(node2EdgeMap, value, mainExplorer);
}
});
return new EdgeExplorer() {
@Override
public EdgeIterator setBaseNode(int baseNode) {
VirtualEdgeIterator iter = node2EdgeMap.get(baseNode);
if (iter != null)
return iter.reset();
return mainExplorer.setBaseNode(baseNode);
}
};
}
/**
* Creates a fake edge iterator pointing to multiple edge states.
*/
private void addVirtualEdges(IntObjectMap<VirtualEdgeIterator> node2EdgeMap, EdgeFilter filter, boolean base,
int node, int virtNode) {
VirtualEdgeIterator existingIter = node2EdgeMap.get(node);
if (existingIter == null) {
existingIter = new VirtualEdgeIterator(10);
node2EdgeMap.put(node, existingIter);
}
EdgeIteratorState edge = base
? virtualEdges.get(virtNode * 4 + VE_BASE)
: virtualEdges.get(virtNode * 4 + VE_ADJ_REV);
if (filter.accept(edge))
existingIter.add(edge);
}
void fillVirtualEdges(IntObjectMap<VirtualEdgeIterator> node2Edge, int towerNode, EdgeExplorer mainExpl) {
if (isVirtualNode(towerNode))
throw new IllegalStateException("Node should not be virtual:" + towerNode + ", " + node2Edge);
VirtualEdgeIterator vIter = node2Edge.get(towerNode);
IntArrayList ignoreEdges = new IntArrayList(vIter.count() * 2);
while (vIter.next()) {
EdgeIteratorState edge = queryResults.get(vIter.getAdjNode() - mainNodes).getClosestEdge();
ignoreEdges.add(edge.getEdge());
}
vIter.reset();
EdgeIterator iter = mainExpl.setBaseNode(towerNode);
while (iter.next()) {
if (!ignoreEdges.contains(iter.getEdge()))
vIter.add(iter.detach(false));
}
}
private boolean isInitialized() {
return queryResults != null;
}
@Override
/**
* @see QueryGraph
*/
public EdgeExplorer createEdgeExplorer() {
return createEdgeExplorer(EdgeFilter.ALL_EDGES);
}
@Override
public AllEdgesIterator getAllEdges() {
throw new UnsupportedOperationException("Not supported yet.");
}
@Override
public EdgeIteratorState edge(int a, int b) {
throw exc();
}
public EdgeIteratorState edge(int a, int b, double distance, int flags) {
throw exc();
}
@Override
public EdgeIteratorState edge(int a, int b, double distance, boolean bothDirections) {
throw exc();
}
@Override
public Graph copyTo(Graph g) {
throw exc();
}
@Override
public GraphExtension getExtension() {
return wrappedExtension;
}
private UnsupportedOperationException exc() {
return new UnsupportedOperationException("QueryGraph cannot be modified.");
}
class QueryGraphTurnExt extends TurnCostExtension {
private final TurnCostExtension mainTurnExtension;
public QueryGraphTurnExt() {
this.mainTurnExtension = (TurnCostExtension) mainGraph.getExtension();
}
@Override
public long getTurnCostFlags(int edgeFrom, int nodeVia, int edgeTo) {
if (isVirtualNode(nodeVia)) {
return 0;
} else if (isVirtualEdge(edgeFrom) || isVirtualEdge(edgeTo)) {
if (isVirtualEdge(edgeFrom)) {
edgeFrom = queryResults.get((edgeFrom - mainEdges) / 4).getClosestEdge().getEdge();
}
if (isVirtualEdge(edgeTo)) {
edgeTo = queryResults.get((edgeTo - mainEdges) / 4).getClosestEdge().getEdge();
}
return mainTurnExtension.getTurnCostFlags(edgeFrom, nodeVia, edgeTo);
} else {
return mainTurnExtension.getTurnCostFlags(edgeFrom, nodeVia, edgeTo);
}
}
}
}