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

initial commit of BRouter Version 0.98

Browse source
This commit is contained in:
Arndt Brenschede 2014-01-18 15:29:05 +01:00
parent e4ae2b37d3
commit 91e62f1164
120 changed files with 15382 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

56
brouter-core/src/main/java/btools/router/MatchedWaypoint.java Normal file
View file

@ -0,0 +1,56 @@
/**
* Information on matched way point
*
* @author ab
*/
package btools.router;
import java.io.DataInput;
import java.io.DataOutput;
import java.io.IOException;
import btools.mapaccess.OsmNode;
final class MatchedWaypoint
{
public OsmNode node1;
public OsmNode node2;
public OsmNodeNamed crosspoint;
public OsmNodeNamed waypoint;
public double radius;
public int cost;
public void writeToStream( DataOutput dos ) throws IOException
{
dos.writeInt( node1.ilat );
dos.writeInt( node1.ilon );
dos.writeInt( node2.ilat );
dos.writeInt( node2.ilon );
dos.writeInt( crosspoint.ilat );
dos.writeInt( crosspoint.ilon );
dos.writeInt( waypoint.ilat );
dos.writeInt( waypoint.ilon );
dos.writeDouble( radius );
}
public static MatchedWaypoint readFromStream( DataInput dis ) throws IOException
{
MatchedWaypoint mwp = new MatchedWaypoint();
mwp.node1 = new OsmNode();
mwp.node2 = new OsmNode();
mwp.crosspoint = new OsmNodeNamed();
mwp.waypoint = new OsmNodeNamed();
mwp.node1.ilat = dis.readInt();
mwp.node1.ilon = dis.readInt();
mwp.node2.ilat = dis.readInt();
mwp.node2.ilon = dis.readInt();
mwp.crosspoint.ilat = dis.readInt();
mwp.crosspoint.ilon = dis.readInt();
mwp.waypoint.ilat = dis.readInt();
mwp.waypoint.ilon = dis.readInt();
mwp.radius = dis.readDouble();
return mwp;
}
}

172
brouter-core/src/main/java/btools/router/OpenSet.java Normal file
View file

@ -0,0 +1,172 @@
/**
* Implementation for the open-set
* that should be somewhat faster
* and memory-efficient than the original
* version based on java.util.TreeSet
*
* It relies on the two double-linked
* lists implemented in OsmPath
*
* @author ab
*/
package btools.router;
import btools.mapaccess.OsmNode;
public class OpenSet
{
private OsmPath start = new OsmPath();
private OsmPath index2 = new OsmPath();
private int addCount = 0;
private int size = 0;
public void clear()
{
start.nextInSet = null;
start.nextInIndexSet = null;
index2.nextInIndexSet = null;
size = 0;
addCount = 0;
}
public void add( OsmPath path )
{
int ac = path.adjustedCost;
OsmPath p1 = index2;
// fast forward along index2
while( p1.nextInIndexSet != null && p1.nextInIndexSet.adjustedCost < ac )
{
p1 = p1.nextInIndexSet;
}
if ( p1 == index2 )
{
p1 = start;
}
// search using index1
for(;;)
{
if ( p1.nextInIndexSet != null && p1.nextInIndexSet.adjustedCost < ac )
{
p1 = p1.nextInIndexSet;
}
else if ( p1.nextInSet != null && p1.nextInSet.adjustedCost < ac )
{
p1 = p1.nextInSet;
}
else
{
break;
}
}
OsmPath p2 = p1.nextInSet;
p1.nextInSet = path;
path.prevInSet = p1;
path.nextInSet = p2;
if ( p2 != null ) { p2.prevInSet = path; }
size++;
addCount++;
// feed random samples to the indices
if ( (addCount & 31) == 0 )
{
addIndex( path, start );
}
else if ( (addCount & 1023) == 1023 )
{
addIndex( path, index2 );
}
}
public void remove( OsmPath path )
{
OsmPath p1 = path.prevInSet;
OsmPath p2 = path.nextInSet;
if ( p1 == null )
{
return; // not in set
}
path.prevInSet = null;
path.nextInSet = null;
if ( p2 != null )
{
p2.prevInSet = p1;
}
p1.nextInSet = p2;
removeIndex( path );
size--;
}
public OsmPath first()
{
return start.nextInSet;
}
public int size()
{
return size;
}
public int[] getExtract()
{
int div = size / 1000 + 1;
int[] res = new int[size/div * 2];
int i = 0;
int cnt = 0;
for( OsmPath p = start.nextInSet; p != null; p = p.nextInSet )
{
if ( (++cnt) % div == 0 )
{
OsmNode n = p.getLink().targetNode;
res[i++] = n.ilon;
res[i++] = n.ilat;
}
}
return res;
}
// index operations
private void addIndex( OsmPath path, OsmPath index )
{
int ac = path.adjustedCost;
OsmPath p1 = index;
OsmPath p2 = p1.nextInIndexSet;
while( p2 != null && p2.adjustedCost < ac )
{
p1 = p2;
p2 = p2.nextInIndexSet;
}
p1.nextInIndexSet = path;
path.prevInIndexSet = p1;
path.nextInIndexSet = p2;
if ( p2 != null ) { p2.prevInIndexSet = path; }
}
private void removeIndex( OsmPath path )
{
OsmPath p1 = path.prevInIndexSet;
OsmPath p2 = path.nextInIndexSet;
if ( p1 == null )
{
return; // not in set
}
path.prevInIndexSet = null;
path.nextInIndexSet = null;
if ( p2 != null )
{
p2.prevInIndexSet = p1;
}
p1.nextInIndexSet = p2;
}
}

33
brouter-core/src/main/java/btools/router/OsmNodeNamed.java Normal file
View file

@ -0,0 +1,33 @@
/**
* Container for an osm node
*
* @author ab
*/
package btools.router;
import btools.mapaccess.OsmNode;
public class OsmNodeNamed extends OsmNode
{
public String name;
public double radius; // radius of nogopoint
public boolean isNogo = false;
@Override
public String toString()
{
return ilon + "," + ilat + "," + name;
}
public static OsmNodeNamed decodeNogo( String s )
{
OsmNodeNamed n = new OsmNodeNamed();
int idx1 = s.indexOf( ',' );
n.ilon = Integer.parseInt( s.substring( 0, idx1 ) );
int idx2 = s.indexOf( ',', idx1+1 );
n.ilat = Integer.parseInt( s.substring( idx1+1, idx2 ) );
n.name = s.substring( idx2+1 );
n.isNogo = true;
return n;
}
}

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

