diff --git a/brouter-core/src/main/java/btools/router/KinematicPath.java b/brouter-core/src/main/java/btools/router/KinematicPath.java index 144d781..5a23a8e 100644 --- a/brouter-core/src/main/java/btools/router/KinematicPath.java +++ b/brouter-core/src/main/java/btools/router/KinematicPath.java @@ -9,15 +9,11 @@ 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 ) { @@ -258,18 +254,6 @@ final class KinematicPath extends OsmPath if ( ekin > e ) ekin = e; } - private static double exp( double e ) - { - double x = e; - double f = 1.; - while( e < -1. ) - { - e += 1.; - f *= 0.367879; - } - return f*( 1. + x*( 1. + x * ( 0.5 + x * ( 0.166667 + 0.0416667 * x) ) ) ); - } - @Override public int elevationCorrection( RoutingContext rc ) @@ -286,13 +270,13 @@ final class KinematicPath extends OsmPath return cost > c + 100; } - - + @Override public double getTotalTime() { return totalTime; } + @Override 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 65fe928..0bf3455 100644 --- a/brouter-core/src/main/java/btools/router/OsmPath.java +++ b/brouter-core/src/main/java/btools/router/OsmPath.java @@ -50,12 +50,6 @@ 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; @@ -375,6 +369,9 @@ abstract class OsmPath implements OsmLinkHolder } } + + + double elevation = ele2 == Short.MIN_VALUE ? 100. : ele2/4.; double sectionCost = processWaySection( rc, dist, delta_h, elevation, angle, cosangle, isStartpoint, nsection, lastpriorityclassifier ); @@ -404,68 +401,6 @@ abstract class OsmPath implements OsmLinkHolder 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; @@ -555,6 +490,10 @@ abstract class OsmPath implements OsmLinkHolder protected abstract double processTargetNode( RoutingContext rc ); + protected void computeKinematic( RoutingContext rc, double dist, double delta_h, boolean detailMode ) + { + } + public abstract int elevationCorrection( RoutingContext rc ); public abstract boolean definitlyWorseThan( OsmPath p, RoutingContext rc ); @@ -587,11 +526,23 @@ abstract class OsmPath implements OsmLinkHolder public double getTotalTime() { - return totalTime; + return 0.; } public double getTotalEnergy() { return 0.; } + + protected static double exp( double e ) + { + double x = e; + double f = 1.; + while( e < -1. ) + { + e += 1.; + f *= 0.367879; + } + return f*( 1. + x*( 1. + x * ( 0.5 + x * ( 0.166667 + 0.0416667 * x) ) ) ); + } } diff --git a/brouter-core/src/main/java/btools/router/StdPath.java b/brouter-core/src/main/java/btools/router/StdPath.java index a0ea647..cff572c 100644 --- a/brouter-core/src/main/java/btools/router/StdPath.java +++ b/brouter-core/src/main/java/btools/router/StdPath.java @@ -18,10 +18,12 @@ final class StdPath extends OsmPath private int ehbd; // in micrometer private int ehbu; // in micrometer - StdPath() { - super(); - computeTime = true; - } + private float totalTime; // travel time (seconds) + private float totalEnergy; // total route energy (Joule) + private float elevation_buffer; // just another elevation buffer (for travel time) + + // Gravitational constant, g + private static final double GRAVITY = 9.81; // in meters per second^(-2) @Override public void init( OsmPath orig ) @@ -30,6 +32,8 @@ final class StdPath extends OsmPath this.ehbd = origin.ehbd; this.ehbu = origin.ehbu; this.totalTime = origin.totalTime; + this.totalEnergy = origin.totalEnergy; + this.elevation_buffer = origin.elevation_buffer; } @Override @@ -37,7 +41,9 @@ final class StdPath extends OsmPath { ehbd = 0; ehbu = 0; - totalTime = 0.; + totalTime = 0.f; + totalEnergy = 0.f; + elevation_buffer = 0.f; } @Override @@ -600,9 +606,76 @@ final class StdPath extends OsmPath return cost > c; } + @Override + protected void computeKinematic( RoutingContext rc, double dist, double delta_h, boolean detailMode ) + { + if ( !detailMode ) + { + return; + } + + // compute incline + double decayFactor = exp( - dist / 100. ); + elevation_buffer += delta_h; + float new_elevation_buffer = (float)( elevation_buffer * decayFactor ); + double incline = ( elevation_buffer - new_elevation_buffer ) / dist; + elevation_buffer = new_elevation_buffer; + + double speed; // Travel speed + if (rc.footMode ) + { + // Use Tobler's hiking function for walking sections + speed = 6 * Math.exp(-3.5 * Math.abs( incline + 0.05)) / 3.6; + } + else if (rc.bikeMode) + { + speed = solveCubic( rc.S_C_x, rc.bikeMass * GRAVITY * ( rc.defaultC_r + incline ), rc.bikerPower ); + speed = Math.min(speed, rc.maxSpeed); + } + else + { + return; + } + float dt = (float) ( dist / speed ); + totalTime += dt; + totalEnergy += dt*rc.bikerPower; + } + + private static double solveCubic( double a, double c, double d ) + { + // Solves a * v^3 + c * v = d with a Newton method + // to get the speed v for the section. + + double v = 8.; + boolean findingStartvalue = true; + for ( int i = 0; i < 10; i++ ) + { + double y = ( a * v * v + c ) * v - d; + if ( y < .1 ) + { + if ( findingStartvalue ) + { + v *= 2.; + continue; + } + break; + } + findingStartvalue = false; + double y_prime = 3 * a * v * v + c; + v -= y / y_prime; + } + return v; + } + @Override public double getTotalTime() { return totalTime; } + + @Override + public double getTotalEnergy() + { + return totalEnergy; + } }