recalc elevation at end

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);

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("<EFBFBD>");
+                    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;
       }