@ -0,0 +1,371 @@
/**
* Container for link between two Osm nodes
*
* @author ab
*/
package btools.router;
import btools.mapaccess.*;
final class OsmPath implements OsmLinkHolder
{
// double-linked lists for the openSet
public OsmPath nextInSet;
public OsmPath prevInSet;
public OsmPath nextInIndexSet;
public OsmPath prevInIndexSet;
/**
* The cost of that path (a modified distance)
*/
public int cost = 0;
/**
* The elevation-hysteresis-buffer (0-10 m)
*/
private int ehbd; // in micrometer
private int ehbu; // in micrometer
// the elevation assumed for that path can have a value
// if the corresponding node has not
public short selev;
private static final int MAX_EHB = 10000000;
public int adjustedCost = 0;
public void setAirDistanceCostAdjustment( int costAdjustment )
{
adjustedCost = cost + costAdjustment;
}
private OsmNode sourcenode;
private OsmLink link;
public OsmPathElement originElement;
private OsmLinkHolder nextForLink = null;
public int treedepth = 0;
// the position of the waypoint just before
// this path position (for angle calculation)
public int originLon;
public int originLat;
// the costfactor of the segment just before this paths position
public float lastCostfactor;
public String message;
OsmPath()
{
}
OsmPath( OsmLink link )
{
this();
this.link = link;
this.selev = link.targetNode.getSElev();
}
OsmPath( OsmNode sourcenode, OsmPath origin, OsmLink link, OsmTrack refTrack, boolean recordTransferNodes, RoutingContext rc )
{
this();
this.originElement = new OsmPathElement( origin );
this.link = link;
this.sourcenode = sourcenode;
this.cost = origin.cost;
this.ehbd = origin.ehbd;
this.ehbu = origin.ehbu;
this.lastCostfactor = origin.lastCostfactor;
addAddionalPenalty(refTrack, recordTransferNodes, origin, link, rc );
}
private void addAddionalPenalty(OsmTrack refTrack, boolean recordTransferNodes, OsmPath origin, OsmLink link, RoutingContext rc )
{
rc.nogomatch = false;
// extract the 3 positions of the first section
int lon0 = origin.originLon;
int lat0 = origin.originLat;
OsmNode p1 = origin.link.targetNode;
int lon1 = p1.getILon();
int lat1 = p1.getILat();
short ele1 = origin.selev;
int linkdisttotal = 0;
int linkdist = 0;
int linkelevationcost = 0;
int linkturncost = 0;
OsmTransferNode transferNode = link.decodeFirsttransfer();
OsmNode targetNode = link.targetNode;
long lastDescription = -1L;
String lastMessage = null;
for(;;)
{
originLon = lon1;
originLat = lat1;
int lon2;
int lat2;
short ele2;
long description;
if ( transferNode == null )
{
lon2 = targetNode.ilon;
lat2 = targetNode.ilat;
ele2 = targetNode.selev;
description = link.descriptionBitmap;
}
else
{
lon2 = transferNode.ilon;
lat2 = transferNode.ilat;
ele2 = transferNode.selev;
description = transferNode.descriptionBitmap;
}
// if way description changed, store message
if ( lastMessage != null && description != lastDescription )
{
originElement.message = lastMessage;
linkdist = 0;
linkelevationcost = 0;
linkturncost = 0;
}
lastDescription = description;
int dist = rc.calcDistance( lon1, lat1, lon2, lat2 );
int elefactor = 250000;
boolean stopAtEndpoint = false;
if ( rc.shortestmatch )
{
elefactor = (int)(elefactor*rc.wayfraction);
if ( rc.isEndpoint )
{
stopAtEndpoint = true;
}
else
{
// we just start here, reset cost
cost = 0;
ehbd = 0;
ehbu = 0;
if ( recordTransferNodes )
{
if ( rc.wayfraction > 0. )
{
originElement = new OsmPathElement( rc.ilonshortest, rc.ilatshortest, ele2, null );
}
else
{
originElement = null; // prevent duplicate point
}
}
}
}
linkdist += dist;
linkdisttotal += dist;
rc.messageHandler.setCurrentPos( lon2, lat2 );
rc.expctxWay.evaluate( description, rc.messageHandler );
// *** penalty for way-change
if ( origin.originElement != null )
{
// penalty proportional to direction change
double cos = rc.calcCosAngle( lon0, lat0, lon1, lat1, lon2, lat2 );
int turncost = (int)(cos * rc.expctxWay.getTurncost() + 0.2 ); // e.g. turncost=90 -> 90 degree = 90m penalty
cost += turncost;
linkturncost += turncost;
}
// *** penalty for elevation (penalty is for descend! in a way that slow descends give no penalty)
// only the part of the descend that does not fit into the elevation-hysteresis-buffer
// leads to an immediate penalty
if ( ele2 == Short.MIN_VALUE ) ele2 = ele1;
if ( ele1 != Short.MIN_VALUE )
{
ehbd += (ele1 - ele2)*elefactor - dist * rc.downhillcutoff;
ehbu += (ele2 - ele1)*elefactor - dist * rc.uphillcutoff;
}
if ( ehbd > MAX_EHB )
{
if ( rc.downhillcostdiv > 0 )
{
int elevationCost = (ehbd-MAX_EHB)/rc.downhillcostdiv;
cost += elevationCost;
linkelevationcost += elevationCost;
}
ehbd = MAX_EHB;
}
else if ( ehbd < 0 )
{
ehbd = 0;
}
if ( ehbu > MAX_EHB )
{
if ( rc.uphillcostdiv > 0 )
{
int elevationCost = (ehbu-MAX_EHB)/rc.uphillcostdiv;
cost += elevationCost;
linkelevationcost += elevationCost;
}
ehbu = MAX_EHB;
}
else if ( ehbu < 0 )
{
ehbu = 0;
}
// *** penalty for distance
float costfactor = rc.expctxWay.getCostfactor();
float fcost = dist * costfactor + 0.5f;
if ( costfactor >= 10000. || fcost + cost >= 2000000000. )
{
cost = -1;
return;
}
int waycost = (int)(fcost);
cost += waycost;
// *** add initial cost if factor changed
float costdiff = costfactor - lastCostfactor;
if ( costdiff > 0.0005 || costdiff < -0.0005 )
{
lastCostfactor = costfactor;
float initialcost = rc.expctxWay.getInitialcost();
int iicost = (int)initialcost;
cost += iicost;
}
if ( recordTransferNodes )
{
int iCost = (int)(rc.expctxWay.getCostfactor()*1000 + 0.5f);
lastMessage = (lon2-180000000) + "\t"
+ (lat2-90000000) + "\t"
+ ele2/4 + "\t"
+ linkdist + "\t"
+ iCost + "\t"
+ linkelevationcost
+ "\t" + linkturncost
+ rc.expctxWay.getCsvDescription( description );
}
if ( stopAtEndpoint )
{
if ( recordTransferNodes )
{
originElement = new OsmPathElement( rc.ilonshortest, rc.ilatshortest, ele2, originElement );
originElement.cost = cost;
}
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( origin.link.targetNode ) )
{
int reftrackcost = linkdisttotal;
cost += reftrackcost;
}
message = lastMessage;
selev = ele2;
break;
}
transferNode = transferNode.next;
if ( recordTransferNodes )
{
originElement = new OsmPathElement( lon2, lat2, ele2, originElement );
originElement.cost = cost;
}
lon0 = lon1;
lat0 = lat1;
lon1 = lon2;
lat1 = lat2;
ele1 = ele2;
}
// check for nogo-matches (after the *actual* start of segment)
if ( rc.nogomatch )
{
cost = -1;
return;
}
// finally add node-costs for target node
if ( targetNode.nodeDescription != 0L )
{
rc.messageHandler.setCurrentPos( targetNode.ilon, targetNode.ilat );
rc.expctxNode.evaluate( targetNode.nodeDescription, rc.messageHandler );
float initialcost = rc.expctxNode.getInitialcost();
if ( initialcost >= 1000000. )
{
cost = -1;
return;
}
int iicost = (int)initialcost;
cost += iicost;
}
}
public int elevationCorrection( RoutingContext rc )
{
return ( rc.downhillcostdiv > 0 ? ehbd/rc.downhillcostdiv : 0 )
+ ( rc.uphillcostdiv > 0 ? ehbu/rc.uphillcostdiv : 0 );
}
public boolean definitlyWorseThan( OsmPath p, RoutingContext rc )
{
int c = p.cost;
if ( rc.downhillcostdiv > 0 )
{
int delta = p.ehbd - ehbd;
if ( delta > 0 ) c += delta/rc.downhillcostdiv;
}
if ( rc.uphillcostdiv > 0 )
{
int delta = p.ehbu - ehbu;
if ( delta > 0 ) c += delta/rc.uphillcostdiv;
}
return cost > c;
}
public OsmNode getSourceNode()
{
return sourcenode;
}
public OsmLink getLink()
{
return link;
}
public void setNextForLink( OsmLinkHolder holder )
{
nextForLink = holder;
}
public OsmLinkHolder getNextForLink()
{
return nextForLink;
}
}

113
brouter-core/src/main/java/btools/router/OsmPathElement.java Normal file
View file

