recalc track at end

brouter-core/src/main/java/btools/router/RoutingEngine.java

@@ -8,7 +8,11 @@ import java.io.StringWriter;
 import java.io.Writer;
 import java.util.ArrayList;
 import java.util.Date;
+import java.util.HashMap;
 import java.util.List;
+import java.util.Map;
+import java.util.SortedSet;
+import java.util.TreeSet;
 
 import btools.mapaccess.MatchedWaypoint;
 import btools.mapaccess.NodesCache;
@@ -383,6 +387,8 @@ public class RoutingEngine extends Thread {
       lastTracks[i] = seg;
     }
 
+    recalcTrack(totaltrack);
+
     totaltrack.matchedWaypoints = matchedWaypoints;
     totaltrack.processVoiceHints(routingContext);
     totaltrack.prepareSpeedProfile(routingContext);
@@ -559,6 +565,136 @@ public class RoutingEngine extends Thread {
     newTarget.message.turnangle = (float) angle;
   }
 
+  private void recalcTrack(OsmTrack t) {
+
+    int i = 0;
+
+    int lon0,
+      lat0,
+      lon1,
+      lat1,
+      lon2,
+      lat2;
+    int totaldist = 0;
+    int totaltime = 0;
+    float lasttime = 0;
+    float lastenergy = 0;
+    float speed_avg = 0;
+    float speed_min = 9999;
+    Map<Integer, Integer> directMap = new HashMap<>();
+    float tmptime = 1;
+    float speed = 1;
+    int dist;
+    double angle;
+
+    double ascend = 0;
+    double ehb = 0.;
+    int ourSize = t.nodes.size();
+
+    short ele_start = Short.MIN_VALUE;
+    short ele_end = Short.MIN_VALUE;
+    double eleFactor = routingContext.inverseRouting ? 0.25 : -0.25;
+
+    for (i = 0; i < ourSize; i++) {
+      OsmPathElement n = t.nodes.get(i);
+      if (n.message == null) n.message = new MessageData();
+      OsmPathElement nLast = null;
+      if (i == 0) {
+        lon0 = t.nodes.get(0).getILon();
+        lat0 = t.nodes.get(0).getILat();
+        lon1 = t.nodes.get(0).getILat();
+        lat1 = t.nodes.get(0).getILat();
+        lon2 = t.nodes.get(i).getILon();
+        lat2 = t.nodes.get(i).getILat();
+        dist = 0;
+      } else if (i == 1) {
+        lon0 = t.nodes.get(0).getILon();
+        lat0 = t.nodes.get(0).getILat();
+        lon1 = t.nodes.get(1).getILon();
+        lat1 = t.nodes.get(1).getILat();
+        lon2 = t.nodes.get(i).getILon();
+        lat2 = t.nodes.get(i).getILat();
+        nLast = t.nodes.get(0);
+        dist = routingContext.calcDistance(lon0, lat0, lon1, lat1);
+      } else {
+        lon0 = t.nodes.get(i - 2).getILon();
+        lat0 = t.nodes.get(i - 2).getILat();
+        lon1 = t.nodes.get(i - 1).getILon();
+        lat1 = t.nodes.get(i - 1).getILat();
+        lon2 = t.nodes.get(i).getILon();
+        lat2 = t.nodes.get(i).getILat();
+        nLast = t.nodes.get(i - 1);
+        dist = routingContext.calcDistance(lon1, lat1, lon2, lat2);
+      }
+      angle = routingContext.anglemeter.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2);
+      n.message.linkdist = dist;
+      n.message.turnangle = (float) angle;
+      totaldist += dist;
+      totaltime += n.getTime();
+      tmptime = (n.getTime() - lasttime);
+      if (dist > 0) {
+        speed = dist / tmptime * 3.6f;
+        speed_min = Math.min(speed_min, speed);
+      }
+      if (tmptime == 1.f) { // no time used here
+        directMap.put(i, dist);
+      }
+
+      lastenergy = n.getEnergy();
+      lasttime = n.getTime();
+
+      short ele = n.getSElev();
+      if (ele != Short.MIN_VALUE)
+        ele_end = ele;
+      if (ele_start == Short.MIN_VALUE)
+        ele_start = ele;
+
+      if (nLast != null) {
+        short ele_last = nLast.getSElev();
+        if (ele_last != Short.MIN_VALUE) {
+          ehb = ehb + (ele_last - ele) * eleFactor;
+        }
+        if (ehb > 10.) {
+          ascend += ehb - 10.;
+          ehb = 10.;
+        } else if (ehb < 0.) {
+          ehb = 0.;
+        }
+      }
+
+    }
+    ascend += ehb;
+
+    t.ascend = (int) ascend;
+    t.plainAscend = (int) ((ele_start - ele_end) * eleFactor + 0.5);
+
+    t.distance = totaldist;
+    //t.energy = totalenergy;
+
+    SortedSet<Integer> keys = new TreeSet<>(directMap.keySet());
+    for (Integer key : keys) {
+      int value = directMap.get(key);
+      float addTime = (value / (speed_min / 3.6f));
+
+      double addEnergy = 0;
+      if (key < ourSize - 1) {
+        double GRAVITY = 9.81;  // in meters per second^(-2)
+        double incline = (t.nodes.get(key).getElev() - t.nodes.get(key + 1).getElev()) / value;
+        double f_roll = routingContext.totalMass * GRAVITY * (routingContext.defaultC_r + incline);
+        double spd = speed_min / 3.6;
+        addEnergy = value * (routingContext.S_C_x * spd * spd + f_roll);
+      }
+      for (int j = key; j < ourSize; j++) {
+        OsmPathElement n = t.nodes.get(j);
+        n.setTime(n.getTime() + addTime);
+        n.setEnergy(n.getEnergy() + (float) addEnergy);
+      }
+    }
+
+    logInfo("track-length total = " + t.distance);
+    logInfo("filtered ascend = " + t.ascend);
+  }
+
   // geometric position matching finding the nearest routable way-section
   private void matchWaypointsToNodes(List<MatchedWaypoint> unmatchedWaypoints) {
     resetCache(false);
@@ -650,6 +786,8 @@ public class RoutingEngine extends Thread {
     }
     if (track == null) throw new IllegalArgumentException("no track found");
 
+    OsmPathElement lastElement = null;
+
     boolean wasClean = nearbyTrack != null && !nearbyTrack.isDirty;
     if (refTrack == null && !(wasClean && isDirty)) // do not overwrite a clean with a dirty track
     {
@@ -1166,18 +1304,18 @@ public class RoutingEngine extends Thread {
     track.energy = (int) path.getTotalEnergy();
 
     int distance = 0;
-    double ascend = 0;
-    double ehb = 0.;
-
-    short ele_start = Short.MIN_VALUE;
-    short ele_end = Short.MIN_VALUE;
 
     double eleFactor = routingContext.inverseRouting ? -0.25 : 0.25;
     while (element != null) {
       if (guideTrack != null && element.message == null) {
         element.message = new MessageData();
       }
-
+      OsmPathElement nextElement = element.origin;
+      // ignore double element
+      if (nextElement != null && nextElement.equals(element)) {
+        element = nextElement;
+        continue;
+      }
       if (routingContext.inverseRouting) {
         element.setTime(totalTime - element.getTime());
         element.setEnergy(totalEnergy - element.getEnergy());
@@ -1186,33 +1324,13 @@ public class RoutingEngine extends Thread {
         track.nodes.add(0, element);
       }
 
-      OsmPathElement nextElement = element.origin;
-
-      short ele = element.getSElev();
-      if (ele != Short.MIN_VALUE) ele_start = ele;
-      if (ele_end == Short.MIN_VALUE) ele_end = ele;
-
       if (nextElement != null) {
         distance += element.calcDistance(nextElement);
-        short ele_next = nextElement.getSElev();
-        if (ele_next != Short.MIN_VALUE) {
-          ehb = ehb + (ele - ele_next) * eleFactor;
-        }
-        if (ehb > 10.) {
-          ascend += ehb - 10.;
-          ehb = 10.;
-        } else if (ehb < 0.) {
-          ehb = 0.;
-        }
       }
       element = nextElement;
     }
-    ascend += ehb;
     track.distance = distance;
-    track.ascend = (int) ascend;
-    track.plainAscend = (int) ((ele_end - ele_start) * eleFactor + 0.5);
     logInfo("track-length = " + track.distance);
-    logInfo("filtered ascend = " + track.ascend);
     track.buildMap();
 
     // for final track..