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

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

@@ -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;
@@ -393,6 +399,73 @@ abstract class OsmPath implements OsmLinkHolder
         traffic += dist*rc.expctxWay.getTrafficSourceDensity()*Math.pow(cost2/10000.f,rc.trafficSourceExponent);
       }
 
+      String wayKeyValues = "";
+      if ( message != null ) {
+        wayKeyValues = rc.expctxWay.getKeyValueDescription( isReverse, description );
+      }
+
+      // Only do speed computation for detailMode (final) and bike or foot modes.
+      if (detailMode && computeTime)
+      {
+        // Travel speed
+        double speed = Double.NaN;
+        if (rc.footMode || (rc.bikeMode && wayKeyValues.contains("bicycle=dismount")))
+        {
+          // 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)
+          {
+            // Newton method did not converge, speed is invalid.
+            speed = Double.NaN;
+          }
+          else
+          {
+            // Speed cannot exceed max speed
+            speed = Math.min(speed, rc.maxSpeed);
+          }
+        }
+        if (!Double.isNaN(speed) && speed > 0)
+        {
+          totalTime += dist / speed;
+        }
+      }
+
         if ( message != null )
         {
           message.turnangle = (float)angle;
@@ -403,7 +476,7 @@ abstract class OsmPath implements OsmLinkHolder
           message.lon = lon2;
           message.lat = lat2;
           message.ele = ele2;
-        message.wayKeyValues = rc.expctxWay.getKeyValueDescription( isReverse, description );
+          message.wayKeyValues = wayKeyValues;
         }
 
         if ( stopAtEndpoint )
@@ -514,7 +587,7 @@ abstract class OsmPath implements OsmLinkHolder
 
   public double getTotalTime()
   {
-    return 0.;
+    return totalTime;
   }
 
   public double getTotalEnergy()

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

@@ -57,6 +57,7 @@ public final class RoutingContext
   public int uphillcutoff;
   public boolean carMode;
   public boolean bikeMode;
+  public boolean footMode;
   public boolean considerTurnRestrictions;
   public boolean processUnusedTags;
   public boolean forceSecondaryData;
@@ -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;
@@ -209,6 +225,13 @@ public final class RoutingContext
   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 )

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
@@ -593,4 +600,9 @@ final class StdPath extends OsmPath
 	  return cost > c;
   }
 
+  @Override
+  public double getTotalTime()
+  {
+    return totalTime;
+  }
 }