@ -0,0 +1,113 @@
/**
* Container for link between two Osm nodes
*
* @author ab
*/
package btools.router;
import java.io.DataInput;
import java.io.DataOutput;
import java.io.IOException;
import btools.mapaccess.OsmNode;
import btools.mapaccess.OsmPos;
final class OsmPathElement implements OsmPos
{
private int ilat; // latitude
private int ilon; // longitude
private short selev; // longitude
public String message = null; // description
public int cost;
// interface OsmPos
public int getILat()
{
return ilat;
}
public int getILon()
{
return ilon;
}
public short getSElev()
{
return selev;
}
public double getElev()
{
return selev / 4.;
}
public long getIdFromPos()
{
return ((long)ilon)<<32 | ilat;
}
public int calcDistance( OsmPos p )
{
double l = (ilat-90000000) * 0.00000001234134;
double l2 = l*l;
double l4 = l2*l2;
double coslat = 1.- l2 + l4 / 6.;
double dlat = (ilat - p.getILat() )/1000000.;
double dlon = (ilon - p.getILon() )/1000000. * coslat;
double d = Math.sqrt( dlat*dlat + dlon*dlon ) * (6378000. / 57.);
return (int)(d + 1.0 );
}
public OsmPathElement origin;
// construct a path element from a path
public OsmPathElement( OsmPath path )
{
OsmNode n = path.getLink().targetNode;
ilat = n.getILat();
ilon = n.getILon();
selev = path.selev;
cost = path.cost;
origin = path.originElement;
message = path.message;
}
public OsmPathElement( int ilon, int ilat, short selev, OsmPathElement origin )
{
this.ilon = ilon;
this.ilat = ilat;
this.selev = selev;
this.origin = origin;
}
private OsmPathElement()
{
}
public String toString()
{
return ilon + "_" + ilat;
}
public void writeToStream( DataOutput dos ) throws IOException
{
dos.writeInt( ilat );
dos.writeInt( ilon );
dos.writeShort( selev );
dos.writeInt( cost );
}
public static OsmPathElement readFromStream( DataInput dis ) throws IOException
{
OsmPathElement pe = new OsmPathElement();
pe.ilat = dis.readInt();
pe.ilon = dis.readInt();
pe.selev = dis.readShort();
pe.cost = dis.readInt();
return pe;
}
}

365
brouter-core/src/main/java/btools/router/OsmTrack.java Normal file
View file

