if (!result.isValid()) { errors += "Cannot find close node found for landmark suggestion[" + lmSuggestionIdx + "]=" + point + ".\n"; continue; landmarkNodeIds.add(result.getClosestNode());
@Test public void testUseMeanElevation() { g.close(); g = new GraphHopperStorage(new RAMDirectory(), encodingManager, true, new GraphExtension.NoOpExtension()).create(100); NodeAccess na = g.getNodeAccess(); na.setNode(0, 0, 0, 0); na.setNode(1, 0, 0.0001, 20); EdgeIteratorState edge = g.edge(0, 1); EdgeIteratorState edgeReverse = edge.detach(true); DistanceCalc2D distCalc = new DistanceCalc2D(); QueryResult qr = new QueryResult(0, 0.00005); qr.setClosestEdge(edge); qr.setWayIndex(0); qr.setSnappedPosition(EDGE); qr.calcSnappedPoint(distCalc); assertEquals(10, qr.getSnappedPoint().getEle(), 1e-1); qr = new QueryResult(0, 0.00005); qr.setClosestEdge(edgeReverse); qr.setWayIndex(0); qr.setSnappedPosition(EDGE); qr.calcSnappedPoint(distCalc); assertEquals(10, qr.getSnappedPoint().getEle(), 1e-1); }
QueryResult qr = locationIndex.findClosest(center.getLat(), center.getLon(), filter); if (!qr.isValid()) throw new IllegalArgumentException("Shape " + shape + " does not cover graph"); if (shape.contains(qr.getSnappedPoint().lat, qr.getSnappedPoint().lon)) edgeIds.add(qr.getClosestEdge().getEdge()); bfs.start(graph.createEdgeExplorer(filter), qr.getClosestNode());
QueryResult createQR(double lat, double lon, int wayIndex, EdgeIteratorState edge) { QueryResult res = new QueryResult(lat, lon); res.setClosestEdge(edge); res.setWayIndex(wayIndex); res.setSnappedPosition(QueryResult.Position.EDGE); res.calcSnappedPoint(Helper.DIST_PLANE); return res; }
void queryIndex(Graph g, LocationIndex idx, double lat, double lon, double expectedDist) { QueryResult res = idx.findClosest(lat, lon, EdgeFilter.ALL_EDGES); if (!res.isValid()) { errors.add("node not found for " + lat + "," + lon); return; } GHPoint found = res.getSnappedPoint(); double dist = distCalc.calcDist(lat, lon, found.lat, found.lon); if (Math.abs(dist - expectedDist) > .1) { errors.add("queried lat,lon=" + (float) lat + "," + (float) lon + " (found: " + (float) found.lat + "," + (float) found.lon + ")" + "\n expected distance:" + expectedDist + ", but was:" + dist); } }
/** * This method fills the edgeIds hash with edgeIds found close (exact match) to the specified lat,lon */ public void findClosestEdge(GHIntHashSet edgeIds, double lat, double lon, EdgeFilter filter) { QueryResult qr = locationIndex.findClosest(lat, lon, filter); if (qr.isValid()) edgeIds.add(qr.getClosestEdge().getEdge()); }
int closestNode = qr.getClosestNode(); if (queryGraph.isVirtualNode(closestNode)) { VirtualEdgeIteratorState outgoingVirtualEdge = j == 0 ? e2 : e1; QueryResult vqr = new QueryResult(qr.getQueryPoint().lat, qr.getQueryPoint().lon); vqr.setQueryDistance(qr.getQueryDistance()); vqr.setClosestNode(qr.getClosestNode()); vqr.setWayIndex(qr.getWayIndex()); vqr.setSnappedPosition(qr.getSnappedPosition()); vqr.setClosestEdge(qr.getClosestEdge()); vqr.calcSnappedPoint(distanceCalc); GPXExtension candidate = new GPXExtension(gpxEntry, vqr, incomingVirtualEdge, outgoingVirtualEdge);
final QueryResult closestMatch = new QueryResult(queryLat, queryLon); for (int iteration = 0; iteration < maxRegionSearch; iteration++) { GHIntHashSet storedNetworkEntryIds = new GHIntHashSet(); if (earlyFinish && closestMatch.isValid()) break; if (closestMatch.isValid()) { closestMatch.setQueryDistance(distCalc.calcDenormalizedDist(closestMatch.getQueryDistance())); closestMatch.calcSnappedPoint(distCalc);
throw new IllegalStateException("You need to create a new LocationIndex instance as it is already closed"); QueryResult res = new QueryResult(queryLat, queryLon); double foundDist = Double.MAX_VALUE; AllEdgesIterator iter = graph.getAllEdges(); res.setQueryDistance(fromDist); res.setClosestEdge(iter.detach(false)); res.setClosestNode(node); foundDist = fromDist; fromLat, fromLon, toLat, toLon)); if (distEdge < foundDist) { res.setQueryDistance(distEdge); res.setClosestNode(node); res.setClosestEdge(iter); if (fromDist > calc.calcDist(toLat, toLon, queryLat, queryLon)) res.setClosestNode(toNode);
@Test public void testAvoidDuplicateVirtualNodesIfIdentical() { initGraph(g); EdgeIteratorState edgeState = GHUtility.getEdge(g, 0, 2); QueryResult res1 = createLocationResult(0.5, 0, edgeState, 0, EDGE); QueryResult res2 = createLocationResult(0.5, 0, edgeState, 0, EDGE); QueryGraph queryGraph = new QueryGraph(g); queryGraph.lookup(Arrays.asList(res1, res2)); assertEquals(new GHPoint(0.5, 0), res1.getSnappedPoint()); assertEquals(new GHPoint(0.5, 0), res2.getSnappedPoint()); assertEquals(3, res1.getClosestNode()); assertEquals(3, res2.getClosestNode()); // force skip due to **tower** node snapping in phase 2, but no virtual edges should be created for res1 edgeState = GHUtility.getEdge(g, 0, 1); res1 = createLocationResult(1, 0, edgeState, 0, EDGE); // now create virtual edges edgeState = GHUtility.getEdge(g, 0, 2); res2 = createLocationResult(0.5, 0, edgeState, 0, EDGE); queryGraph = new QueryGraph(g); queryGraph.lookup(Arrays.asList(res1, res2)); // make sure only one virtual node was created assertEquals(queryGraph.getNodes(), g.getNodes() + 1); EdgeIterator iter = queryGraph.createEdgeExplorer().setBaseNode(0); assertEquals(GHUtility.asSet(1, 3), GHUtility.getNeighbors(iter)); }
double mainLat = nodeAccess.getLatitude(id); double mainLon = nodeAccess.getLongitude(id); final QueryResult res = new QueryResult(queryLat, queryLon); res.setClosestNode(id); res.setQueryDistance(distCalc.calcNormalizedDist(queryLat, queryLon, mainLat, mainLon)); goFurtherHook(id); new BreadthFirstSearch() { res.setQueryDistance(distCalc.calcDenormalizedDist(res.getQueryDistance())); return res;
@Override public int doCalc(boolean warmup, int run) { double lat = rand.nextDouble() * latDelta + bbox.minLat; double lon = rand.nextDouble() * lonDelta + bbox.minLon; int val = idx.findClosest(lat, lon, EdgeFilter.ALL_EDGES).getClosestNode(); // if (!warmup && val >= 0) // list.add(val); return val; } }.setIterations(count).start();
@Override protected boolean goFurther(int baseNode) { if (baseNode == id) return true; goFurtherHook(baseNode); double currLat = nodeAccess.getLatitude(baseNode); double currLon = nodeAccess.getLongitude(baseNode); double currNormedDist = distCalc.calcNormalizedDist(queryLat, queryLon, currLat, currLon); if (currNormedDist < res.getQueryDistance()) { res.setQueryDistance(currNormedDist); res.setClosestNode(baseNode); return true; } return currNormedDist < maxRasterWidth2InMeterNormed; } }.start(graph.createEdgeExplorer(), id);
int edge = qr.getClosestEdge().getEdge(); List<GPXExtension> extensionList = extensionMap.get(edge); if (extensionList == null) { goalSet.add(qr.getClosestNode()); double distance = distanceCalc.calcDist(qr.getQueryPoint().getLat(), qr.getQueryPoint().getLon(), qr.getSnappedPoint().getLat(), qr.getSnappedPoint().getLon()); algo.initFrom(qr.getClosestNode(), customWeighting.getMinWeight(distance * maxSearchWeightMultiplier)); int lastMatchNode = pathEdgeList.get(pathEdgeList.size() - 1).getAdjNode(); for (QueryResult qr : endQRList) { if (qr.getClosestNode() == lastMatchNode) { firstQueryResults.add(qr);
@Override public boolean apply(int edgeId, List<QueryResult> results) { EdgeIteratorState closestEdge = results.get(0).getClosestEdge(); final PointList fullPL = closestEdge.fetchWayGeometry(3); int baseNode = closestEdge.getBaseNode(); 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(); res.setClosestNode(prevNodeId); continue; createEdges(origTraversalKey, origRevTraversalKey, prevPoint, prevWayIndex, res.getSnappedPoint(), res.getWayIndex(), fullPL, closestEdge, prevNodeId, virtNodeId, reverseFlags); res.setClosestNode(virtNodeId); prevNodeId = virtNodeId; prevWayIndex = res.getWayIndex() + 1; prevPoint = currSnapped; virtNodeId++;
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); closestEdge = closestEdge.detach(true); PointList fullPL = closestEdge.fetchWayGeometry(3); res.setClosestEdge(closestEdge); if (res.getSnappedPosition() == QueryResult.Position.PILLAR) res.setWayIndex(fullPL.getSize() - res.getWayIndex() - 1); else res.setWayIndex(fullPL.getSize() - res.getWayIndex() - 2); if (res.getWayIndex() < 0) throw new IllegalStateException("Problem with wayIndex while reversing closest edge:" + closestEdge + ", " + res);
/** * Calculates the closet point on the edge from the query point. */ public void calcSnappedPoint(DistanceCalc distCalc) { if (closestEdge == null) throw new IllegalStateException("No closest edge?"); if (snappedPoint != null) throw new IllegalStateException("Calculate snapped point only once"); PointList fullPL = getClosestEdge().fetchWayGeometry(3); double tmpLat = fullPL.getLatitude(wayIndex); double tmpLon = fullPL.getLongitude(wayIndex); double tmpEle = fullPL.getElevation(wayIndex); if (snappedPosition != Position.EDGE) { snappedPoint = new GHPoint3D(tmpLat, tmpLon, tmpEle); return; } double queryLat = getQueryPoint().lat, queryLon = getQueryPoint().lon; double adjLat = fullPL.getLatitude(wayIndex + 1), adjLon = fullPL.getLongitude(wayIndex + 1); if (distCalc.validEdgeDistance(queryLat, queryLon, tmpLat, tmpLon, adjLat, adjLon)) { GHPoint tmpPoint = distCalc.calcCrossingPointToEdge(queryLat, queryLon, tmpLat, tmpLon, adjLat, adjLon); double adjEle = fullPL.getElevation(wayIndex + 1); snappedPoint = new GHPoint3D(tmpPoint.lat, tmpPoint.lon, (tmpEle + adjEle) / 2); } else // outside of edge boundaries snappedPoint = new GHPoint3D(tmpLat, tmpLon, tmpEle); }
@Test public void testSnappedPointAndGeometry() { Graph graph = createTestGraph(encodingManager); LocationIndex index = createIndex(graph, -1); // query directly the tower node QueryResult res = index.findClosest(-0.4, 0.9, EdgeFilter.ALL_EDGES); assertTrue(res.isValid()); assertEquals(new GHPoint(-0.4, 0.9), res.getSnappedPoint()); res = index.findClosest(-0.6, 1.6, EdgeFilter.ALL_EDGES); assertTrue(res.isValid()); assertEquals(new GHPoint(-0.6, 1.6), res.getSnappedPoint()); // query the edge (1,3). The edge (0,4) has 27674 as distance res = index.findClosest(-0.2, 0.3, EdgeFilter.ALL_EDGES); assertTrue(res.isValid()); assertEquals(26936, res.getQueryDistance(), 1); assertEquals(new GHPoint(-0.441624, 0.317259), res.getSnappedPoint()); }
public EdgeIteratorState getOriginalEdgeFromVirtNode(int nodeId) { return queryResults.get(nodeId - mainNodes).getClosestEdge(); }
private String getSnappedCandidates(Collection<GPXExtension> candidates) { String str = ""; for (GPXExtension gpxe : candidates) { if (!str.isEmpty()) { str += ", "; } str += "distance: " + gpxe.getQueryResult().getQueryDistance() + " to " + gpxe.getQueryResult().getSnappedPoint(); } return "[" + str + "]"; }