From e4d0b3adc537e878778306266bb9974cd070126b Mon Sep 17 00:00:00 2001 From: "Phyks (Lucas Verney)" Date: Sun, 18 Nov 2018 19:56:00 +0100 Subject: [PATCH 1/2] 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. --- .../src/main/java/btools/router/Cubic.java | 240 ++++++++++++++++++ .../java/btools/router/RoutingContext.java | 49 +++- .../src/main/java/btools/router/StdPath.java | 41 ++- 3 files changed, 314 insertions(+), 16 deletions(-) create mode 100644 brouter-core/src/main/java/btools/router/Cubic.java diff --git a/brouter-core/src/main/java/btools/router/Cubic.java b/brouter-core/src/main/java/btools/router/Cubic.java new file mode 100644 index 0000000..675d91e --- /dev/null +++ b/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 + *

+ * ax3 + bx2 + cx + d = 0 + *

+ * To solve a cubic equation, construct an instance of class Cubic; call the + * Cubic object's solve() method, passing in the coefficients a, + * b, c, and d; and obtain the roots from the Cubic + * object's fields. The number of (real) roots, either 1 or 3, is stored in + * field nRoots. If there is one root, it is stored in field + * x1, and fields x2 and x3 are set to NaN. If there + * are three roots, they are stored in fields x1, x2, and + * x3 in descending order. + *

+ * The same Cubic object may be used to solve several cubic equations. Each time + * the solve() method is called, the solution is stored in the Cubic + * object's fields. + *

+ * The formulas for the roots of a cubic equation come from: + *

+ * E. Weisstein. "Cubic formula." From MathWorld--A Wolfram Web Resource. + * http://mathworld.wolfram.com/CubicFormula.html + * + * @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 x3. + * @param b Coefficient of x2. + * @param c Coefficient of x. + * @param d Constant coefficient. + * + * @exception IllegalArgumentException + * (unchecked exception) Thrown if a 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. + *

+ * Usage: java edu.rit.numeric.Cubic a b c d + */ + 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 "); + System.err.println ("Solves ax^3 + bx^2 + cx + d = 0"); + System.exit (1); + } + + } diff --git a/brouter-core/src/main/java/btools/router/RoutingContext.java b/brouter-core/src/main/java/btools/router/RoutingContext.java index 01ee5e2..7489425 100644 --- a/brouter-core/src/main/java/btools/router/RoutingContext.java +++ b/brouter-core/src/main/java/btools/router/RoutingContext.java @@ -34,7 +34,7 @@ public final class RoutingContext public Map 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 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 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; } - + } diff --git a/brouter-core/src/main/java/btools/router/StdPath.java b/brouter-core/src/main/java/btools/router/StdPath.java index c1a8524..7d7c57b 100644 --- a/brouter-core/src/main/java/btools/router/StdPath.java +++ b/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; + } } From 0cecf87102c497feb2ead77bceb391aedb10fea4 Mon Sep 17 00:00:00 2001 From: "Phyks (Lucas Verney)" Date: Thu, 22 Nov 2018 13:32:41 +0100 Subject: [PATCH 2/2] Review * Only call speed computation for the final segments. * Replace the cubic equation solved by a Newton method. * Handle bicycle=dismount. --- .../src/main/java/btools/router/Cubic.java | 240 ------------------ .../java/btools/router/KinematicPath.java | 40 +-- .../src/main/java/btools/router/OsmPath.java | 173 +++++++++---- .../src/main/java/btools/router/StdPath.java | 35 +-- 4 files changed, 151 insertions(+), 337 deletions(-) delete mode 100644 brouter-core/src/main/java/btools/router/Cubic.java diff --git a/brouter-core/src/main/java/btools/router/Cubic.java b/brouter-core/src/main/java/btools/router/Cubic.java deleted file mode 100644 index 675d91e..0000000 --- a/brouter-core/src/main/java/btools/router/Cubic.java +++ /dev/null @@ -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 - *

- * ax3 + bx2 + cx + d = 0 - *

- * To solve a cubic equation, construct an instance of class Cubic; call the - * Cubic object's solve() method, passing in the coefficients a, - * b, c, and d; and obtain the roots from the Cubic - * object's fields. The number of (real) roots, either 1 or 3, is stored in - * field nRoots. If there is one root, it is stored in field - * x1, and fields x2 and x3 are set to NaN. If there - * are three roots, they are stored in fields x1, x2, and - * x3 in descending order. - *

- * The same Cubic object may be used to solve several cubic equations. Each time - * the solve() method is called, the solution is stored in the Cubic - * object's fields. - *

- * The formulas for the roots of a cubic equation come from: - *

- * E. Weisstein. "Cubic formula." From MathWorld--A Wolfram Web Resource. - * http://mathworld.wolfram.com/CubicFormula.html - * - * @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 x3. - * @param b Coefficient of x2. - * @param c Coefficient of x. - * @param d Constant coefficient. - * - * @exception IllegalArgumentException - * (unchecked exception) Thrown if a 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. - *

- * Usage: java edu.rit.numeric.Cubic a b c d - */ - 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 "); - System.err.println ("Solves ax^3 + bx^2 + cx + d = 0"); - System.exit (1); - } - - } diff --git a/brouter-core/src/main/java/btools/router/KinematicPath.java b/brouter-core/src/main/java/btools/router/KinematicPath.java index 74f4075..144d781 100644 --- a/brouter-core/src/main/java/btools/router/KinematicPath.java +++ b/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; diff --git a/brouter-core/src/main/java/btools/router/OsmPath.java b/brouter-core/src/main/java/btools/router/OsmPath.java index fe0f8b3..65fe928 100644 --- a/brouter-core/src/main/java/btools/router/OsmPath.java +++ b/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 ) - { - message.turnangle = (float)angle; - 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 ); + String wayKeyValues = ""; + if ( message != null ) { + 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 ); - originElement.cost = cost; - if ( message != null ) + // 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) { - 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 ) - { - // *** penalty for being part of the reference track - if ( refTrack != null && refTrack.containsNode( targetNode ) && refTrack.containsNode( sourceNode ) ) + if ( message != null ) { - int reftrackcost = linkdisttotal; - cost += reftrackcost; + message.turnangle = (float)angle; + 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 ) - { - originElement = OsmPathElement.create( lon2, lat2, ele2, originElement, rc.countTraffic ); - originElement.cost = cost; - originElement.addTraffic( traffic ); - traffic = 0; + if ( stopAtEndpoint ) + { + if ( recordTransferNodes ) + { + originElement = OsmPathElement.create( rc.ilonshortest, rc.ilatshortest, ele2, originElement, rc.countTraffic ); + 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.; diff --git a/brouter-core/src/main/java/btools/router/StdPath.java b/brouter-core/src/main/java/btools/router/StdPath.java index 7d7c57b..a0ea647 100644 --- a/brouter-core/src/main/java/btools/router/StdPath.java +++ b/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; }