@ -0,0 +1,365 @@
/**
* Container for a track
*
* @author ab
*/
package btools.router;
import java.io.BufferedInputStream;
import java.io.BufferedOutputStream;
import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileOutputStream;
import java.io.FileWriter;
import java.io.InputStreamReader;
import java.util.ArrayList;
import btools.mapaccess.OsmPos;
import btools.util.CompactLongMap;
import btools.util.FrozenLongMap;
public final class OsmTrack
{
public MatchedWaypoint endPoint;
private class OsmPathElementHolder
{
public OsmPathElement node;
public OsmPathElementHolder nextHolder;
}
public ArrayList<OsmPathElement> nodes = new ArrayList<OsmPathElement>();
private CompactLongMap<OsmPathElementHolder> nodesMap;
public String message = null;
public ArrayList<String> messageList = null;
public String name = "unset";
public void addNode( OsmPathElement node )
{
nodes.add( 0, node );
}
public void buildMap()
{
nodesMap = new CompactLongMap<OsmPathElementHolder>();
for( OsmPathElement node: nodes )
{
long id = node.getIdFromPos();
OsmPathElementHolder nh = new OsmPathElementHolder();
nh.node = node;
OsmPathElementHolder h = nodesMap.get( id );
if ( h != null )
{
while( h.nextHolder != null )
{
h = h.nextHolder;
}
h.nextHolder = nh;
}
else
{
nodesMap.fastPut( id, nh );
}
}
nodesMap = new FrozenLongMap<OsmPathElementHolder>( nodesMap );
}
/**
* writes the track in binary-format to a file
* @param filename the filename to write to
*/
public void writeBinary( String filename ) throws Exception
{
DataOutputStream dos = new DataOutputStream( new BufferedOutputStream( new FileOutputStream( filename ) ) );
endPoint.writeToStream( dos );
dos.writeInt( nodes.size() );
for( OsmPathElement node: nodes )
{
node.writeToStream( dos );
}
dos.close();
}
public static OsmTrack readBinary( String filename, OsmNodeNamed newEp )
{
OsmTrack t = null;
if ( filename != null )
{
File f = new File( filename );
if ( f.exists() )
{
try
{
DataInputStream dis = new DataInputStream( new BufferedInputStream( new FileInputStream( f ) ) );
MatchedWaypoint ep = MatchedWaypoint.readFromStream( dis );
int dlon = ep.waypoint.ilon - newEp.ilon;
int dlat = ep.waypoint.ilat - newEp.ilat;
if ( dlon < 20 && dlon > -20 && dlat < 20 && dlat > -20 )
{
t = new OsmTrack();
t.endPoint = ep;
int n = dis.readInt();
OsmPathElement last_pe = null;
for( int i=0; i<n; i++ )
{
OsmPathElement pe = OsmPathElement.readFromStream( dis );
pe.origin = last_pe;
last_pe = pe;
t.nodes.add( pe );
}
t.cost = last_pe.cost;
t.buildMap();
}
dis.close();
}
catch( Exception e )
{
throw new RuntimeException( "Exception reading rawTrack: " + e );
}
}
}
return t;
}
public void addNodes( OsmTrack t )
{
for( OsmPathElement n : t.nodes ) addNode( n );
buildMap();
}
public boolean containsNode( OsmPos node )
{
return nodesMap.contains( node.getIdFromPos() );
}
public OsmPathElement getLink( long n1, long n2 )
{
OsmPathElementHolder h = nodesMap.get( n2 );
while( h != null )
{
OsmPathElement e1 = h.node.origin;
if ( e1 != null && e1.getIdFromPos() == n1 )
{
return h.node;
}
h = h.nextHolder;
}
return null;
}
public void appendTrack( OsmTrack t )
{
for( int i=0; i<t.nodes.size(); i++ )
{
if ( i > 0 || nodes.size() == 0 )
{
nodes.add( t.nodes.get(i) );
}
}
distance += t.distance;
ascend += t.ascend;
plainAscend += t.plainAscend;
cost += t.cost;
}
public int distance;
public int ascend;
public int plainAscend;
public int cost;
/**
* writes the track in gpx-format to a file
* @param filename the filename to write to
*/
public void writeGpx( String filename ) throws Exception
{
BufferedWriter bw = new BufferedWriter( new FileWriter( filename ) );
bw.write( formatAsGpx() );
bw.close();
}
public String formatAsGpx()
{
StringBuilder sb = new StringBuilder(8192);
sb.append( "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n" );
for( int i=messageList.size()-1; i >= 0; i-- )
{
String message = messageList.get(i);
if ( i < messageList.size()-1 ) message = "(alt-index " + i + ": " + message + " )";
if ( message != null ) sb.append( "<!-- " + message + " -->\n" );
}
sb.append( "<gpx \n" );
sb.append( " xmlns=\"http://www.topografix.com/GPX/1/1\" \n" );
sb.append( " xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" \n" );
sb.append( " xsi:schemaLocation=\"http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd\" \n" );
sb.append( " creator=\"BRouter-0.98\" version=\"1.1\">\n" );
sb.append( " <trk>\n" );
sb.append( " <name>" + name + "</name>\n" );
sb.append( " <trkseg>\n" );
for( OsmPathElement n : nodes )
{
String sele = n.getSElev() == Short.MIN_VALUE ? "" : "<ele>" + n.getElev() + "</ele>";
sb.append( " <trkpt lon=\"" + formatPos( n.getILon() - 180000000 ) + "\" lat=\"" + formatPos( n.getILat() - 90000000 ) + "\">" + sele + "</trkpt>\n" );
}
sb.append( " </trkseg>\n" );
sb.append( " </trk>\n" );
sb.append( "</gpx>\n" );
return sb.toString();
}
public void writeKml( String filename ) throws Exception
{
BufferedWriter bw = new BufferedWriter( new FileWriter( filename ) );
bw.write( formatAsKml() );
bw.close();
}
public String formatAsKml()
{
StringBuilder sb = new StringBuilder(8192);
sb.append( "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n" );
sb.append( "<kml xmlns=\"http://earth.google.com/kml/2.0\">\n" );
sb.append( " <Document>\n" );
sb.append( " <name>KML Samples</name>\n" );
sb.append( " <open>1</open>\n" );
sb.append( " <distance>3.497064</distance>\n" );
sb.append( " <traveltime>872</traveltime>\n" );
sb.append( " <description>To enable simple instructions add: 'instructions=1' as parameter to the URL</description>\n" );
sb.append( " <Folder>\n" );
sb.append( " <name>Paths</name>\n" );
sb.append( " <visibility>0</visibility>\n" );
sb.append( " <description>Examples of paths.</description>\n" );
sb.append( " <Placemark>\n" );
sb.append( " <name>Tessellated</name>\n" );
sb.append( " <visibility>0</visibility>\n" );
sb.append( " <description><![CDATA[If the <tessellate> tag has a value of 1, the line will contour to the underlying terrain]]></description>\n" );
sb.append( " <LineString>\n" );
sb.append( " <tessellate>1</tessellate>\n" );
sb.append( " <coordinates> " );
for( OsmPathElement n : nodes )
{
sb.append( formatPos( n.getILon() - 180000000 ) + "," + formatPos( n.getILat() - 90000000 ) + "\n" );
}
sb.append( " </coordinates>\n" );
sb.append( " </LineString>\n" );
sb.append( " </Placemark>\n" );
sb.append( " </Folder>\n" );
sb.append( " </Document>\n" );
sb.append( "</kml>\n" );
return sb.toString();
}
private static String formatPos( int p )
{
boolean negative = p < 0;
if ( negative ) p = -p;
char[] ac = new char[12];
int i = 11;
while( p != 0 || i > 3 )
{
ac[i--] = (char)('0' + (p % 10));
p /= 10;
if ( i == 5 ) ac[i--] = '.';
}
if ( negative ) ac[i--] = '-';
return new String( ac, i+1, 11-i );
}
public void dumpMessages( String filename, RoutingContext rc ) throws Exception
{
BufferedWriter bw = filename == null ? null : new BufferedWriter( new FileWriter( filename ) );
// csv-header-line
String header = "Longitude\tLatitude\tElevation\tDistance\tCostPerKm\tElevCost\tTurnCost";
if ( rc.expctxWay != null )
{
header += rc.expctxWay.getCsvHeader();
}
dumpLine( bw, header );
for( OsmPathElement n : nodes )
{
if ( n.message != null )
{
dumpLine( bw, n.message );
}
}
if ( bw != null ) bw.close();
}
private void dumpLine( BufferedWriter bw, String s) throws Exception
{
if ( bw == null )
{
System.out.println( s );
}
else
{
bw.write( s );
bw.write( "\n" );
}
}
public void readGpx( String filename ) throws Exception
{
File f = new File( filename );
if ( !f.exists() ) return;
BufferedReader br = new BufferedReader(
new InputStreamReader(
new FileInputStream( f ) ) );
for(;;)
{
String line = br.readLine();
if ( line == null ) break;
int idx0 = line.indexOf( "<trkpt lon=\"" );
if ( idx0 >= 0 )
{
idx0 += 12;
int idx1 = line.indexOf( '"', idx0 );
int ilon = (int)((Double.parseDouble( line.substring( idx0, idx1 ) ) + 180. )*1000000. + 0.5);
int idx2 = line.indexOf( " lat=\"" );
if ( idx2 < 0 ) continue;
idx2 += 6;
int idx3 = line.indexOf( '"', idx2 );
int ilat = (int)((Double.parseDouble( line.substring( idx2, idx3 ) ) + 90. )*1000000. + 0.5);
nodes.add( new OsmPathElement( ilon, ilat, (short)0, null ) );
}
}
br.close();
}
public boolean equalsTrack( OsmTrack t )
{
if ( nodes.size() != t.nodes.size() ) return false;
for( int i=0; i<nodes.size(); i++ )
{
OsmPathElement e1 = nodes.get(i);
OsmPathElement e2 = t.nodes.get(i);
if ( e1.getILon() != e2.getILon() || e1.getILat() != e2.getILat() ) return false;
}
return true;
}
}

241
brouter-core/src/main/java/btools/router/RoutingContext.java Normal file
View file

@ -0,0 +1,241 @@
/**
* Container for routig configs
*
* @author ab
*/
package btools.router;
import java.util.ArrayList;
import java.util.List;
import btools.mapaccess.*;
import btools.expressions.*;
public final class RoutingContext implements DistanceChecker
{
public void setAlternativeIdx( int idx )
{
if ( idx < 0 ) idx = 0;
if ( idx > 3 ) idx = 3;
alternativeIdx = idx;
}
public int getAlternativeIdx()
{
return alternativeIdx;
}
public int alternativeIdx = 0;
public String localFunction;
public String rawTrackPath;
public String getProfileName()
{
String name = localFunction == null ? "unknown" : localFunction;
if ( name.endsWith( ".brf" ) ) name = name.substring( 0, localFunction.length() - 4 );
int idx = name.lastIndexOf( '/' );
if ( idx >= 0 ) name = name.substring( idx+1 );
return name;
}
public BExpressionContext expctxWay;
public BExpressionContext expctxNode;
public int downhillcostdiv;
public int downhillcutoff;
public int uphillcostdiv;
public int uphillcutoff;
public boolean carMode;
public double pass1coefficient;
public double pass2coefficient;
public void readGlobalConfig( BExpressionContext expctxGlobal )
{
downhillcostdiv = (int)expctxGlobal.getVariableValue( "downhillcost" );
downhillcutoff = (int)(expctxGlobal.getVariableValue( "downhillcutoff" )*10000);
uphillcostdiv = (int)expctxGlobal.getVariableValue( "uphillcost" );
uphillcutoff = (int)(expctxGlobal.getVariableValue( "uphillcutoff" )*10000);
if ( downhillcostdiv != 0 ) downhillcostdiv = 1000000/downhillcostdiv;
if ( uphillcostdiv != 0 ) uphillcostdiv = 1000000/uphillcostdiv;
carMode = 0.f != expctxGlobal.getVariableValue( "validForCars" );
pass1coefficient = expctxGlobal.getVariableValue( "pass1coefficient", 1.5f );
pass2coefficient = expctxGlobal.getVariableValue( "pass2coefficient", 0.f );
}
public RoutingMessageHandler messageHandler = new RoutingMessageHandler();
public List<OsmNodeNamed> nogopoints = null;
private List<OsmNodeNamed> keepnogopoints = null;
private double coslat;
public boolean nogomatch = false;
public boolean isEndpoint = false;
public boolean shortestmatch = false;
public double wayfraction;
public int ilatshortest;
public int ilonshortest;
public void prepareNogoPoints( List<OsmNodeNamed> nogos )
{
for( OsmNodeNamed nogo : nogos )
{
String s = nogo.name;
int idx = s.indexOf( ' ' );
if ( idx > 0 ) s = s.substring( 0 , idx );
int ir = 20; // default radius
if ( s.length() > 4 )
{
try { ir = Integer.parseInt( s.substring( 4 ) ); }
catch( Exception e ) { /* ignore */ }
}
nogo.radius = ir / 111894.; // 6378000. / 57.;
}
}
public void setWaypoint( OsmNodeNamed wp, boolean endpoint )
{
keepnogopoints = nogopoints;
nogopoints = new ArrayList<OsmNodeNamed>();
nogopoints.add( wp );
if ( keepnogopoints != null ) nogopoints.addAll( keepnogopoints );
isEndpoint = endpoint;
}
public void unsetWaypoint()
{
nogopoints = keepnogopoints;
isEndpoint = false;
}
public int calcDistance( int lon1, int lat1, int lon2, int lat2 )
{
double l = (lat2 - 90000000) * 0.00000001234134;
double l2 = l*l;
double l4 = l2*l2;
coslat = 1.- l2 + l4 / 6.;
double coslat6 = coslat*0.000001;
double dx = (lon2 - lon1 ) * coslat6;
double dy = (lat2 - lat1 ) * 0.000001;
double d = Math.sqrt( dy*dy + dx*dx );
shortestmatch = false;
if ( d > 0. && nogopoints != null )
{
for( OsmNodeNamed nogo : nogopoints )
{
double x1 = (lon1 - nogo.ilon) * coslat6;
double y1 = (lat1 - nogo.ilat) * 0.000001;
double x2 = (lon2 - nogo.ilon) * coslat6;
double y2 = (lat2 - nogo.ilat) * 0.000001;
double r12 = x1*x1 + y1*y1;
double r22 = x2*x2 + y2*y2;
double radius = Math.abs( r12 < r22 ? y1*dx - x1*dy : y2*dx - x2*dy ) / d;
if ( radius < nogo.radius ) // 20m
{
double s1 = x1*dx + y1*dy;
double s2 = x2*dx + y2*dy;
if ( s1 < 0. ) { s1 = -s1; s2 = -s2; }
if ( s2 > 0. )
{
radius = Math.sqrt( s1 < s2 ? r12 : r22 );
if ( radius > nogo.radius ) continue; // 20m ^ 2
}
if ( nogo.isNogo ) nogomatch = true;
else
{
shortestmatch = true;
nogo.radius = radius; // shortest distance to way
// calculate remaining distance
if ( s2 < 0. )
{
double distance = d > 0. ? -s2 / d : 0.;
wayfraction = d > 0. ? distance / d : 0.;
double xm = x2 - wayfraction*dx;
double ym = y2 - wayfraction*dy;
ilonshortest = (int)(xm / coslat6 + nogo.ilon);
ilatshortest = (int)(ym / 0.000001 + nogo.ilat);
}
else if ( s1 > s2 )
{
wayfraction = 0.;
ilonshortest = lon2;
ilatshortest = lat2;
}
else
{
wayfraction = 1.;
ilonshortest = lon1;
ilatshortest = lat1;
}
// here it gets nasty: there can be nogo-points in the list
// *after* the shortest distance point. In case of a shortest-match
// we use the reduced way segment for nogo-matching, in order not
// to cut our escape-way if we placed a nogo just in front of where we are
if ( isEndpoint )
{
wayfraction = 1. - wayfraction;
lon2 = ilonshortest;
lat2 = ilatshortest;
}
else
{
nogomatch = false;
lon1 = ilonshortest;
lat1 = ilatshortest;
}
dx = (lon2 - lon1 ) * coslat6;
dy = (lat2 - lat1 ) * 0.000001;
d = Math.sqrt( dy*dy + dx*dx );
}
}
}
}
double dd = d * 111894.7368; // 6378000. / 57.;
return (int)(dd + 1.0 );
}
// assumes that calcDistance/calcCosAngle called in sequence, so coslat valid
public double calcCosAngle( int lon0, int lat0, int lon1, int lat1, int lon2, int lat2 )
{
double dlat1 = (lat1 - lat0);
double dlon1 = (lon1 - lon0) * coslat;
double dlat2 = (lat2 - lat1);
double dlon2 = (lon2 - lon1) * coslat;
double dd = Math.sqrt( (dlat1*dlat1 + dlon1*dlon1)*(dlat2*dlat2 + dlon2*dlon2) );
if ( dd == 0. ) return 0.;
double cosp = (dlat1*dlat2 + dlon1*dlon2)/dd;
return 1.-cosp; // don't care to really do acos..
}
@Override
public boolean isWithinRadius( int ilon0, int ilat0, OsmTransferNode firstTransfer, int ilon1, int ilat1 )
{
OsmNodeNamed wp = nogopoints.get(0);
double keepRadius = wp.radius;
try
{
int ilon = ilon0;
int ilat = ilat0;
for( OsmTransferNode trans = firstTransfer; trans != null; trans = trans.next )
{
calcDistance( ilon, ilat, trans.ilon, trans.ilat );
ilon = trans.ilon;
ilat = trans.ilat;
}
calcDistance( ilon, ilat, ilon1, ilat1 );
return wp.radius < keepRadius;
}
finally
{
wp.radius = keepRadius;
}
}
}

968
brouter-core/src/main/java/btools/router/RoutingEngine.java Normal file
View file

@ -0,0 +1,968 @@
package btools.router;
import java.io.File;
import java.util.ArrayList;
import java.util.List;
import btools.expressions.BExpressionContext;
import btools.mapaccess.NodesCache;
import btools.mapaccess.OsmLink;
import btools.mapaccess.OsmLinkHolder;
import btools.mapaccess.OsmNode;
import btools.mapaccess.OsmNodesMap;
public class RoutingEngine extends Thread
{
private OsmNodesMap nodesMap;
private NodesCache nodesCache;
private OpenSet openSet = new OpenSet();
private boolean finished = false;
private List<OsmNodeNamed> waypoints = null;
private int linksProcessed = 0;
private OsmTrack foundTrack = new OsmTrack();
private OsmTrack foundRawTrack = null;
private int alternativeIndex = 0;
private String errorMessage = null;
private volatile boolean terminated;
private String segmentDir;
private String outfileBase;
private String logfileBase;
private boolean infoLogEnabled;
private RoutingContext routingContext;
private double airDistanceCostFactor;
private OsmTrack guideTrack;
private OsmPathElement matchPath;
private long startTime;
private long maxRunningTime;
public boolean quite = false;
public RoutingEngine( String outfileBase, String logfileBase, String segmentDir,
List<OsmNodeNamed> waypoints, RoutingContext rc )
{
this.segmentDir = segmentDir;
this.outfileBase = outfileBase;
this.logfileBase = logfileBase;
this.waypoints = waypoints;
this.infoLogEnabled = outfileBase != null;
this.routingContext = rc;
if ( rc.localFunction != null )
{
String profileBaseDir = System.getProperty( "profileBaseDir" );
File profileDir;
File profileFile;
if ( profileBaseDir == null )
{
profileDir = new File( rc.localFunction ).getParentFile();
profileFile = new File( rc.localFunction ) ;
}
else
{
profileDir = new File( profileBaseDir );
profileFile = new File( profileDir, rc.localFunction + ".brf" ) ;
}
BExpressionContext expctxGlobal = new BExpressionContext( "global" );
expctxGlobal.readMetaData( new File( profileDir, "lookups.dat" ) );
expctxGlobal.parseFile( profileFile, null );
expctxGlobal.evaluate( 1L, rc.messageHandler );
rc.readGlobalConfig(expctxGlobal);
rc.expctxWay = new BExpressionContext( "way", 4096 );
rc.expctxWay.readMetaData( new File( profileDir, "lookups.dat" ) );
rc.expctxWay.parseFile( profileFile, "global" );
rc.expctxNode = new BExpressionContext( "node", 1024 );
rc.expctxNode.readMetaData( new File( profileDir, "lookups.dat" ) );
rc.expctxNode.parseFile( profileFile, "global" );
}
}
private void logInfo( String s )
{
if ( infoLogEnabled )
{
System.out.println( s );
}
}
public void run()
{
doRun( 0 );
}
public void doRun( long maxRunningTime )
{
try
{
startTime = System.currentTimeMillis();
this.maxRunningTime = maxRunningTime;
OsmTrack sum = null;
OsmTrack track = null;
ArrayList<String> messageList = new ArrayList<String>();
for( int i=0; !terminated; i++ )
{
track = findTrack( sum );
track.message = "track-length = " + track.distance + " filtered ascend = " + track.ascend
+ " plain-ascend = " + track.plainAscend + " cost=" + track.cost;
track.name = "brouter_" + routingContext.getProfileName() + "_" + i;
messageList.add( track.message );
track.messageList = messageList;
if ( outfileBase != null )
{
String filename = outfileBase + i + ".gpx";
OsmTrack oldTrack = new OsmTrack();
oldTrack.readGpx(filename);
if ( track.equalsTrack( oldTrack ) )
{
if ( sum == null ) sum = new OsmTrack();
sum.addNodes( track );
continue;
}
track.writeGpx( filename );
foundTrack = track;
alternativeIndex = i;
}
else
{
if ( i == routingContext.getAlternativeIdx() )
{
if ( "CSV".equals( System.getProperty( "reportFormat" ) ) )
{
track.dumpMessages( null, routingContext );
}
else
{
if ( !quite )
{
System.out.println( track.formatAsGpx() );
}
}
foundTrack = track;
}
else
{
if ( sum == null ) sum = new OsmTrack();
sum.addNodes( track );
continue;
}
}
if ( logfileBase != null )
{
String logfilename = logfileBase + i + ".csv";
track.dumpMessages( logfilename, routingContext );
}
break;
}
long endTime = System.currentTimeMillis();
logInfo( "execution time = " + (endTime-startTime)/1000. + " seconds" );
}
catch( Exception e)
{
errorMessage = e instanceof IllegalArgumentException ? e.getMessage() : e.toString();
logInfo( "Exception (linksProcessed=" + linksProcessed + ": " + errorMessage );
e.printStackTrace();
}
catch( Error e)
{
String hint = cleanOnOOM();
errorMessage = e.toString() + hint;
logInfo( "Error (linksProcessed=" + linksProcessed + ": " + errorMessage );
e.printStackTrace();
}
finally
{
openSet.clear();
finished = true; // this signals termination to outside
}
}
public String cleanOnOOM()
{
boolean oom_carsubset_hint = nodesCache == null ? false : nodesCache.oom_carsubset_hint;
nodesMap = null;
nodesCache = null;
terminate();
return oom_carsubset_hint ? "\nPlease use 'carsubset' maps for long-distance car-routing" : "";
}
private OsmTrack findTrack( OsmTrack refTrack )
{
OsmTrack totaltrack = new OsmTrack();
MatchedWaypoint[] wayointIds = new MatchedWaypoint[waypoints.size()];
// check for a track for that target
OsmTrack nearbyTrack = null;
if ( refTrack == null )
{
nearbyTrack = OsmTrack.readBinary( routingContext.rawTrackPath, waypoints.get( waypoints.size()-1) );
if ( nearbyTrack != null )
{
wayointIds[waypoints.size()-1] = nearbyTrack.endPoint;
}
}
// match waypoints to nodes
for( int i=0; i<waypoints.size(); i++ )
{
if ( wayointIds[i] == null )
{
wayointIds[i] = matchNodeForPosition( waypoints.get(i) );
}
}
for( int i=0; i<waypoints.size() -1; i++ )
{
OsmTrack seg = searchTrack( wayointIds[i], wayointIds[i+1], i == waypoints.size()-2 ? nearbyTrack : null, refTrack );
if ( seg == null ) return null;
totaltrack.appendTrack( seg );
}
return totaltrack;
}
// geometric position matching finding the nearest routable way-section
private MatchedWaypoint matchNodeForPosition( OsmNodeNamed wp )
{
try
{
routingContext.setWaypoint( wp, false );
int minRingWith = 1;
for(;;)
{
MatchedWaypoint mwp = _matchNodeForPosition( wp, minRingWith );
if ( mwp.node1 != null )
{
int mismatch = wp.calcDistance( mwp.crosspoint );
if ( mismatch < 50*minRingWith )
{
return mwp;
}
}
if ( minRingWith++ == 5 )
{
throw new IllegalArgumentException( wp.name + "-position not mapped" );
}
}
}
finally
{
routingContext.unsetWaypoint();
}
}
private MatchedWaypoint _matchNodeForPosition( OsmNodeNamed wp, int minRingWidth )
{
wp.radius = 1e9;
resetCache();
preloadPosition( wp, minRingWidth, 2000 );
nodesCache.distanceChecker = routingContext;
List<OsmNode> nodeList = nodesCache.getAllNodes();
MatchedWaypoint mwp = new MatchedWaypoint();
mwp.waypoint = wp;
// first loop just to expand reverse links
for( OsmNode n : nodeList )
{
if ( !nodesCache.obtainNonHollowNode( n ) )
{
continue;
}
expandHollowLinkTargets( n, false );
OsmLink startLink = new OsmLink();
startLink.targetNode = n;
OsmPath startPath = new OsmPath( startLink );
startLink.addLinkHolder( startPath );
for( OsmLink link = n.firstlink; link != null; link = link.next )
{
if ( link.counterLinkWritten ) continue; // reverse link not found
OsmNode nextNode = link.targetNode;
if ( nextNode.isHollow() ) continue; // border node?
if ( nextNode.firstlink == null ) continue; // don't care about dead ends
if ( nextNode == n ) continue; // ?
double oldRadius = wp.radius;
OsmPath testPath = new OsmPath( n, startPath, link, null, false, routingContext );
if ( wp.radius < oldRadius )
{
if ( testPath.cost < 0 )
{
wp.radius = oldRadius; // no valid way
}
else
{
mwp.node1 = n;
mwp.node2 = nextNode;
mwp.radius = wp.radius;
mwp.cost = testPath.cost;
mwp.crosspoint = new OsmNodeNamed();
mwp.crosspoint.ilon = routingContext.ilonshortest;
mwp.crosspoint.ilat = routingContext.ilatshortest;
}
}
}
}
return mwp;
}
// expand hollow link targets and resolve reverse links
private void expandHollowLinkTargets( OsmNode n, boolean failOnReverseNotFound )
{
for( OsmLink link = n.firstlink; link != null; link = link.next )
{
if ( ! nodesCache.obtainNonHollowNode( link.targetNode ) )
{
continue;
}
if ( link.counterLinkWritten )
{
OsmLink rlink = link.targetNode.getReverseLink( n.getILon(), n.getILat() );
if ( rlink == null )
{
if ( failOnReverseNotFound ) throw new RuntimeException( "reverse link not found!" );
}
else
{
link.descriptionBitmap = rlink.descriptionBitmap;
link.firsttransferBytes = rlink.firsttransferBytes;
link.counterLinkWritten = false;
}
}
}
n.wasProcessed = true;
}
private OsmTrack searchTrack( MatchedWaypoint startWp, MatchedWaypoint endWp, OsmTrack nearbyTrack, OsmTrack refTrack )
{
OsmTrack track = null;
double[] airDistanceCostFactors = new double[]{ routingContext.pass1coefficient, routingContext.pass2coefficient };
boolean isDirty = false;
if ( nearbyTrack != null )
{
airDistanceCostFactor = 0.;
try
{
track = findTrack( "re-routing", startWp, endWp, nearbyTrack , refTrack, true );
}
catch( IllegalArgumentException iae )
{
// fast partial recalcs: if that timed out, but we had a match,
// build the concatenation from the partial and the nearby track
if ( matchPath != null )
{
track = mergeTrack( matchPath, nearbyTrack );
isDirty = true;
}
maxRunningTime += System.currentTimeMillis() - startTime; // reset timeout...
}
}
if ( track == null )
{
for( int cfi = 0; cfi < airDistanceCostFactors.length && !terminated; cfi++ )
{
airDistanceCostFactor = airDistanceCostFactors[cfi];
if ( airDistanceCostFactor < 0. )
{
continue;
}
OsmTrack t = findTrack( cfi == 0 ? "pass0" : "pass1", startWp, endWp, track , refTrack, false );
if ( t == null && track != null && matchPath != null )
{
// ups, didn't find it, use a merge
t = mergeTrack( matchPath, track );
}
if ( t != null )
{
track = t;
}
else
{
throw new IllegalArgumentException( "no track found at pass=" + cfi );
}
}
}
if ( track == null ) throw new IllegalArgumentException( "no track found" );
if ( refTrack == null && !isDirty )
{
track.endPoint = endWp;
foundRawTrack = track;
}
// final run for verbose log info and detail nodes
airDistanceCostFactor = 0.;
guideTrack = track;
try
{
OsmTrack tt = findTrack( "re-tracking", startWp, endWp, null , refTrack, false );
if ( tt == null ) throw new IllegalArgumentException( "error re-tracking track" );
return tt;
}
finally
{
guideTrack = null;
}
}
private void resetCache()
{
nodesMap = new OsmNodesMap();
nodesCache = new NodesCache(segmentDir, nodesMap, routingContext.expctxWay.lookupVersion, routingContext.carMode, nodesCache );
}
private OsmNode getStartNode( long startId )
{
// initialize the start-node
OsmNode start = nodesMap.get( startId );
if ( start == null )
{
start = new OsmNode( startId );
start.setHollow();
nodesMap.put( startId, start );
}
if ( !nodesCache.obtainNonHollowNode( start ) )
{
return null;
}
expandHollowLinkTargets( start, true );
return start;
}
private OsmPath getStartPath( OsmNode n1, OsmNode n2, MatchedWaypoint mwp, MatchedWaypoint endWp, boolean sameSegmentSearch )
{
OsmPath p = getStartPath( n1, n2, mwp.waypoint, endWp.crosspoint );
// special case: start+end on same segment
if ( sameSegmentSearch )
{
OsmPath pe = getEndPath( n1, p.getLink(), endWp.crosspoint, endWp.crosspoint );
OsmPath pt = getEndPath( n1, p.getLink(), null, endWp.crosspoint );
int costdelta = pt.cost - p.cost;
if ( pe.cost >= costdelta )
{
pe.cost -= costdelta;
pe.adjustedCost -= costdelta;
if ( guideTrack != null )
{
// nasty stuff: combine the path cause "new OsmPath()" cannot handle start+endpoint
OsmPathElement startElement = p.originElement;
while( startElement.origin != null )
{
startElement = startElement.origin;
}
if ( pe.originElement.cost > costdelta )
{
OsmPathElement e = pe.originElement;
while( e.origin != null && e.origin.cost > costdelta )
{
e = e.origin;
e.cost -= costdelta;
}
e.origin = startElement;
}
else
{
pe.originElement = startElement;
}
}
return pe;
}
}
return p;
}
private OsmPath getStartPath( OsmNode n1, OsmNode n2, OsmNodeNamed wp, OsmNode endPos )
{
try
{
routingContext.setWaypoint( wp, false );
OsmPath bestPath = null;
OsmLink bestLink = null;
OsmLink startLink = new OsmLink();
startLink.targetNode = n1;
OsmPath startPath = new OsmPath( startLink );
startLink.addLinkHolder( startPath );
double minradius = 1e10;
for( OsmLink link = n1.firstlink; link != null; link = link.next )
{
OsmNode nextNode = link.targetNode;
if ( nextNode.isHollow() ) continue; // border node?
if ( nextNode.firstlink == null ) continue; // don't care about dead ends
if ( nextNode == n1 ) continue; // ?
if ( nextNode != n2 ) continue; // just that link
wp.radius = 1e9;
OsmPath testPath = new OsmPath( null, startPath, link, null, guideTrack != null, routingContext );
testPath.setAirDistanceCostAdjustment( (int)( nextNode.calcDistance( endPos ) * airDistanceCostFactor ) );
if ( wp.radius < minradius )
{
bestPath = testPath;
minradius = wp.radius;
bestLink = link;
}
}
if ( bestLink != null )
{
bestLink.addLinkHolder( bestPath );
}
bestPath.treedepth = 1;
return bestPath;
}
finally
{
routingContext.unsetWaypoint();
}
}
private OsmPath getEndPath( OsmNode n1, OsmLink link, OsmNodeNamed wp, OsmNode endPos )
{
try
{
if ( wp != null ) routingContext.setWaypoint( wp, true );
OsmLink startLink = new OsmLink();
startLink.targetNode = n1;
OsmPath startPath = new OsmPath( startLink );
startLink.addLinkHolder( startPath );
if ( wp != null ) wp.radius = 1e-5;
OsmPath testPath = new OsmPath( n1, startPath, link, null, guideTrack != null, routingContext );
testPath.setAirDistanceCostAdjustment( 0 );
return testPath;
}
finally
{
if ( wp != null ) routingContext.unsetWaypoint();
}
}
private OsmTrack findTrack( String operationName, MatchedWaypoint startWp, MatchedWaypoint endWp, OsmTrack costCuttingTrack, OsmTrack refTrack, boolean reducedTimeoutWhenUnmatched )
{
boolean verbose = guideTrack != null;
int maxTotalCost = 1000000000;
logInfo( "findtrack with maxTotalCost=" + maxTotalCost + " airDistanceCostFactor=" + airDistanceCostFactor );
matchPath = null;
int nodesVisited = 0;
resetCache();
long endNodeId1 = endWp.node1.getIdFromPos();
long endNodeId2 = endWp.node2.getIdFromPos();
long startNodeId1 = startWp.node1.getIdFromPos();
long startNodeId2 = startWp.node2.getIdFromPos();
OsmNode endPos = endWp.crosspoint;
boolean sameSegmentSearch = ( startNodeId1 == endNodeId1 && startNodeId2 == endNodeId2 )
|| ( startNodeId1 == endNodeId2 && startNodeId2 == endNodeId1 );
OsmNode start1 = getStartNode( startNodeId1 );
OsmNode start2 = getStartNode( startNodeId2 );
if ( start1 == null || start2 == null ) return null;
OsmPath startPath1 = getStartPath( start1, start2, startWp, endWp, sameSegmentSearch );
OsmPath startPath2 = getStartPath( start2, start1, startWp, endWp, sameSegmentSearch );
int maxAdjCostFromQueue = 0;
synchronized( openSet )
{
openSet.clear();
if ( startPath1.cost >= 0 ) openSet.add( startPath1 );
if ( startPath2.cost >= 0 ) openSet.add( startPath2 );
}
while(!terminated)
{
if ( maxRunningTime > 0 )
{
long timeout = ( matchPath == null && reducedTimeoutWhenUnmatched ) ? maxRunningTime/3 : maxRunningTime;
if ( System.currentTimeMillis() - startTime > timeout )
{
throw new IllegalArgumentException( operationName + " timeout after " + (timeout/1000) + " seconds" );
}
}
OsmPath path = null;
synchronized( openSet )
{
if ( openSet.size() == 0 ) break;
path = openSet.first();
openSet.remove( path );
}
if ( path.adjustedCost < maxAdjCostFromQueue && airDistanceCostFactor == 0.)
{
throw new RuntimeException( "assertion failed: path.adjustedCost < maxAdjCostFromQueue: " + path.adjustedCost + "<" + maxAdjCostFromQueue );
}
maxAdjCostFromQueue = path.adjustedCost;
nodesVisited++;
linksProcessed++;
OsmLink currentLink = path.getLink();
OsmNode currentNode = currentLink.targetNode;
OsmNode sourceNode = path.getSourceNode();
long currentNodeId = currentNode.getIdFromPos();
if ( sourceNode != null )
{
long sourceNodeId = sourceNode.getIdFromPos();
if ( ( sourceNodeId == endNodeId1 && currentNodeId == endNodeId2 )
|| ( sourceNodeId == endNodeId2 && currentNodeId == endNodeId1 ) )
{
// track found, compile
logInfo( "found track at cost " + path.cost + " nodesVisited = " + nodesVisited );
return compileTrack( path, verbose );
}
}
// recheck cutoff before doing expensive stuff
int airDistance2 = currentNode.calcDistance( endPos );
if ( path.cost + airDistance2 > maxTotalCost + 10 )
{
continue;
}
if ( !currentNode.wasProcessed )
{
expandHollowLinkTargets( currentNode, true );
nodesMap.removeCompletedNodes();
}
if ( sourceNode != null )
{
sourceNode.unlinkLink ( currentLink );
}
OsmLink counterLink = null;
for( OsmLink link = currentNode.firstlink; link != null; link = link.next )
{
OsmNode nextNode = link.targetNode;
if ( nextNode.isHollow() )
{
continue; // border node?
}
if ( nextNode.firstlink == null )
{
continue; // don't care about dead ends
}
if ( nextNode == sourceNode )
{
counterLink = link;
continue; // border node?
}
if ( guideTrack != null )
{
int gidx = path.treedepth + 1;
if ( gidx >= guideTrack.nodes.size() )
{
continue;
}
OsmPathElement guideNode = guideTrack.nodes.get( gidx );
if ( nextNode.getILat() != guideNode.getILat() || nextNode.getILon() != guideNode.getILon() )
{
continue;
}
}
OsmPath bestPath = null;
boolean isFinalLink = false;
long targetNodeId = link.targetNode.getIdFromPos();
if ( currentNodeId == endNodeId1 || currentNodeId == endNodeId2 )
{
if ( targetNodeId == endNodeId1 || targetNodeId == endNodeId2 )
{
isFinalLink = true;
}
}
for( OsmLinkHolder linkHolder = currentLink.firstlinkholder; linkHolder != null; linkHolder = linkHolder.getNextForLink() )
{
OsmPath otherPath = (OsmPath)linkHolder;
try
{
if ( isFinalLink )
{
endWp.crosspoint.radius = 1e-5;
routingContext.setWaypoint( endWp.crosspoint, true );
}
OsmPath testPath = new OsmPath( currentNode, otherPath, link, refTrack, guideTrack != null, routingContext );
if ( testPath.cost >= 0 && ( bestPath == null || testPath.cost < bestPath.cost ) )
{
bestPath = testPath;
}
}
finally
{
routingContext.unsetWaypoint();
}
if ( otherPath != path )
{
synchronized( openSet )
{
openSet.remove( otherPath );
}
}
}
if ( bestPath != null )
{
int airDistance = isFinalLink ? 0 : nextNode.calcDistance( endPos );
bestPath.setAirDistanceCostAdjustment( (int)( airDistance * airDistanceCostFactor ) );
// check for a match with the cost-cutting-track
if ( costCuttingTrack != null )
{
OsmPathElement pe = costCuttingTrack.getLink( currentNodeId, targetNodeId );
if ( pe != null )
{
int costEstimate = bestPath.cost
+ bestPath.elevationCorrection( routingContext )
+ ( costCuttingTrack.cost - pe.cost );
if ( costEstimate <= maxTotalCost )
{
matchPath = new OsmPathElement( bestPath );
}
if ( costEstimate < maxTotalCost )
{
logInfo( "maxcost " + maxTotalCost + " -> " + costEstimate + " airDistance=" + airDistance );
maxTotalCost = costEstimate;
}
}
}
if ( isFinalLink || bestPath.cost + airDistance <= maxTotalCost + 10 )
{
// add only if this may beat an existing path for that link
OsmLinkHolder dominator = link.firstlinkholder;
while( dominator != null )
{
if ( bestPath.definitlyWorseThan( (OsmPath)dominator, routingContext ) )
{
break;
}
dominator = dominator.getNextForLink();
}
if ( dominator == null )
{
bestPath.treedepth = path.treedepth + 1;
link.addLinkHolder( bestPath );
synchronized( openSet )
{
openSet.add( bestPath );
}
}
}
}
}
// if the counterlink does not yet have a path, remove it
if ( counterLink != null && counterLink.firstlinkholder == null )
{
currentNode.unlinkLink(counterLink);
}
}
return null;
}
private void preloadPosition( OsmNode n, int minRingWidth, int minCount )
{
int c = 0;
int ring = 0;
while( ring <= minRingWidth || ( c < minCount && ring <= 5 ) )
{
c += preloadRing( n, ring++ );
}
}
private int preloadRing( OsmNode n, int ring )
{
int d = 12500;
int c = 0;
for( int idxLat=-ring; idxLat<=ring; idxLat++ )
for( int idxLon=-ring; idxLon<=ring; idxLon++ )
{
int absLat = idxLat < 0 ? -idxLat : idxLat;
int absLon = idxLon < 0 ? -idxLon : idxLon;
int max = absLat > absLon ? absLat : absLon;
if ( max < ring ) continue;
c += nodesCache.loadSegmentFor( n.ilon + d*idxLon , n.ilat +d*idxLat );
}
return c;
}
private OsmTrack compileTrack( OsmPath path, boolean verbose )
{
OsmPathElement element = new OsmPathElement( path );
// for final track, cut endnode
if ( guideTrack != null ) element = element.origin;
OsmTrack track = new OsmTrack();
track.cost = path.cost;
int distance = 0;
double ascend = 0;
double ehb = 0.;
short ele_start = Short.MIN_VALUE;
short ele_end = Short.MIN_VALUE;
while ( element != null )
{
track.addNode( element );
OsmPathElement nextElement = element.origin;
short ele = element.getSElev();
if ( ele != Short.MIN_VALUE ) ele_start = ele;
if ( ele_end == Short.MIN_VALUE ) ele_end = ele;
if ( nextElement != null )
{
distance += element.calcDistance( nextElement );
short ele_next = nextElement.getSElev();
if ( ele_next != Short.MIN_VALUE )
{
ehb = ehb + (ele - ele_next)/4.;
}
if ( ehb > 10. )
{
ascend += ehb-10.;
ehb = 10.;
}
else if ( ehb < 0. )
{
ehb = 0.;
}
}
element = nextElement ;
}
ascend += ehb;
track.distance = distance;
track.ascend = (int)ascend;
track.plainAscend = ( ele_end - ele_start ) / 4;
logInfo( "track-length = " + track.distance );
logInfo( "filtered ascend = " + track.ascend );
track.buildMap();
return track;
}
private OsmTrack mergeTrack( OsmPathElement match, OsmTrack oldTrack )
{
OsmPathElement element = match;
OsmTrack track = new OsmTrack();
while ( element != null )
{
track.addNode( element );
element = element.origin ;
}
long lastId = 0;
long id1 = match.getIdFromPos();
long id0 = match.origin == null ? 0 : match.origin.getIdFromPos();
boolean appending = false;
for( OsmPathElement n : oldTrack.nodes )
{
if ( appending )
{
track.nodes.add( n );
}
long id = n.getIdFromPos();
if ( id == id1 && lastId == id0 )
{
appending = true;
}
lastId = id;
}
track.buildMap();
return track;
}
public int[] getOpenSet()
{
synchronized( openSet )
{
return openSet.getExtract();
}
}
public boolean isFinished()
{
return finished;
}
public int getLinksProcessed()
{
return linksProcessed;
}
public int getDistance()
{
return foundTrack.distance;
}
public int getAscend()
{
return foundTrack.ascend;
}
public int getPlainAscend()
{
return foundTrack.plainAscend;
}
public OsmTrack getFoundTrack()
{
return foundTrack;
}
public int getAlternativeIndex()
{
return alternativeIndex;
}
public OsmTrack getFoundRawTrack()
{
return foundRawTrack;
}
public String getErrorMessage()
{
return errorMessage;
}
public void terminate()
{
terminated = true;
}
}

28
brouter-core/src/main/java/btools/router/RoutingMessageHandler.java Normal file
View file

@ -0,0 +1,28 @@
/**
* Container for routig configs
*
* @author ab
*/
package btools.router;
import btools.expressions.BExpressionReceiver;
final class RoutingMessageHandler implements BExpressionReceiver
{
private int ilon;
private int ilat;
public void setCurrentPos( int lon, int lat)
{
ilon = lon;
ilat = lat;
}
@Override
public void expressionWarning( String context, String message )
{
System.out.println( "message (lon=" + (ilon-180000000) + " lat=" + (ilat-90000000)
+ " context " + context + "): " + message );
}
}