Merge pull request #125 from Phyks/travelTime

Travel time computation for bikes and foot

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;

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 )
+      String wayKeyValues = "";
-      {
+      if ( message != null ) {
-        message.turnangle = (float)angle;
+        wayKeyValues = rc.expctxWay.getKeyValueDescription( isReverse, description );
-        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 );
       }
 
-      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 );
+          // Use Tobler's hiking function for walking sections
-          originElement.cost = cost;
+          speed = 6 * Math.exp(-3.5 * Math.abs(elevation / dist + 0.05)) / 3.6;
-          if ( message != null )
+        }
+        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 )
+        if ( message != null )
-      {
-        // *** penalty for being part of the reference track
-        if ( refTrack != null && refTrack.containsNode( targetNode ) && refTrack.containsNode( sourceNode ) )
         {
-          int reftrackcost = linkdisttotal;
+          message.turnangle = (float)angle;
-          cost += reftrackcost;
+          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 )
+        if ( stopAtEndpoint )
-      {
+        {
-        originElement = OsmPathElement.create( lon2, lat2, ele2, originElement, rc.countTraffic );
+          if ( recordTransferNodes )
-        originElement.cost = cost;
+          {
-        originElement.addTraffic( traffic );
+            originElement = OsmPathElement.create( rc.ilonshortest, rc.ilatshortest, ele2, originElement, rc.countTraffic );
-        traffic = 0;
+            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.;

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

@@ -34,7 +34,7 @@ public final class RoutingContext
   public Map<String,String> 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<OsmNodeNamed> 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<OsmNodeNamed> 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;
   }
-  
+
 }

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;
+  }
 }