Examples with GHPoint3D com.graphhopper.util.shapes.GHPoint3D used on opensource projects

Example 1 with GHPoint3D

use of com.graphhopper.util.shapes.GHPoint3D in project graphhopper by graphhopper.

the class QueryGraph method lookup.

/**
 * 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());
    // 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;
            // 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;
        }
    });
}

Example 2 with GHPoint3D

use of com.graphhopper.util.shapes.GHPoint3D in project graphhopper by graphhopper.

the class GpxConversions method createGPXList.

public static List<GPXEntry> createGPXList(InstructionList instructions) {
    List<GPXEntry> gpxList = new ArrayList<>();
    long timeOffset = 0;
    for (Instruction instruction : instructions) {
        int i = 0;
        for (GHPoint3D point : instruction.getPoints()) {
            GPXEntry gpxEntry;
            if (i == 0) {
                gpxEntry = new GPXEntry(point, timeOffset);
            } else {
                // We don't have timestamps for pillar nodes
                gpxEntry = new GPXEntry(point);
            }
            gpxList.add(gpxEntry);
            i++;
        }
        timeOffset = timeOffset + instruction.getTime();
    }
    return gpxList;
}

Example 3 with GHPoint3D

use of com.graphhopper.util.shapes.GHPoint3D in project graphhopper by graphhopper.

the class GraphHopperTest method testSRTMWithInstructions.

@Test
public void testSRTMWithInstructions() {
    final String profile = "profile";
    final String vehicle = "foot";
    final String weighting = "shortest";
    GraphHopper hopper = new GraphHopper().setGraphHopperLocation(GH_LOCATION).setOSMFile(MONACO).setProfiles(new Profile(profile).setVehicle(vehicle).setWeighting(weighting)).setStoreOnFlush(true);
    hopper.setElevationProvider(new SRTMProvider(DIR));
    hopper.importOrLoad();
    GHResponse rsp = hopper.route(new GHRequest(43.730729, 7.421288, 43.727697, 7.419199).setAlgorithm(ASTAR).setProfile(profile));
    ResponsePath res = rsp.getBest();
    assertEquals(1614.3, res.getDistance(), .1);
    assertEquals(55, res.getPoints().size());
    assertTrue(res.getPoints().is3D());
    InstructionList il = res.getInstructions();
    assertEquals(12, il.size());
    assertTrue(il.get(0).getPoints().is3D());
    String str = res.getPoints().toString();
    assertEquals("(43.730684662577524,7.421283725164733,62.0), (43.7306797,7.4213823,66.0), " + "(43.731098,7.4215463,45.0), (43.7312991,7.42159,45.0), (43.7313271,7.4214147,45.0), " + "(43.7312506,7.4213664,45.0), (43.7312822,7.4211156,52.0), (43.7313624,7.4211455,52.0), " + "(43.7313714,7.4211233,52.0), (43.7314858,7.4211734,52.0), (43.7315753,7.4208688,52.0), " + "(43.7316061,7.4208249,52.0), (43.7316404,7.4208503,52.0), (43.7316741,7.4210502,52.0), " + "(43.7316276,7.4214636,45.0), (43.7316391,7.4215065,45.0), (43.7316664,7.4214904,45.0), " + "(43.7317185,7.4211861,52.0), (43.7319676,7.4206159,19.0), (43.732038,7.4203936,20.0), " + "(43.7322266,7.4196414,26.0), (43.7323236,7.4192656,26.0), (43.7323374,7.4190461,26.0), " + "(43.7323875,7.4189195,26.0), (43.731974,7.4181688,29.0), (43.7316421,7.4173042,23.0), " + "(43.7315686,7.4170356,31.0), (43.7314269,7.4166815,31.0), (43.7312401,7.4163184,49.0), " + "(43.7308286,7.4157613,29.399999618530273), (43.730662,7.4155599,22.0), " + "(43.7303643,7.4151193,51.0), (43.729579,7.4137274,40.0), (43.7295167,7.4137244,40.0), " + "(43.7294669,7.4137725,40.0), (43.7285987,7.4149068,23.0), (43.7285167,7.4149272,22.0), " + "(43.7283974,7.4148646,22.0), (43.7285619,7.4151365,23.0), (43.7285774,7.4152444,23.0), " + "(43.7285763,7.4159759,21.0), (43.7285238,7.4161982,20.0), (43.7284592,7.4163655,18.0), " + "(43.7281669,7.4168192,18.0), (43.7281442,7.4169449,18.0), (43.7281684,7.4172435,14.0), " + "(43.7282784,7.4179606,14.0), (43.7282757,7.418175,11.0), (43.7282319,7.4183683,11.0), " + "(43.7281482,7.4185473,11.0), (43.7280654,7.4186535,11.0), (43.7279259,7.418748,11.0), " + "(43.727779,7.4187731,11.0), (43.7276825,7.4190072,11.0), " + "(43.72767974015672,7.419198523220426,11.0)", str);
    assertEquals(84, res.getAscend(), 1e-1);
    assertEquals(135, res.getDescend(), 1e-1);
    assertEquals(55, res.getPoints().size());
    assertEquals(new GHPoint3D(43.73068455771767, 7.421283689825812, 62.0), res.getPoints().get(0));
    assertEquals(new GHPoint3D(43.727679637988224, 7.419198521975086, 11.0), res.getPoints().get(res.getPoints().size() - 1));
    assertEquals(62, res.getPoints().get(0).getEle(), 1e-2);
    assertEquals(66, res.getPoints().get(1).getEle(), 1e-2);
    assertEquals(52, res.getPoints().get(10).getEle(), 1e-2);
}

Example 4 with GHPoint3D

use of com.graphhopper.util.shapes.GHPoint3D in project graphhopper by graphhopper.

the class QueryOverlayBuilder method buildVirtualEdges.

/**
 * For all specified snaps calculate the snapped point and if necessary set the closest node
 * to a virtual one and reverse the closest edge. Additionally the wayIndex can change if an edge is
 * swapped.
 */
