diff --git a/brouter-core/src/main/java/btools/router/RoutingEngine.java b/brouter-core/src/main/java/btools/router/RoutingEngine.java index 34f511c..71e3602 100644 --- a/brouter-core/src/main/java/btools/router/RoutingEngine.java +++ b/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 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 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 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..