refactored waypoint-matching into map-access-layer

brouter-mapaccess/src/main/java/btools/mapaccess/MatchedWaypoint.java

@@ -0,0 +1,58 @@
+/**
+ * Information on matched way point
+ *
+ * @author ab
+ */
+package btools.mapaccess;
+
+import java.io.DataInput;
+import java.io.DataOutput;
+import java.io.IOException;
+
+import btools.mapaccess.OsmNode;
+
+public final class MatchedWaypoint
+{
+  public OsmNode node1;
+  public OsmNode node2;
+  public OsmNode crosspoint;
+  public OsmNode waypoint;
+  public String name;  // waypoint name used in error messages
+  public double radius;  // distance in meter between waypoint and crosspoint
+  
+  public boolean hasUpdate;
+
+  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 OsmNode();
+	mwp.waypoint = new OsmNode();
+
+	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;
+  }
+
+}

brouter-mapaccess/src/main/java/btools/mapaccess/NodesCache.java

@@ -8,6 +8,7 @@ package btools.mapaccess;
 import java.io.File;
 import java.io.IOException;
 import java.util.HashMap;
+import java.util.List;
 
 import btools.codec.DataBuffers;
 import btools.codec.MicroCache;
@@ -53,10 +54,10 @@ public final class NodesCache
     return "collecting=" + garbageCollectionEnabled + " noGhosts=" + ghostCleaningDone + " cacheSum=" + cacheSum + " cacheSumClean=" + cacheSumClean + " ghostSum=" + ghostSum + " ghostWakeup=" + ghostWakeup ;
   }
 
-  public NodesCache( String segmentDir, OsmNodesMap nodesMap, BExpressionContextWay ctxWay, boolean forceSecondaryData, long maxmem, NodesCache oldCache, boolean detailed )
+  public NodesCache( String segmentDir, BExpressionContextWay ctxWay, boolean forceSecondaryData, long maxmem, NodesCache oldCache, boolean detailed )
   {
     this.segmentDir = new File( segmentDir );
-    this.nodesMap = nodesMap;
+    this.nodesMap = new OsmNodesMap();
     this.expCtxWay = ctxWay;
     this.lookupVersion = ctxWay.meta.lookupVersion;
     this.lookupMinorVersion = ctxWay.meta.lookupMinorVersion;
@@ -275,6 +276,71 @@ public final class NodesCache
     }
   }
 
