Search in sources :

Example 1 with AssertionFailedException

use of com.vividsolutions.jts.util.AssertionFailedException in project OpenTripPlanner by opentripplanner.

the class MidblockMatchState method getNextStates.

@Override
public List<MatchState> getNextStates() {
    ArrayList<MatchState> nextStates = new ArrayList<MatchState>();
    if (routeIndex.getSegmentIndex() == routeGeometry.getNumPoints() - 1) {
        // this has either hit the end, or gone off the end. It's not real clear which.
        // for now, let's assume it means that the ending is somewhere along this edge,
        // so we return an end state
        Coordinate pt = routeIndex.getCoordinate(routeGeometry);
        double error = distance(pt, edgeIndex.getCoordinate(edgeGeometry));
        nextStates.add(new EndMatchState(this, error, 0));
        return nextStates;
    }
    LinearIterator it = new LinearIterator(routeGeometry, routeIndex);
    if (it.hasNext()) {
        it.next();
        LinearLocation routeSuccessor = it.getLocation();
        // now we want to see where this new point is in terms of the edge's geometry
        Coordinate newRouteCoord = routeSuccessor.getCoordinate(routeGeometry);
        LinearLocation newEdgeIndex = indexedEdge.project(newRouteCoord);
        Coordinate edgeCoord = newEdgeIndex.getCoordinate(edgeGeometry);
        if (newEdgeIndex.compareTo(edgeIndex) <= 0) {
            /* this should not require the try/catch, but there is a bug in JTS */
            try {
                LinearLocation projected2 = indexedEdge.indexOfAfter(edgeCoord, edgeIndex);
                // another bug in JTS
                if (Double.isNaN(projected2.getSegmentFraction())) {
                    // we are probably moving backwards
                    return Collections.emptyList();
                } else {
                    newEdgeIndex = projected2;
                    if (newEdgeIndex.equals(edgeIndex)) {
                        return Collections.emptyList();
                    }
                }
                edgeCoord = newEdgeIndex.getCoordinate(edgeGeometry);
            } catch (AssertionFailedException e) {
                // we are not making progress, so just return an empty list
                return Collections.emptyList();
            }
        }
        if (newEdgeIndex.getSegmentIndex() == edgeGeometry.getNumPoints() - 1) {
            // we might choose to continue from the end of the edge and a point mid-way
            // along this route segment
            // find nearest point that makes progress along the route
            Vertex toVertex = edge.getToVertex();
            Coordinate endCoord = toVertex.getCoordinate();
            LocationIndexedLine indexedRoute = new LocationIndexedLine(routeGeometry);
            // FIXME: it would be better to do this project/indexOfAfter in one step
            // as the two-step version could snap to a bad place and be unable to escape.
            LinearLocation routeProjectedEndIndex = indexedRoute.project(endCoord);
            Coordinate routeProjectedEndCoord = routeProjectedEndIndex.getCoordinate(routeGeometry);
            if (routeProjectedEndIndex.compareTo(routeIndex) <= 0) {
                try {
                    routeProjectedEndIndex = indexedRoute.indexOfAfter(routeProjectedEndCoord, routeIndex);
                    if (Double.isNaN(routeProjectedEndIndex.getSegmentFraction())) {
                        // can't go forward
                        // this is bad, but not terrible
                        routeProjectedEndIndex = routeIndex;
                    // since we are advancing along the edge
                    }
                } catch (AssertionFailedException e) {
                    routeProjectedEndIndex = routeIndex;
                }
                routeProjectedEndCoord = routeProjectedEndIndex.getCoordinate(routeGeometry);
            }
            double positionError = distance(routeProjectedEndCoord, endCoord);
            double travelAlongRoute = distanceAlongGeometry(routeGeometry, routeIndex, routeProjectedEndIndex);
            double travelAlongEdge = distanceAlongGeometry(edgeGeometry, edgeIndex, newEdgeIndex);
            double travelError = Math.abs(travelAlongEdge - travelAlongRoute);
            double error = positionError + travelError;
            if (error > MAX_ERROR) {
                // totally wrong
                return nextStates;
            }
            for (Edge e : getOutgoingMatchableEdges(toVertex)) {
                double cost = error + NEW_SEGMENT_PENALTY;
                if (!carsCanTraverse(e)) {
                    cost += NO_TRAVERSE_PENALTY;
                }
                MatchState nextState = new MidblockMatchState(this, routeGeometry, e, routeProjectedEndIndex, new LinearLocation(), cost, travelAlongRoute);
                nextStates.add(nextState);
            }
        } else {
            double travelAlongEdge = distanceAlongGeometry(edgeGeometry, edgeIndex, newEdgeIndex);
            double travelAlongRoute = distanceAlongGeometry(routeGeometry, routeIndex, routeSuccessor);
            double travelError = Math.abs(travelAlongRoute - travelAlongEdge);
            double positionError = distance(edgeCoord, newRouteCoord);
            double error = travelError + positionError;
            MatchState nextState = new MidblockMatchState(this, routeGeometry, edge, routeSuccessor, newEdgeIndex, error, travelAlongRoute);
            nextStates.add(nextState);
            // it's also possible that, although we have not yet reached the end of this edge,
            // we are going to turn, because the route turns earlier than the edge. In that
            // case, we jump to the corner, and our error is the distance from the route point
            // and the corner
            Vertex toVertex = edge.getToVertex();
            double travelAlongOldEdge = distanceAlongGeometry(edgeGeometry, edgeIndex, null);
            for (Edge e : getOutgoingMatchableEdges(toVertex)) {
                Geometry newEdgeGeometry = e.getGeometry();
                LocationIndexedLine newIndexedEdge = new LocationIndexedLine(newEdgeGeometry);
                newEdgeIndex = newIndexedEdge.project(newRouteCoord);
                Coordinate newEdgeCoord = newEdgeIndex.getCoordinate(newEdgeGeometry);
                positionError = distance(newEdgeCoord, newRouteCoord);
                travelAlongEdge = travelAlongOldEdge + distanceAlongGeometry(newEdgeGeometry, new LinearLocation(), newEdgeIndex);
                travelError = Math.abs(travelAlongRoute - travelAlongEdge);
                error = travelError + positionError;
                if (error > MAX_ERROR) {
                    // totally wrong
                    return nextStates;
                }
                double cost = error + NEW_SEGMENT_PENALTY;
                if (!carsCanTraverse(e)) {
                    cost += NO_TRAVERSE_PENALTY;
                }
                nextState = new MidblockMatchState(this, routeGeometry, e, routeSuccessor, new LinearLocation(), cost, travelAlongRoute);
                nextStates.add(nextState);
            }
        }
        return nextStates;
    } else {
        Coordinate routeCoord = routeIndex.getCoordinate(routeGeometry);
        LinearLocation projected = indexedEdge.project(routeCoord);
        double locationError = distance(projected.getCoordinate(edgeGeometry), routeCoord);
        MatchState end = new EndMatchState(this, locationError, 0);
        return Arrays.asList(end);
    }
}
Also used : Vertex(org.opentripplanner.routing.graph.Vertex) LinearLocation(com.vividsolutions.jts.linearref.LinearLocation) LocationIndexedLine(com.vividsolutions.jts.linearref.LocationIndexedLine) ArrayList(java.util.ArrayList) Geometry(com.vividsolutions.jts.geom.Geometry) Coordinate(com.vividsolutions.jts.geom.Coordinate) AssertionFailedException(com.vividsolutions.jts.util.AssertionFailedException) Edge(org.opentripplanner.routing.graph.Edge)

Aggregations

Coordinate (com.vividsolutions.jts.geom.Coordinate)1 Geometry (com.vividsolutions.jts.geom.Geometry)1 LinearLocation (com.vividsolutions.jts.linearref.LinearLocation)1 LocationIndexedLine (com.vividsolutions.jts.linearref.LocationIndexedLine)1 AssertionFailedException (com.vividsolutions.jts.util.AssertionFailedException)1 ArrayList (java.util.ArrayList)1 Edge (org.opentripplanner.routing.graph.Edge)1 Vertex (org.opentripplanner.routing.graph.Vertex)1