From 3d34340e141ece51b8e550b71d9d16e5127f62b7 Mon Sep 17 00:00:00 2001 From: afischerdev Date: Mon, 16 Jan 2023 11:18:12 +0100 Subject: [PATCH] recalc elevation at end --- .../java/btools/router/OsmPathElement.java | 16 ++- .../java/btools/router/RoutingEngine.java | 117 +++++++++++++++++- 2 files changed, 131 insertions(+), 2 deletions(-) diff --git a/brouter-core/src/main/java/btools/router/OsmPathElement.java b/brouter-core/src/main/java/btools/router/OsmPathElement.java index c89f5e3..f9b2750 100644 --- a/brouter-core/src/main/java/btools/router/OsmPathElement.java +++ b/brouter-core/src/main/java/btools/router/OsmPathElement.java @@ -36,6 +36,10 @@ public class OsmPathElement implements OsmPos { return selev; } + public final void setSElev(short s) { + selev = s; + } + public final double getElev() { return selev / 4.; } @@ -60,6 +64,12 @@ public class OsmPathElement implements OsmPos { } } + public final void setAngle(float e) { + if (message != null) { + message.turnangle = e; + } + } + public final long getIdFromPos() { return ((long) ilon) << 32 | ilat; } @@ -73,7 +83,7 @@ public class OsmPathElement implements OsmPos { // construct a path element from a path public static final OsmPathElement create(OsmPath path, boolean countTraffic) { OsmNode n = path.getTargetNode(); - OsmPathElement pe = create(n.getILon(), n.getILat(), path.selev, path.originElement, countTraffic); + OsmPathElement pe = create(n.getILon(), n.getILat(), n.getSElev(), path.originElement, countTraffic); pe.cost = path.cost; pe.message = path.message; return pe; @@ -98,6 +108,10 @@ public class OsmPathElement implements OsmPos { return ilon + "_" + ilat; } + public boolean positionEquals(OsmPathElement e) { + return this.ilat == e.ilat && this.ilon == e.ilon; + } + public void writeToStream(DataOutput dos) throws IOException { dos.writeInt(ilat); dos.writeInt(ilon); diff --git a/brouter-core/src/main/java/btools/router/RoutingEngine.java b/brouter-core/src/main/java/btools/router/RoutingEngine.java index 5c4f429..d69a11d 100644 --- a/brouter-core/src/main/java/btools/router/RoutingEngine.java +++ b/brouter-core/src/main/java/btools/router/RoutingEngine.java @@ -237,6 +237,119 @@ public class RoutingEngine extends Thread { } } + private void postElevationCheck(OsmTrack track) { + OsmPathElement lastPt = null; + OsmPathElement startPt = null; + short lastElev = Short.MIN_VALUE; + short startElev = Short.MIN_VALUE; + short endElev = Short.MIN_VALUE; + int startIdx = 0; + int endIdx = -1; + int dist = 0; + for (int idx = 0; idx < track.nodes.size(); idx++) { + OsmPathElement n = track.nodes.get(idx); + if (n.getSElev() == Short.MIN_VALUE && lastElev != Short.MIN_VALUE) { + // start one point before entry point to get better elevation results + if (idx > 1) + startElev = track.nodes.get(idx - 2).getSElev(); + if (startElev == Short.MIN_VALUE) + startElev = lastElev; + startIdx = idx; + startPt = lastPt; + dist = 0; + if (lastPt != null) + dist += n.calcDistance(lastPt); + } else if (n.getSElev() != Short.MIN_VALUE && lastElev == Short.MIN_VALUE && startElev != Short.MIN_VALUE) { + // end one point behind exit point to get better elevation results + if (idx + 1 < track.nodes.size()) + endElev = track.nodes.get(idx + 1).getSElev(); + if (endElev == Short.MIN_VALUE) + endElev = n.getSElev(); + endIdx = idx; + OsmPathElement tmpPt = track.nodes.get(startIdx > 1 ? startIdx - 2 : startIdx - 1); + int diffElev = endElev - startElev; + dist += tmpPt.calcDistance(startPt); + dist += n.calcDistance(lastPt); + int distRest = dist; + double incline = diffElev / (dist / 100.); + String lastMsg = ""; + double tmpincline = 0; + double startincline = 0; + double selev = track.nodes.get(startIdx - 2).getSElev(); + boolean hasInclineTags = false; + for (int i = startIdx - 1; i < endIdx + 1; i++) { + OsmPathElement tmp = track.nodes.get(i); + if (tmp.message != null) { + MessageData md = tmp.message.copy(); + String msg = md.wayKeyValues; + if (!msg.equals(lastMsg)) { + boolean revers = msg.contains("reversedirection=yes"); + int pos = msg.indexOf("incline="); + if (pos != -1) { + hasInclineTags = true; + String s = msg.substring(pos + 8); + pos = s.indexOf(" "); + if (pos != -1) + s = s.substring(0, pos); + + if (s.length() > 0) { + try { + int ind = s.indexOf("%"); + if (ind != -1) + s = s.substring(0, ind); + ind = s.indexOf("�"); + if (ind != -1) + s = s.substring(0, ind); + tmpincline = Double.parseDouble(s.trim()); + if (revers) + tmpincline *= -1; + } catch (NumberFormatException e) { + tmpincline = 0; + } + } + } else { + tmpincline = 0; + } + if (startincline == 0) { + startincline = tmpincline; + } else if (startincline < 0 && tmpincline > 0) { + // for the way ?p find the exit point + double diff = endElev - selev; + tmpincline = diff / (distRest / 100.); + } + } + lastMsg = msg; + } + int tmpdist = tmp.calcDistance(tmpPt); + distRest -= tmpdist; + if (hasInclineTags) + incline = tmpincline; + selev = (selev + (tmpdist / 100. * incline)); + tmp.setSElev((short) selev); + tmp.message.ele = (short) selev; + tmpPt = tmp; + } + dist = 0; + } else if (n.getSElev() != Short.MIN_VALUE && lastElev == Short.MIN_VALUE && startIdx == 0) { + // fill at start + for (int i = 0; i < idx; i++) { + track.nodes.get(i).setSElev(n.getSElev()); + } + } else if (n.getSElev() == Short.MIN_VALUE && idx == track.nodes.size() - 1) { + // fill at end + for (int i = startIdx; i < track.nodes.size(); i++) { + track.nodes.get(i).setSElev(startElev); + } + } else if (n.getSElev() == Short.MIN_VALUE) { + if (lastPt != null) + dist += n.calcDistance(lastPt); + } + lastElev = n.getSElev(); + lastPt = n; + } + + } + private void logException(Throwable t) { errorMessage = t instanceof IllegalArgumentException ? t.getMessage() : t.toString(); logInfo("Error (linksProcessed=" + linksProcessed + " open paths: " + openSet.getSize() + "): " + errorMessage); @@ -389,6 +502,8 @@ public class RoutingEngine extends Thread { lastTracks[i] = seg; } + postElevationCheck(totaltrack); + recalcTrack(totaltrack); matchedWaypoints.get(matchedWaypoints.size() - 1).indexInTrack = totaltrack.nodes.size() - 1; @@ -1315,7 +1430,7 @@ public class RoutingEngine extends Thread { } OsmPathElement nextElement = element.origin; // ignore double element - if (nextElement != null && nextElement.equals(element)) { + if (nextElement != null && nextElement.positionEquals(element)) { element = nextElement; continue; }