Travel time computation for bikes and foot

Based on simple models for travel speed (Tobler's hiking function for
foot, constant power for bike).

Fixes #6.

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

@@ -0,0 +1,240 @@
+//******************************************************************************
+//
+// File:    Cubic.java
+// Package: edu.rit.numeric
+// Unit:    Class edu.rit.numeric.Cubic
+//
+// This Java source file is copyright (C) 2008 by Alan Kaminsky. All rights
+// reserved. For further information, contact the author, Alan Kaminsky, at
+// ark@cs.rit.edu.
+//
+// This Java source file is part of the Parallel Java Library ("PJ"). PJ is free
+// software; you can redistribute it and/or modify it under the terms of the GNU
+// General Public License as published by the Free Software Foundation; either
+// version 3 of the License, or (at your option) any later version.
+//
+// PJ is distributed in the hope that it will be useful, but WITHOUT ANY
+// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
+// A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+//
+// Linking this library statically or dynamically with other modules is making a
+// combined work based on this library. Thus, the terms and conditions of the
+// GNU General Public License cover the whole combination.
+//
+// As a special exception, the copyright holders of this library give you
+// permission to link this library with independent modules to produce an
+// executable, regardless of the license terms of these independent modules, and
+// to copy and distribute the resulting executable under terms of your choice,
+// provided that you also meet, for each linked independent module, the terms
+// and conditions of the license of that module. An independent module is a
+// module which is not derived from or based on this library. If you modify this
+// library, you may extend this exception to your version of the library, but
+// you are not obligated to do so. If you do not wish to do so, delete this
+// exception statement from your version.
+//
+// A copy of the GNU General Public License is provided in the file gpl.txt. You
+// may also obtain a copy of the GNU General Public License on the World Wide
+// Web at http://www.gnu.org/licenses/gpl.html.
+//
+//******************************************************************************
+
+package btools.router;
+
+/**
+ * Class Cubic solves for the real roots of a cubic equation with real
+ * coefficients. The cubic equation is of the form
+ * <P>
+ * <I>ax</I><SUP>3</SUP> + <I>bx</I><SUP>2</SUP> + <I>cx</I> + <I>d</I> = 0
+ * <P>
+ * To solve a cubic equation, construct an instance of class Cubic; call the
+ * Cubic object's <TT>solve()</TT> method, passing in the coefficients <I>a</I>,
+ * <I>b</I>, <I>c</I>, and <I>d</I>; and obtain the roots from the Cubic
+ * object's fields. The number of (real) roots, either 1 or 3, is stored in
+ * field <TT>nRoots</TT>. If there is one root, it is stored in field
+ * <TT>x1</TT>, and fields <TT>x2</TT> and <TT>x3</TT> are set to NaN. If there
+ * are three roots, they are stored in fields <TT>x1</TT>, <TT>x2</TT>, and
+ * <TT>x3</TT> in descending order.
+ * <P>
+ * The same Cubic object may be used to solve several cubic equations. Each time
+ * the <TT>solve()</TT> method is called, the solution is stored in the Cubic
+ * object's fields.
+ * <P>
+ * The formulas for the roots of a cubic equation come from:
+ * <P>
+ * E. Weisstein. "Cubic formula." From <I>MathWorld</I>--A Wolfram Web Resource.
+ * <A HREF="http://mathworld.wolfram.com/CubicFormula.html" TARGET="_top">http://mathworld.wolfram.com/CubicFormula.html</A>
+ *
+ * @author  Alan Kaminsky
+ * @version 02-Feb-2008
+ */
+public class Cubic
+    {
+
+// Hidden constants.
+
+    private static final double TWO_PI = 2.0 * Math.PI;
+    private static final double FOUR_PI = 4.0 * Math.PI;
+
+// Exported fields.
+
+    /**
+     * The number of real roots.
+     */
+    public int nRoots;
+
+    /**
+     * The first real root.
+     */
+    public double x1;
+
+    /**
+     * The second real root.
+     */
+    public double x2;
+
+    /**
+     * The third real root.
+     */
+    public double x3;
+
+// Exported constructors.
+
+    /**
+     * Construct a new Cubic object.
+     */
+    public Cubic()
+        {
+        }
+
+// Exported operations.
+
+    /**
+     * Solve the cubic equation with the given coefficients. The results are
+     * stored in this Cubic object's fields.
+     *
+     * @param  a  Coefficient of <I>x</I><SUP>3</SUP>.
+     * @param  b  Coefficient of <I>x</I><SUP>2</SUP>.
+     * @param  c  Coefficient of <I>x</I>.
+     * @param  d  Constant coefficient.
+     *
+     * @exception  IllegalArgumentException
+     *     (unchecked exception) Thrown if <TT>a</TT> is 0; in other words, the
+     *     coefficients do not represent a cubic equation.
+     */
+    public void solve
+        (double a,
+         double b,
+         double c,
+         double d)
+        {
+        // Verify preconditions.
+        if (a == 0.0)
+            {
+            throw new IllegalArgumentException ("Cubic.solve(): a = 0");
+            }
+
+        // Normalize coefficients.
+        double denom = a;
+        a = b/denom;
+        b = c/denom;
+        c = d/denom;
+
+        // Commence solution.
+        double a_over_3 = a / 3.0;
+        double Q = (3*b - a*a) / 9.0;
+        double Q_CUBE = Q*Q*Q;
+        double R = (9*a*b - 27*c - 2*a*a*a) / 54.0;
+        double R_SQR = R*R;
+        double D = Q_CUBE + R_SQR;
+
+        if (D < 0.0)
+            {
+            // Three unequal real roots.
+            nRoots = 3;
+            double theta = Math.acos (R / Math.sqrt (-Q_CUBE));
+            double SQRT_Q = Math.sqrt (-Q);
+            x1 = 2.0 * SQRT_Q * Math.cos (theta/3.0) - a_over_3;
+            x2 = 2.0 * SQRT_Q * Math.cos ((theta+TWO_PI)/3.0) - a_over_3;
+            x3 = 2.0 * SQRT_Q * Math.cos ((theta+FOUR_PI)/3.0) - a_over_3;
+            sortRoots();
+            }
+        else if (D > 0.0)
+            {
+            // One real root.
+            nRoots = 1;
+            double SQRT_D = Math.sqrt (D);
+            double S = Math.cbrt (R + SQRT_D);
+            double T = Math.cbrt (R - SQRT_D);
+            x1 = (S + T) - a_over_3;
+            x2 = Double.NaN;
+            x3 = Double.NaN;
+            }
+        else
+            {
+            // Three real roots, at least two equal.
+            nRoots = 3;
+            double CBRT_R = Math.cbrt (R);
+            x1 = 2*CBRT_R - a_over_3;
+            x2 = x3 = CBRT_R - a_over_3;
+            sortRoots();
+            }
+        }
+
+// Hidden operations.
+
+    /**
+     * Sort the roots into descending order.
+     */
+    private void sortRoots()
+        {
+        if (x1 < x2)
+            {
+            double tmp = x1; x1 = x2; x2 = tmp;
+            }
+        if (x2 < x3)
+            {
+            double tmp = x2; x2 = x3; x3 = tmp;
+            }
+        if (x1 < x2)
+            {
+            double tmp = x1; x1 = x2; x2 = tmp;
+            }
+        }
+
+// Unit test main program.
+
+    /**
+     * Unit test main program.
+     * <P>
+     * Usage: java edu.rit.numeric.Cubic <I>a</I> <I>b</I> <I>c</I> <I>d</I>
+     */
+    public static void main
+        (String[] args)
+        throws Exception
+        {
+        if (args.length != 4) usage();
+        double a = Double.parseDouble (args[0]);
+        double b = Double.parseDouble (args[1]);
+        double c = Double.parseDouble (args[2]);
+        double d = Double.parseDouble (args[3]);
+        Cubic cubic = new Cubic();
+        cubic.solve (a, b, c, d);
+        System.out.println ("x1 = " + cubic.x1);
+        if (cubic.nRoots == 3)
+            {
+            System.out.println ("x2 = " + cubic.x2);
+            System.out.println ("x3 = " + cubic.x3);
+            }
+        }
+
+    /**
+     * Print a usage message and exit.
+     */
+    private static void usage()
+        {
+        System.err.println ("Usage: java edu.rit.numeric.Cubic <a> <b> <c> <d>");
+        System.err.println ("Solves ax^3 + bx^2 + cx + d = 0");
+        System.exit (1);
+        }
+
+    }

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

