Fabio/brouter
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions 1

Review

Browse source 
* Only call speed computation for the final segments.
* Replace the cubic equation solved by a Newton method.
* Handle bicycle=dismount.
This commit is contained in:
Phyks (Lucas Verney) 2018-11-22 13:32:41 +01:00
parent e4d0b3adc5
commit 0cecf87102
4 changed files with 151 additions and 337 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

240
brouter-core/src/main/java/btools/router/Cubic.java
View file

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

40
brouter-core/src/main/java/btools/router/KinematicPath.java
View file

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

173
brouter-core/src/main/java/btools/router/OsmPath.java
View file

@ -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.;

35
brouter-core/src/main/java/btools/router/StdPath.java
View file

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