private void buildVirtualEdges(List<Snap> snaps) {
    GHIntObjectHashMap<List<Snap>> edge2res = new GHIntObjectHashMap<>(snaps.size());
    // calculate snapped point and swap direction of closest edge if necessary
    for (Snap snap : snaps) {
        // Do not create virtual node for a snap if it is directly on a tower node or not found
        if (snap.getSnappedPosition() == Snap.Position.TOWER)
            continue;
        EdgeIteratorState closestEdge = snap.getClosestEdge();
        if (closestEdge == null)
            throw new IllegalStateException("Do not call QueryGraph.create with invalid Snap " + snap);
        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(FetchMode.PILLAR_ONLY);
            if (pl.size() > 1)
                doReverse = pl.getLat(0) > pl.getLat(pl.size() - 1);
        }
        if (doReverse) {
            closestEdge = closestEdge.detach(true);
            PointList fullPL = closestEdge.fetchWayGeometry(FetchMode.ALL);
            snap.setClosestEdge(closestEdge);
            if (snap.getSnappedPosition() == Snap.Position.PILLAR)
                // ON pillar node
                snap.setWayIndex(fullPL.size() - snap.getWayIndex() - 1);
            else
                // for case "OFF pillar node"
                snap.setWayIndex(fullPL.size() - snap.getWayIndex() - 2);
            if (snap.getWayIndex() < 0)
                throw new IllegalStateException("Problem with wayIndex while reversing closest edge:" + closestEdge + ", " + snap);
        }
        // find multiple results on same edge
        int edgeId = closestEdge.getEdge();
        List<Snap> list = edge2res.get(edgeId);
        if (list == null) {
            list = new ArrayList<>(5);
            edge2res.put(edgeId, list);
        }
        list.add(snap);
    }
    // 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<Snap>>() {

        @Override
        public boolean apply(int edgeId, List<Snap> results) {
            // we can expect at least one entry in the results
            EdgeIteratorState closestEdge = results.get(0).getClosestEdge();
            final PointList fullPL = closestEdge.fetchWayGeometry(FetchMode.ALL);
            int baseNode = closestEdge.getBaseNode();
            Collections.sort(results, new Comparator<Snap>() {

                @Override
                public int compare(Snap o1, Snap o2) {
                    int diff = Integer.compare(o1.getWayIndex(), o2.getWayIndex());
                    if (diff == 0) {
                        return Double.compare(distanceOfSnappedPointToPillarNode(o1), distanceOfSnappedPointToPillarNode(o2));
                    } else {
                        return diff;
                    }
                }

                private double distanceOfSnappedPointToPillarNode(Snap o) {
                    GHPoint snappedPoint = o.getSnappedPoint();
                    double fromLat = fullPL.getLat(o.getWayIndex());
                    double fromLon = fullPL.getLon(o.getWayIndex());
                    return DistancePlaneProjection.DIST_PLANE.calcNormalizedDist(fromLat, fromLon, snappedPoint.lat, snappedPoint.lon);
                }
            });
            GHPoint3D prevPoint = fullPL.get(0);
            int adjNode = closestEdge.getAdjNode();
            int origEdgeKey = closestEdge.getEdgeKey();
            int origRevEdgeKey = GHUtility.reverseEdgeKey(origEdgeKey);
            int prevWayIndex = 1;
            int prevNodeId = baseNode;
            int virtNodeId = queryOverlay.getVirtualNodes().size() + firstVirtualNodeId;
            boolean addedEdges = false;
            // fullPL into the right pieces.
            for (Snap res : results) {
                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;
                }
                queryOverlay.getClosestEdges().add(res.getClosestEdge().getEdge());
                boolean isPillar = res.getSnappedPosition() == Snap.Position.PILLAR;
                createEdges(origEdgeKey, origRevEdgeKey, prevPoint, prevWayIndex, isPillar, res.getSnappedPoint(), res.getWayIndex(), fullPL, closestEdge, prevNodeId, virtNodeId);
                queryOverlay.getVirtualNodes().add(currSnapped.lat, currSnapped.lon, currSnapped.ele);
                // add edges again to set adjacent edges for newVirtNodeId
                if (addedEdges) {
                    queryOverlay.addVirtualEdge(queryOverlay.getVirtualEdge(queryOverlay.getNumVirtualEdges() - 2));
                    queryOverlay.addVirtualEdge(queryOverlay.getVirtualEdge(queryOverlay.getNumVirtualEdges() - 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(origEdgeKey, origRevEdgeKey, prevPoint, prevWayIndex, false, fullPL.get(fullPL.size() - 1), fullPL.size() - 2, fullPL, closestEdge, virtNodeId - 1, adjNode);
            return true;
        }
    });
}

Example 5 with GHPoint3D

use of com.graphhopper.util.shapes.GHPoint3D in project graphhopper by graphhopper.

the class QueryResult method calcSnappedPoint.

/**
 * 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);
}