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; + } }