HashSet<Vertex> closed = new HashSet<Vertex>();
// initialize queue with all vertices which already have known elevation
for (StreetEdge e : edgesWithElevation) {
PackedCoordinateSequence profile = e.getElevationProfile();
if (!elevations.containsKey(e.getFromVertex())) {
double firstElevation = profile.getOrdinate(0, 1);
ElevationRepairState state = new ElevationRepairState(null, null,
e.getFromVertex(), 0, firstElevation);
pq.insert(state, 0);
elevations.put(e.getFromVertex(), firstElevation);
}
if (!elevations.containsKey(e.getToVertex())) {
double lastElevation = profile.getOrdinate(profile.size() - 1, 1);
ElevationRepairState state = new ElevationRepairState(null, null, e.getToVertex(),
0, lastElevation);
pq.insert(state, 0);
elevations.put(e.getToVertex(), lastElevation);
}
}
// Grow an SPT outward from vertices with known elevations into regions where the
// elevation is not known. when a branch hits a region with known elevation, follow the
// back pointers through the region of unknown elevation, setting elevations via interpolation.
while (!pq.empty()) {
ElevationRepairState state = pq.extract_min();
if (closed.contains(state.vertex)) continue;
closed.add(state.vertex);
ElevationRepairState curState = state;
Vertex initialVertex = null;
while (curState != null) {
initialVertex = curState.vertex;
curState = curState.backState;
}
double bestDistance = Double.MAX_VALUE;
double bestElevation = 0;
for (Edge e : state.vertex.getOutgoing()) {
if (!(e instanceof StreetEdge)) {
continue;
}
StreetEdge edge = (StreetEdge) e;
Vertex tov = e.getToVertex();
if (tov == initialVertex)
continue;
Double elevation = elevations.get(tov);
if (elevation != null) {
double distance = e.getDistance();
if (distance < bestDistance) {
bestDistance = distance;
bestElevation = elevation;
}
} else {
// continue
ElevationRepairState newState = new ElevationRepairState(edge, state, tov,
e.getDistance() + state.distance, state.initialElevation);
pq.insert(newState, e.getDistance() + state.distance);
}
} // end loop over outgoing edges
for (Edge e : state.vertex.getIncoming()) {
if (!(e instanceof StreetEdge)) {
continue;
}
StreetEdge edge = (StreetEdge) e;
Vertex fromv = e.getFromVertex();
if (fromv == initialVertex)
continue;
Double elevation = elevations.get(fromv);
if (elevation != null) {
double distance = e.getDistance();
if (distance < bestDistance) {
bestDistance = distance;
bestElevation = elevation;
}
} else {
// continue
ElevationRepairState newState = new ElevationRepairState(edge, state, fromv,
e.getDistance() + state.distance, state.initialElevation);
pq.insert(newState, e.getDistance() + state.distance);
}
} // end loop over incoming edges
//limit elevation propagation to at max 2km; this prevents an infinite loop
//in the case of islands missing elevation (and some other cases)
if (bestDistance == Double.MAX_VALUE && state.distance > 2000) {
log.warn("While propagating elevations, hit 2km distance limit at " + state.vertex);
bestDistance = state.distance;
bestElevation = state.initialElevation;
}
if (bestDistance != Double.MAX_VALUE) {
// we have found a second vertex with elevation, so we can interpolate the elevation
// for this point
double totalDistance = bestDistance + state.distance;
// trace backwards, setting states as we go
while (true) {
// watch out for division by 0 here, which will propagate NaNs
// all the way out to edge lengths
if (totalDistance == 0)
elevations.put(state.vertex, bestElevation);
else {
double elevation = (bestElevation * state.distance +
state.initialElevation * bestDistance) / totalDistance;
elevations.put(state.vertex, elevation);
}
if (state.backState == null)
break;
bestDistance += state.backEdge.getDistance();
state = state.backState;
if (elevations.containsKey(state.vertex))
break;
}
}
} // end loop over states
// do actual assignments
for (Vertex v : graph.getVertices()) {
Double fromElevation = elevations.get(v);
for (Edge e : v.getOutgoing()) {
if (e instanceof StreetWithElevationEdge) {
StreetWithElevationEdge edge = ((StreetWithElevationEdge) e);
Double toElevation = elevations.get(edge.getToVertex());
if (fromElevation == null || toElevation == null) {
if (!edge.isElevationFlattened() && !edge.isSlopeOverride())
log.warn("Unexpectedly missing elevation for edge " + edge);
continue;
}
if (edge.getElevationProfile() != null && edge.getElevationProfile().size() > 2) {
continue;
}
Coordinate[] coords = new Coordinate[2];
coords[0] = new Coordinate(0, fromElevation);
coords[1] = new Coordinate(edge.getDistance(), toElevation);
PackedCoordinateSequence profile = new PackedCoordinateSequence.Double(coords);
if (edge.setElevationProfile(profile, true)) {
log.trace(graph.addBuilderAnnotation(new ElevationFlattened(edge)));
}
}