// 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
routeProjectedEndIndex = routeIndex; // this is bad, but not terrible
// 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) {
// we're not going to bother with states which are
// 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);