@@ -12,6 +12,10 @@ import btools.mapaccess.TurnRestriction;
 
 final class StdPath extends OsmPath
 {
+  private double totalTime;  // travel time (seconds)
+  // Gravitational constant, g
+  private double GRAVITY = 9.81;  // in meters per second^(-2)
+
   /**
    * The elevation-hysteresis-buffer (0-10 m)
    */
@@ -24,6 +28,7 @@ final class StdPath extends OsmPath
     StdPath origin = (StdPath)orig;
     this.ehbd = origin.ehbd;
     this.ehbu = origin.ehbu;
+    this.totalTime = origin.totalTime;
   }
 
   @Override
@@ -31,6 +36,7 @@ final class StdPath extends OsmPath
   {
     ehbd = 0;
     ehbu = 0;
+    totalTime = 0;
   }
 
   @Override
@@ -139,7 +145,31 @@ final class StdPath extends OsmPath
     }
 
     sectionCost += dist * costfactor + 0.5f;
-      
+
+    if (rc.bikeMode || rc.footMode) {
+      // Uphill angle
+      double alpha = Math.atan2(delta_h, distance);
+
+      // Travel speed
+      double speed = Double.NaN;
+      if (rc.footMode) { // TODO: || tags['way'].search('bicycle=dismount') !== -1) {
+        // Use Tobler's hiking function for walking sections
+        speed = 6 * Math.exp(-3.5 * Math.abs(delta_h / distance + 0.05)) / 3.6;
+      } else {
+        // Compute the speed assuming a basic kinematic model with constant
+        // power.
+        Cubic speedEquation = new Cubic();
+        speedEquation.solve(rc.S_C_x, 0.0, (rc.bikeMass * GRAVITY * (rc.defaultC_r + Math.sin(alpha))), -1.0 * rc.bikerPower);
+        if (speedEquation.nRoots > 0 && speedEquation.x1 >= 0) {
+          // Roots are sorted in decreasing order
+          speed = Math.min(speedEquation.x1, rc.maxSpeed);
+        }
+      }
+      if (!Double.isNaN(speed) && speed > 0) {
+        totalTime += distance / speed;
+      }
+    }
+
     return sectionCost;
   }
 
@@ -589,8 +619,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;
+  }
 }