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