rework same-segment search logic

brouter-core/src/main/java/btools/router/OsmPath.java

@@ -298,6 +298,16 @@ abstract class OsmPath implements OsmLinkHolder
               originElement = null; // prevent duplicate point
             }
           }
+
+          if ( rc.checkPendingEndpoint() )
+          {
+            dist = rc.calcDistance( rc.ilonshortest, rc.ilatshortest, lon2, lat2 );
+            if ( rc.shortestmatch )
+            {
+              stopAtEndpoint = true;
+              ele2 = interpolateEle( ele1, ele2, rc.wayfraction );
+            }
+          }
         }
       }
 

brouter-core/src/main/java/btools/router/RoutingContext.java

@@ -192,6 +192,7 @@ public final class RoutingContext
 
   public List<OsmNodeNamed> nogopoints = null;
   private List<OsmNodeNamed> keepnogopoints = null;
+  private OsmNodeNamed pendingEndpoint = null;
 
   public Integer startDirection;
   public boolean startDirectionValid;
@@ -297,17 +298,36 @@ public final class RoutingContext
   }
 
   public void setWaypoint( OsmNodeNamed wp, boolean endpoint )
+  {
+    setWaypoint( wp, null, endpoint );
+  }
+
+  public void setWaypoint( OsmNodeNamed wp, OsmNodeNamed pendingEndpoint, boolean endpoint )
   {
     keepnogopoints = nogopoints;
     nogopoints = new ArrayList<OsmNodeNamed>();
     nogopoints.add( wp );
     if ( keepnogopoints != null ) nogopoints.addAll( keepnogopoints );
     isEndpoint = endpoint;
+    this.pendingEndpoint = pendingEndpoint;
+  }
+
+  public boolean checkPendingEndpoint()
+  {
+    if ( pendingEndpoint != null )
+    {
+      isEndpoint = true;
+      nogopoints.set( 0, pendingEndpoint );
+      pendingEndpoint = null;
+      return true;
+    }
+    return false;
   }
 
   public void unsetWaypoint()
   {
     nogopoints = keepnogopoints;
+    pendingEndpoint = null;
     isEndpoint = false;
   }
 

brouter-core/src/main/java/btools/router/RoutingEngine.java

@@ -587,62 +587,27 @@ public class RoutingEngine extends Thread
 
   private OsmPath getStartPath( OsmNode n1, OsmNode n2, MatchedWaypoint mwp, OsmNodeNamed endPos, boolean sameSegmentSearch )
   {
-    OsmPath p = getStartPath( n1, n2, new OsmNodeNamed( mwp.waypoint ), endPos );
+    if ( endPos != null )
+    {
+      endPos.radius = 1.5;
+    }
+    OsmPath p = getStartPath( n1, n2, new OsmNodeNamed( mwp.crosspoint ), endPos, sameSegmentSearch );
 
     // special case: start+end on same segment
-    if ( p.cost >= 0 && sameSegmentSearch )
+    if ( p.cost >= 0 && sameSegmentSearch && endPos != null && endPos.radius < 1.5 )
     {
-      OsmPath path2end = getStartPath( n1, n2, endPos, endPos );
+     p.treedepth = 0; // hack: mark for the final-check
-      boolean isDirect = path2end.cost >= p.cost;
-
-      OsmPath pe = getEndPath( n1, p.getLink(), endPos );
-      OsmPath pt = getEndPath( n1, p.getLink(), null );
-      int costdelta = pt.cost - p.cost;
-      if ( isDirect && pe.cost < costdelta )
-      {
-         costdelta = pe.cost;
-      }
-      if ( pe.cost >= costdelta )
-      {
-    	pe.cost -= 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;
-    	  }
-    	}
-    	  pe.treedepth = 0; // hack: mark for the final-check
-        return pe;
-      }
     }
     return p;
   }
 
 
 
-  private OsmPath getStartPath( OsmNode n1, OsmNode n2, OsmNodeNamed wp, OsmNodeNamed endPos )
+  private OsmPath getStartPath( OsmNode n1, OsmNode n2, OsmNodeNamed wp, OsmNodeNamed endPos, boolean sameSegmentSearch )
   {
     try
     {
-      routingContext.setWaypoint( wp, false );
+      routingContext.setWaypoint( wp, sameSegmentSearch ? endPos : null, false );
       OsmPath bestPath = null;
       OsmLink bestLink = null;
       OsmLink startLink = new OsmLink( null, n1 );
@@ -657,7 +622,7 @@ public class RoutingEngine extends Thread
         if ( nextNode == n1 ) continue; // ?
         if ( nextNode != n2 ) continue; // just that link
 
-         wp.radius = 1e9;
+         wp.radius = 1.5;
          OsmPath testPath = routingContext.createPath( startPath, link, null, guideTrack != null );
          testPath.airdistance = endPos == null ? 0 : nextNode.calcDistance( endPos );
          if ( wp.radius < minradius )
@@ -681,25 +646,6 @@ public class RoutingEngine extends Thread
     }
   }
 
-  private OsmPath getEndPath( OsmNode n1, OsmLink link, OsmNodeNamed wp )
-  {
-    try
-    {
-      if ( wp != null ) routingContext.setWaypoint( wp, true );
-      OsmLink startLink = new OsmLink( null, n1 );
-      OsmPath startPath = routingContext.createPath( startLink );
-      startLink.addLinkHolder( startPath, null );
-
-      if ( wp != null ) wp.radius = 1.5;
-
-      return routingContext.createPath( startPath, link, null, guideTrack != null );
-    }
-    finally
-    {
-      if ( wp != null ) routingContext.unsetWaypoint();
-    }
-  }
-
   private OsmTrack findTrack( String operationName, MatchedWaypoint startWp, MatchedWaypoint endWp, OsmTrack costCuttingTrack, OsmTrack refTrack, boolean fastPartialRecalc )
   {
     try
@@ -985,13 +931,16 @@ public class RoutingEngine extends Thread
         ((OsmPath)linkHolder).airdistance = -1; // invalidate the entry in the open set;
       }
 
+      if ( path.treedepth > 1 )
+      {
         boolean isBidir = currentLink.isBidirectional();
-      sourceNode.unlinkLink ( currentLink );
+        sourceNode.unlinkLink( currentLink );
 
         // if the counterlink is alive and does not yet have a path, remove it
         if ( isBidir && currentLink.getFirstLinkHolder( currentNode ) == null && !routingContext.considerTurnRestrictions )
         {
-        currentNode.unlinkLink(currentLink);
+          currentNode.unlinkLink( currentLink );
+        }
       }
 
       // recheck cutoff before doing expensive stuff
@@ -1123,7 +1072,8 @@ public class RoutingEngine extends Thread
             OsmLinkHolder dominator = link.getFirstLinkHolder( currentNode );
             while( !trafficSim && dominator != null )
             {
-              if ( bestPath.definitlyWorseThan( (OsmPath)dominator, routingContext ) )
+              OsmPath dp = (OsmPath)dominator;
+              if ( dp.airdistance != -1 && bestPath.definitlyWorseThan( dp, routingContext ) )
               {
                 break;
               }