diff --git a/brouter-core/src/main/java/btools/router/KinematicPath.java b/brouter-core/src/main/java/btools/router/KinematicPath.java index 74f4075..144d781 100644 --- a/brouter-core/src/main/java/btools/router/KinematicPath.java +++ b/brouter-core/src/main/java/btools/router/KinematicPath.java @@ -9,11 +9,15 @@ package btools.router; final class KinematicPath extends OsmPath { private double ekin; // kinetic energy (Joule) - private double totalTime; // travel time (seconds) private double totalEnergy; // total route energy (Joule) private float floatingAngleLeft; // sliding average left bend (degree) private float floatingAngleRight; // sliding average right bend (degree) + KinematicPath() { + super(); + computeTime = false; // Time is already computed by the kinematic model. + } + @Override protected void init( OsmPath orig ) { @@ -35,12 +39,12 @@ final class KinematicPath extends OsmPath floatingAngleLeft = 0.f; floatingAngleRight = 0.f; } - + @Override protected double processWaySection( RoutingContext rc, double dist, double delta_h, double elevation, double angle, double cosangle, boolean isStartpoint, int nsection, int lastpriorityclassifier ) { KinematicModel km = (KinematicModel)rc.pm; - + double cost = 0.; double extraTime = 0.; @@ -64,7 +68,7 @@ final class KinematicPath extends OsmPath if ( angle < 0 ) floatingAngleLeft -= (float)angle; else floatingAngleRight += (float)angle; float aa = Math.max( floatingAngleLeft, floatingAngleRight ); - + if ( aa > 130. ) turnspeed = 0.; else if ( aa > 100. ) turnspeed = 1.; else if ( aa > 70. ) turnspeed = 2.; @@ -73,9 +77,9 @@ final class KinematicPath extends OsmPath else if ( aa > 20. ) turnspeed = 14.; else if ( aa > 10. ) turnspeed = 20.; } - + if ( nsection == 0 ) // process slowdown by crossing geometry - { + { int classifiermask = (int)rc.expctxWay.getClassifierMask(); // penalty for equal priority crossing @@ -85,12 +89,12 @@ final class KinematicPath extends OsmPath for( OsmPrePath prePath = rc.firstPrePath; prePath != null; prePath = prePath.next ) { KinematicPrePath pp = (KinematicPrePath)prePath; - + if ( ( (pp.classifiermask ^ classifiermask) & 8 ) != 0 ) // exactly one is linktype { continue; } - + if ( ( pp.classifiermask & 32 ) != 0 ) // touching a residential? { hasResidential = true; @@ -104,7 +108,7 @@ final class KinematicPath extends OsmPath } } double residentialSpeed = 13.; - + if ( hasLeftWay && turnspeed > km.leftWaySpeed ) turnspeed = km.leftWaySpeed; if ( hasRightWay && turnspeed > km.rightWaySpeed ) turnspeed = km.rightWaySpeed; if ( hasResidential && turnspeed > residentialSpeed ) turnspeed = residentialSpeed; @@ -141,7 +145,7 @@ final class KinematicPath extends OsmPath protected double evolveDistance( KinematicModel km, double dist, double delta_h, double f_air ) - { + { // elevation force double fh = delta_h * km.totalweight * 9.81 / dist; @@ -154,7 +158,7 @@ final class KinematicPath extends OsmPath double vb = km.getBreakingSpeed( effectiveSpeedLimit ); double elow = 0.5*km.totalweight*vb*vb; - double elapsedTime = 0.; + double elapsedTime = 0.; double dissipatedEnergy = 0.; double v = Math.sqrt( 2. * ekin / km.totalweight ); @@ -167,7 +171,7 @@ final class KinematicPath extends OsmPath double f = km.f_roll + f_air*v*v + fh; double f_recup = Math.max( 0., fast ? -f : (slow ? km.f_recup :0 ) -fh ); // additional recup for slow part f += f_recup; - + double delta_ekin; double timeStep; double x; @@ -215,7 +219,7 @@ final class KinematicPath extends OsmPath } dissipatedEnergy += elapsedTime * km.p_standby; - + totalTime += elapsedTime; totalEnergy += dissipatedEnergy + dist*fh; @@ -235,7 +239,7 @@ final class KinematicPath extends OsmPath if ( initialcost >= 1000000. ) { return -1.; - } + } cutEkin( km.totalweight, km.getNodeMaxspeed() ); // apply node maxspeed if ( message != null ) @@ -247,7 +251,7 @@ final class KinematicPath extends OsmPath } return 0.; } - + private void cutEkin( double weight, double speed ) { double e = 0.5*weight*speed*speed; @@ -264,7 +268,7 @@ final class KinematicPath extends OsmPath f *= 0.367879; } return f*( 1. + x*( 1. + x * ( 0.5 + x * ( 0.166667 + 0.0416667 * x) ) ) ); - } + } @Override @@ -281,14 +285,14 @@ final class KinematicPath extends OsmPath int c = p.cost; return cost > c + 100; } - + public double getTotalTime() { return totalTime; } - + public double getTotalEnergy() { return totalEnergy; diff --git a/brouter-core/src/main/java/btools/router/OsmPath.java b/brouter-core/src/main/java/btools/router/OsmPath.java index fe0f8b3..65fe928 100644 --- a/brouter-core/src/main/java/btools/router/OsmPath.java +++ b/brouter-core/src/main/java/btools/router/OsmPath.java @@ -25,7 +25,7 @@ abstract class OsmPath implements OsmLinkHolder public short selev; public int airdistance = 0; // distance to endpos - + protected OsmNode sourceNode; protected OsmNode targetNode; @@ -50,6 +50,12 @@ abstract class OsmPath implements OsmLinkHolder protected int priorityclassifier; + protected boolean computeTime = false; + protected double totalTime; // travel time (seconds) + // Gravitational constant, g + private double GRAVITY = 9.81; // in meters per second^(-2) + + private static final int PATH_START_BIT = 1; private static final int CAN_LEAVE_DESTINATION_BIT = 2; private static final int IS_ON_DESTINATION_BIT = 4; @@ -129,7 +135,7 @@ abstract class OsmPath implements OsmLinkHolder init( origin ); addAddionalPenalty(refTrack, detailMode, origin, link, rc ); } - + protected abstract void init( OsmPath orig ); protected abstract void resetState(); @@ -191,7 +197,7 @@ abstract class OsmPath implements OsmLinkHolder lastClassifier = newClassifier; lastInitialCost = newInitialCost; - // *** destination logic: no destination access in between + // *** destination logic: no destination access in between int classifiermask = (int)rc.expctxWay.getClassifierMask(); boolean newDestination = (classifiermask & 64) != 0; boolean oldDestination = getBit( IS_ON_DESTINATION_BIT ); @@ -217,14 +223,14 @@ abstract class OsmPath implements OsmLinkHolder } } setBit( IS_ON_DESTINATION_BIT, newDestination ); - + OsmTransferNode transferNode = link.geometry == null ? null : rc.geometryDecoder.decodeGeometry( link.geometry, sourceNode, targetNode, isReverse ); for(int nsection=0; ;nsection++) { - + originLon = lon1; originLat = lat1; @@ -300,7 +306,7 @@ abstract class OsmPath implements OsmLinkHolder } int dist = rc.calcDistance( lon1, lat1, lon2, lat2 ); - + boolean stopAtEndpoint = false; if ( rc.shortestmatch ) { @@ -393,63 +399,130 @@ abstract class OsmPath implements OsmLinkHolder traffic += dist*rc.expctxWay.getTrafficSourceDensity()*Math.pow(cost2/10000.f,rc.trafficSourceExponent); } - if ( message != null ) - { - message.turnangle = (float)angle; - message.time = (float)getTotalTime(); - message.energy = (float)getTotalEnergy(); - message.priorityclassifier = priorityclassifier; - message.classifiermask = classifiermask; - message.lon = lon2; - message.lat = lat2; - message.ele = ele2; - message.wayKeyValues = rc.expctxWay.getKeyValueDescription( isReverse, description ); + String wayKeyValues = ""; + if ( message != null ) { + wayKeyValues = rc.expctxWay.getKeyValueDescription( isReverse, description ); } - if ( stopAtEndpoint ) + // Only do speed computation for detailMode (final) and bike or foot modes. + if (detailMode && computeTime) { - if ( recordTransferNodes ) + // Travel speed + double speed = Double.NaN; + if (rc.footMode || (rc.bikeMode && wayKeyValues.contains("bicycle=dismount"))) { - originElement = OsmPathElement.create( rc.ilonshortest, rc.ilatshortest, ele2, originElement, rc.countTraffic ); - originElement.cost = cost; - if ( message != null ) + // Use Tobler's hiking function for walking sections + speed = 6 * Math.exp(-3.5 * Math.abs(elevation / dist + 0.05)) / 3.6; + } + else if (rc.bikeMode) + { + // Uphill angle + double alpha = Math.atan2(delta_h, dist); + + // Compute the speed assuming a basic kinematic model with constant + // power. + // Solves a * v^3 + b * v^2 + c * v + d = 0 with a Newton method to get + // the speed v for the section. + double a = rc.S_C_x; + double b = 0.0; + double c = (rc.bikeMass * GRAVITY * (rc.defaultC_r + Math.sin(alpha))); + double d = -1. * rc.bikerPower; + + double tolerance = 1e-3; + int max_count = 10; + + // Initial guess, this works rather well considering the allowed speeds. + speed = rc.maxSpeed; + double y = (a * speed * speed * speed) + (b * speed * speed) + (c * speed) + d; + double y_prime = (3 * a * speed * speed) + (2 * b * speed) + c; + + int i = 0; + for (i = 0; (Math.abs(y) > tolerance) && (i < max_count); i++) { + speed = speed - y / y_prime; + if (speed > rc.maxSpeed || speed <= 0) { + // No need to compute further, the speed is likely to be + // invalid or force set to maxspeed. + speed = rc.maxSpeed; + break; + } + y = (a * speed * speed * speed) + (b * speed * speed) + (c * speed) + d; + y_prime = (3 * a * speed * speed) + (2 * b * speed) + c; + } + + if (i == max_count) { - originElement.message = message; + // Newton method did not converge, speed is invalid. + speed = Double.NaN; + } + else + { + // Speed cannot exceed max speed + speed = Math.min(speed, rc.maxSpeed); } } - if ( rc.nogomatch ) + if (!Double.isNaN(speed) && speed > 0) { - cost = -1; + totalTime += dist / speed; } - return; } - if ( transferNode == null ) - { - // *** penalty for being part of the reference track - if ( refTrack != null && refTrack.containsNode( targetNode ) && refTrack.containsNode( sourceNode ) ) + if ( message != null ) { - int reftrackcost = linkdisttotal; - cost += reftrackcost; + message.turnangle = (float)angle; + message.time = (float)getTotalTime(); + message.energy = (float)getTotalEnergy(); + message.priorityclassifier = priorityclassifier; + message.classifiermask = classifiermask; + message.lon = lon2; + message.lat = lat2; + message.ele = ele2; + message.wayKeyValues = wayKeyValues; } - selev = ele2; - break; - } - transferNode = transferNode.next; - if ( recordTransferNodes ) - { - originElement = OsmPathElement.create( lon2, lat2, ele2, originElement, rc.countTraffic ); - originElement.cost = cost; - originElement.addTraffic( traffic ); - traffic = 0; + if ( stopAtEndpoint ) + { + if ( recordTransferNodes ) + { + originElement = OsmPathElement.create( rc.ilonshortest, rc.ilatshortest, ele2, originElement, rc.countTraffic ); + originElement.cost = cost; + if ( message != null ) + { + originElement.message = message; + } + } + if ( rc.nogomatch ) + { + cost = -1; + } + return; + } + + if ( transferNode == null ) + { + // *** penalty for being part of the reference track + if ( refTrack != null && refTrack.containsNode( targetNode ) && refTrack.containsNode( sourceNode ) ) + { + int reftrackcost = linkdisttotal; + cost += reftrackcost; + } + selev = ele2; + break; + } + transferNode = transferNode.next; + + if ( recordTransferNodes ) + { + originElement = OsmPathElement.create( lon2, lat2, ele2, originElement, rc.countTraffic ); + originElement.cost = cost; + originElement.addTraffic( traffic ); + traffic = 0; + } + lon0 = lon1; + lat0 = lat1; + lon1 = lon2; + lat1 = lat2; + ele1 = ele2; } - lon0 = lon1; - lat0 = lat1; - lon1 = lon2; - lat1 = lat2; - ele1 = ele2; - } // check for nogo-matches (after the *actual* start of segment) if ( rc.nogomatch ) @@ -514,9 +587,9 @@ abstract class OsmPath implements OsmLinkHolder public double getTotalTime() { - return 0.; + return totalTime; } - + public double getTotalEnergy() { return 0.; diff --git a/brouter-core/src/main/java/btools/router/RoutingContext.java b/brouter-core/src/main/java/btools/router/RoutingContext.java index 01ee5e2..7489425 100644 --- a/brouter-core/src/main/java/btools/router/RoutingContext.java +++ b/brouter-core/src/main/java/btools/router/RoutingContext.java @@ -34,7 +34,7 @@ public final class RoutingContext public Map keyValues; public String rawTrackPath; - + public String getProfileName() { String name = localFunction == null ? "unknown" : localFunction; @@ -46,17 +46,18 @@ public final class RoutingContext public BExpressionContextWay expctxWay; public BExpressionContextNode expctxNode; - + public GeometryDecoder geometryDecoder = new GeometryDecoder(); public int memoryclass = 64; - + public int downhillcostdiv; public int downhillcutoff; public int uphillcostdiv; public int uphillcutoff; public boolean carMode; public boolean bikeMode; + public boolean footMode; public boolean considerTurnRestrictions; public boolean processUnusedTags; public boolean forceSecondaryData; @@ -74,8 +75,8 @@ public final class RoutingContext public double inittimeadjustment; public double starttimeoffset; public boolean transitonly; - - + + private void setModel( String className ) { if ( className == null ) @@ -96,7 +97,7 @@ public final class RoutingContext } initModel(); } - + public void initModel() { pm.init( expctxWay, expctxNode, keyValues ); @@ -120,7 +121,7 @@ public final class RoutingContext BExpressionContext expctxGlobal = expctxWay; // just one of them... setModel( expctxGlobal._modelClass ); - + downhillcostdiv = (int)expctxGlobal.getVariableValue( "downhillcost", 0.f ); downhillcutoff = (int)(expctxGlobal.getVariableValue( "downhillcutoff", 0.f )*10000); uphillcostdiv = (int)expctxGlobal.getVariableValue( "uphillcost", 0.f ); @@ -129,6 +130,7 @@ public final class RoutingContext if ( uphillcostdiv != 0 ) uphillcostdiv = 1000000/uphillcostdiv; carMode = 0.f != expctxGlobal.getVariableValue( "validForCars", 0.f ); bikeMode = 0.f != expctxGlobal.getVariableValue( "validForBikes", 0.f ); + footMode = 0.f != expctxGlobal.getVariableValue( "validForFoot", 0.f ); // turn-restrictions used per default for car profiles considerTurnRestrictions = 0.f != expctxGlobal.getVariableValue( "considerTurnRestrictions", carMode ? 1.f : 0.f ); @@ -170,6 +172,20 @@ public final class RoutingContext } turnInstructionCatchingRange = expctxGlobal.getVariableValue( "turnInstructionCatchingRange", 40.f ); turnInstructionRoundabouts = expctxGlobal.getVariableValue( "turnInstructionRoundabouts", 1.f ) != 0.f; + + // Speed computation model (for bikes) + if (bikeMode) { + // Mass of the biker + bike + luggages, in kg + bikeMass = expctxGlobal.getVariableValue( "bikeMass", 90.f ); + // Max speed (before braking), in km/h in profile and m/s in code + maxSpeed = expctxGlobal.getVariableValue( "maxSpeed", 45.f ) / 3.6; + // Equivalent surface for wind, S * C_x, F = -1/2 * S * C_x * v^2 = - S_C_x * v^2 + S_C_x = expctxGlobal.getVariableValue( "S_C_x", 0.5f * 0.45f ); + // Default resistance of the road, F = - m * g * C_r (for good quality road) + defaultC_r = expctxGlobal.getVariableValue( "C_r", 0.01f ); + // Constant power of the biker (in W) + bikerPower = expctxGlobal.getVariableValue( "bikerPower", 100.f ); + } } public List nogopoints = null; @@ -202,13 +218,20 @@ public final class RoutingContext public boolean showspeed; public boolean inverseRouting; - + public OsmPrePath firstPrePath; public int turnInstructionMode; // 0=none, 1=auto, 2=locus, 3=osmand, 4=comment-style, 5=gpsies-style public double turnInstructionCatchingRange; public boolean turnInstructionRoundabouts; + // Speed computation model (for bikes) + public double bikeMass; + public double maxSpeed; + public double S_C_x; + public double defaultC_r; + public double bikerPower; + public static void prepareNogoPoints( List nogos ) { for( OsmNodeNamed nogo : nogos ) @@ -242,7 +265,7 @@ public final class RoutingContext { if ( wp.calcDistance( nogo ) < radiusInMeter && (!(nogo instanceof OsmNogoPolygon) - || (((OsmNogoPolygon)nogo).isClosed + || (((OsmNogoPolygon)nogo).isClosed ? ((OsmNogoPolygon)nogo).isWithin(wp.ilon, wp.ilat) : ((OsmNogoPolygon)nogo).isOnPolyline(wp.ilon, wp.ilat)))) { @@ -268,7 +291,7 @@ public final class RoutingContext } return cs; } - + public void setWaypoint( OsmNodeNamed wp, boolean endpoint ) { keepnogopoints = nogopoints; @@ -438,9 +461,9 @@ public final class RoutingContext double x4 = x2*x2; return x * ( 57.4539 + 9.57565 * x2 + 4.30904 * x4 + 2.56491 * x2*x4 ); } - + public OsmPathModel pm; - + public OsmPrePath createPrePath( OsmPath origin, OsmLink link ) { OsmPrePath p = pm.createPrePath(); @@ -464,5 +487,5 @@ public final class RoutingContext p.init( origin, link, refTrack, detailMode, this ); return p; } - + } diff --git a/brouter-core/src/main/java/btools/router/StdPath.java b/brouter-core/src/main/java/btools/router/StdPath.java index c1a8524..a0ea647 100644 --- a/brouter-core/src/main/java/btools/router/StdPath.java +++ b/brouter-core/src/main/java/btools/router/StdPath.java @@ -18,12 +18,18 @@ final class StdPath extends OsmPath private int ehbd; // in micrometer private int ehbu; // in micrometer + StdPath() { + super(); + computeTime = true; + } + @Override public void init( OsmPath orig ) { StdPath origin = (StdPath)orig; this.ehbd = origin.ehbd; this.ehbu = origin.ehbu; + this.totalTime = origin.totalTime; } @Override @@ -31,6 +37,7 @@ final class StdPath extends OsmPath { ehbd = 0; ehbu = 0; + totalTime = 0.; } @Override @@ -139,7 +146,7 @@ final class StdPath extends OsmPath } sectionCost += dist * costfactor + 0.5f; - + return sectionCost; } @@ -589,8 +596,13 @@ final class StdPath extends OsmPath int delta = p.ehbu - ehbu; if ( delta > 0 ) c += delta/rc.uphillcostdiv; } - + return cost > c; } - + + @Override + public double getTotalTime() + { + return totalTime; + } }