bike ETA fine-tuning

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

@@ -9,15 +9,11 @@ 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 )
   {
@@ -258,18 +254,6 @@ final class KinematicPath extends OsmPath
     if ( ekin > e ) ekin = e;
   }
 
-  private static double exp( double e )
-  {
-    double x = e;
-    double f = 1.;
-    while( e < -1. )
-    {
-      e += 1.;
-      f *= 0.367879;
-    }
-    return f*( 1. + x*( 1. + x * ( 0.5 +  x * ( 0.166667 + 0.0416667 * x) ) ) );
-  }
-
 
   @Override
   public int elevationCorrection( RoutingContext rc )
@@ -286,13 +270,13 @@ final class KinematicPath extends OsmPath
 	  return cost > c + 100;
   }
 
-
-
+  @Override
   public double getTotalTime()
   {
     return totalTime;
   }
 
+  @Override
   public double getTotalEnergy()
   {
     return totalEnergy;

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

@@ -50,12 +50,6 @@ 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;
@@ -375,6 +369,9 @@ abstract class OsmPath implements OsmLinkHolder
         }
       }
 
+      
+
+
       double elevation = ele2 == Short.MIN_VALUE ? 100. : ele2/4.;
 
       double sectionCost = processWaySection( rc, dist, delta_h, elevation, angle, cosangle, isStartpoint, nsection, lastpriorityclassifier );
@@ -404,68 +401,6 @@ abstract class OsmPath implements OsmLinkHolder
         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;
@@ -555,6 +490,10 @@ abstract class OsmPath implements OsmLinkHolder
 
   protected abstract double processTargetNode( RoutingContext rc );
 
+  protected void computeKinematic( RoutingContext rc, double dist, double delta_h, boolean detailMode )
+  {
+  }
+
   public abstract int elevationCorrection( RoutingContext rc );
 
   public abstract boolean definitlyWorseThan( OsmPath p, RoutingContext rc );
@@ -587,11 +526,23 @@ abstract class OsmPath implements OsmLinkHolder
 
   public double getTotalTime()
   {
-    return totalTime;
+    return 0.;
   }
 
   public double getTotalEnergy()
   {
     return 0.;
   }
+
+  protected static double exp( double e )
+  {
+    double x = e;
+    double f = 1.;
+    while( e < -1. )
+    {
+      e += 1.;
+      f *= 0.367879;
+    }
+    return f*( 1. + x*( 1. + x * ( 0.5 +  x * ( 0.166667 + 0.0416667 * x) ) ) );
+  }
 }

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

@@ -18,10 +18,12 @@ final class StdPath extends OsmPath
   private int ehbd; // in micrometer
   private int ehbu; // in micrometer
 
-  StdPath() {
-    super();
-    computeTime = true;
-  }
+  private float totalTime;  // travel time (seconds)
+  private float totalEnergy; // total route energy (Joule)
+  private float elevation_buffer; // just another elevation buffer (for travel time)
+
+  // Gravitational constant, g
+  private static final double GRAVITY = 9.81;  // in meters per second^(-2)
 
   @Override
   public void init( OsmPath orig )
@@ -30,6 +32,8 @@ final class StdPath extends OsmPath
     this.ehbd = origin.ehbd;
     this.ehbu = origin.ehbu;
     this.totalTime = origin.totalTime;
+    this.totalEnergy = origin.totalEnergy;
+    this.elevation_buffer = origin.elevation_buffer;
   }
 
   @Override
@@ -37,7 +41,9 @@ final class StdPath extends OsmPath
   {
     ehbd = 0;
     ehbu = 0;
-    totalTime = 0.;
+    totalTime = 0.f;
+    totalEnergy = 0.f;
+    elevation_buffer = 0.f;
   }
 
   @Override
@@ -600,9 +606,76 @@ final class StdPath extends OsmPath
 	  return cost > c;
   }
 
+  @Override
+  protected void computeKinematic( RoutingContext rc, double dist, double delta_h, boolean detailMode )
+  {
+    if ( !detailMode )
+    {
+      return;
+    }
+    
+    // compute incline
+    double decayFactor = exp( - dist / 100. );
+    elevation_buffer += delta_h;
+    float new_elevation_buffer = (float)( elevation_buffer * decayFactor );
+    double incline = ( elevation_buffer - new_elevation_buffer ) / dist;
+    elevation_buffer = new_elevation_buffer;
+    
+    double speed; // Travel speed
+    if (rc.footMode )
+    {
+      // Use Tobler's hiking function for walking sections
+      speed = 6 * Math.exp(-3.5 * Math.abs( incline + 0.05)) / 3.6;
+    }
+    else if (rc.bikeMode)
+    {
+      speed = solveCubic( rc.S_C_x, rc.bikeMass * GRAVITY * ( rc.defaultC_r + incline ), rc.bikerPower );
+      speed = Math.min(speed, rc.maxSpeed);
+    }
+    else
+    {
+      return;
+    }
+    float dt = (float) ( dist / speed );
+    totalTime += dt;
+    totalEnergy += dt*rc.bikerPower;
+  }
+
+  private static double solveCubic( double a, double c, double d )
+  {
+    // Solves a * v^3 + c * v = d with a Newton method
+    // to get the speed v for the section.
+
+    double v = 8.;
+    boolean findingStartvalue = true;
+    for ( int i = 0; i < 10; i++ )
+    {
+      double y = ( a * v * v + c ) * v - d;
+      if ( y < .1 )
+      {
+        if ( findingStartvalue )
+        {
+          v *= 2.;
+          continue;
+        }
+        break;
+      }
+      findingStartvalue = false;
+      double y_prime = 3 * a * v * v + c;
+      v -= y / y_prime;
+    }
+    return v;
+  }
+
   @Override
   public double getTotalTime()
   {
     return totalTime;
   }
+
+  @Override
+  public double getTotalEnergy()
+  {
+    return totalEnergy;
+  }
 }