Review

* Only call speed computation for the final segments.
* Replace the cubic equation solved by a Newton method.
* Handle bicycle=dismount.

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

@@ -1,240 +0,0 @@
-//******************************************************************************
-//
-// 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/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/StdPath.java

@@ -12,16 +12,17 @@ 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)
    */
   private int ehbd; // in micrometer
   private int ehbu; // in micrometer
 
+  StdPath() {
+    super();
+    computeTime = true;
+  }
+
   @Override
   public void init( OsmPath orig )
   {
@@ -36,7 +37,7 @@ final class StdPath extends OsmPath
   {
     ehbd = 0;
     ehbu = 0;
-    totalTime = 0;
+    totalTime = 0.;
   }
 
   @Override
@@ -146,30 +147,6 @@ 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;
   }