+  /**
+   * get a node for the given id with all link-targets also non-hollow
+   * 
+   * It is required that an instance of the start-node does not yet
+   * exist, not even a hollow instance, so getStartNode should only
+   * be called once right after resetting the cache
+   * 
+   * @param id the id of the node to load
+   * 
+   * @return the fully expanded node for id, or null if it was not found
+   */
+  public OsmNode getStartNode( long id )
+  {
+    // initialize the start-node
+    OsmNode n = new OsmNode( id );
+    n.setHollow();
+    if ( !obtainNonHollowNode( n ) )
+    {
+      return null;
+    }
+    expandHollowLinkTargets( n );
+    return n;
+  }
+
+  public void matchWaypointsToNodes( List<MatchedWaypoint> unmatchedWaypoints, double maxDistance, OsmNodePairSet islandNodePairs )
+  {
+    waypointMatcher = new WaypointMatcherImpl( unmatchedWaypoints, 250., islandNodePairs );
+    for( MatchedWaypoint mwp : unmatchedWaypoints )
+    {
+      preloadPosition( mwp.waypoint );
+    }
+
+    if ( first_file_access_failed )
+    {
+      throw new IllegalArgumentException( "datafile " + first_file_access_name + " not found" );
+    }
+    for( MatchedWaypoint mwp : unmatchedWaypoints )
+    {
+      if ( mwp.crosspoint == null )
+      {
+        throw new IllegalArgumentException( mwp.name + "-position not mapped in existing datafile" );
+      }
+    }
+  }
+
+  private void preloadPosition( OsmNode n )
+  {
+    int d = 12500;
+    first_file_access_failed = false;
+    first_file_access_name = null;
+    loadSegmentFor( n.ilon, n.ilat );
+    if ( first_file_access_failed )
+    {
+      throw new IllegalArgumentException( "datafile " + first_file_access_name + " not found" );
+    }
+    for( int idxLat=-1; idxLat<=1; idxLat++ )
+      for( int idxLon=-1; idxLon<=1; idxLon++ )
+      {
+        if ( idxLon != 0 || idxLat != 0 )
+        {
+          loadSegmentFor( n.ilon + d*idxLon , n.ilat +d*idxLat );
+        }
+      }
+  }
+
   private OsmFile fileForSegment( int lonDegree, int latDegree ) throws Exception
   {
     int lonMod5 = lonDegree % 5;

brouter-mapaccess/src/main/java/btools/mapaccess/WaypointMatcherImpl.java

@@ -0,0 +1,157 @@
+package btools.mapaccess;
+
+import java.util.List;
+
+import btools.codec.WaypointMatcher;
+import btools.mapaccess.OsmNode;
+import btools.mapaccess.OsmNodePairSet;
+import btools.util.CheapRuler;
+
+/**
+ * the WaypointMatcher is feeded by the decoder with geoemtries of ways that are
+ * already check for allowed access according to the current routing profile
+ *
+ * It matches these geometries against the list of waypoints to find the best
+ * match for each waypoint
+ */
+public final class WaypointMatcherImpl implements WaypointMatcher
+{
+  private List<MatchedWaypoint> waypoints;
+  private OsmNodePairSet islandPairs;
+
+  private int lonStart;
+  private int latStart;
+  private int lonTarget;
+  private int latTarget;
+  private boolean anyUpdate;
+  private int lonLast;
+  private int latLast;
+
+  public WaypointMatcherImpl( List<MatchedWaypoint> waypoints, double maxDistance, OsmNodePairSet islandPairs )
+  {
+    this.waypoints = waypoints;
+    this.islandPairs = islandPairs;
+    for ( MatchedWaypoint mwp : waypoints )
+    {
+      mwp.radius = maxDistance;
+    }
+  }
+
+  private void checkSegment( int lon1, int lat1, int lon2, int lat2 )
+  {
+    // todo: bounding-box pre-filter
+
+    double[] lonlat2m = CheapRuler.getLonLatToMeterScales( (lat1+lat2) >> 1 );
+    double dlon2m = lonlat2m[0];
+    double dlat2m = lonlat2m[1];
+
+    double dx = ( lon2 - lon1 ) * dlon2m;
+    double dy = ( lat2 - lat1 ) * dlat2m;
+    double d = Math.sqrt( dy * dy + dx * dx );
+
+    if ( d == 0. )
+      return;
+
+    for ( MatchedWaypoint mwp : waypoints )
+    {
+      OsmNode wp = mwp.waypoint;
+
+      double x1 = ( lon1 - wp.ilon ) * dlon2m;
+      double y1 = ( lat1 - wp.ilat ) * dlat2m;
+      double x2 = ( lon2 - wp.ilon ) * dlon2m;
+      double y2 = ( lat2 - wp.ilat ) * dlat2m;
+      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 < mwp.radius )
+      {
+        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 > mwp.radius )
+            continue;
+        }
+        // new match for that waypoint
+        mwp.radius = radius; // shortest distance to way
+        mwp.hasUpdate = true;
+        anyUpdate = true;
+        // calculate crosspoint
+        if ( mwp.crosspoint == null )
+          mwp.crosspoint = new OsmNode();
+        if ( s2 < 0. )
+        {
+          double wayfraction = -s2 / ( d * d );
+          double xm = x2 - wayfraction * dx;
+          double ym = y2 - wayfraction * dy;
+          mwp.crosspoint.ilon = (int) ( xm / dlon2m + wp.ilon );
+          mwp.crosspoint.ilat = (int) ( ym / dlat2m + wp.ilat );
+        }
+        else if ( s1 > s2 )
+        {
+          mwp.crosspoint.ilon = lon2;
+          mwp.crosspoint.ilat = lat2;
+        }
+        else
+        {
+          mwp.crosspoint.ilon = lon1;
+          mwp.crosspoint.ilat = lat1;
+        }
+      }
+    }
+  }
+
+  @Override
+  public boolean start( int ilonStart, int ilatStart, int ilonTarget, int ilatTarget )
+  {
+    if ( islandPairs.size() > 0 )
+    {
+      long n1 = ( (long) ilonStart ) << 32 | ilatStart;
+      long n2 = ( (long) ilonTarget ) << 32 | ilatTarget;
+      if ( islandPairs.hasPair( n1, n2 ) )
+      {
+        return false;
+      }
+    }
+    lonLast = lonStart = ilonStart;
+    latLast = latStart = ilatStart;
+    lonTarget = ilonTarget;
+    latTarget = ilatTarget;
+    anyUpdate = false;
+    return true;
+  }
+
+  @Override
+  public void transferNode( int ilon, int ilat )
+  {
+    checkSegment( lonLast, latLast, ilon, ilat );
+    lonLast = ilon;
+    latLast = ilat;
+  }
+
+  @Override
+  public void end()
+  {
+    checkSegment( lonLast, latLast, lonTarget, latTarget );
+    if ( anyUpdate )
+    {
+      for ( MatchedWaypoint mwp : waypoints )
+      {
+        if ( mwp.hasUpdate )
+        {
+          mwp.hasUpdate = false;
+          mwp.node1 = new OsmNode( lonStart, latStart );
+          mwp.node2 = new OsmNode( lonTarget, latTarget );
+        }
+      }
+    }
+  }
+}