voice hints, first draft

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

@@ -16,6 +16,8 @@ final class MessageData implements Cloneable
   int linkinitcost = 0;
   
   float costfactor;
+  float priorityclassifier;
+  float turnangle;
   String wayKeyValues;
   String nodeKeyValues;
 
@@ -64,4 +66,10 @@ final class MessageData implements Cloneable
       throw new RuntimeException( e );
     }
   }
+
+  @Override
+  public String toString()
+  {
+    return "dist=" + linkdist + " prio=" + priorityclassifier + " turn=" + turnangle;
+  }
 }

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

@@ -6,7 +6,6 @@
 package btools.router;
 
 import java.io.IOException;
-import java.util.ArrayList;
 
 import btools.mapaccess.OsmLink;
 import btools.mapaccess.OsmLinkHolder;
@@ -162,7 +161,7 @@ final class OsmPath implements OsmLinkHolder
       boolean sameData = rc.expctxWay.evaluate( rc.inverseDirection ^ link.counterLinkWritten, description, rc.messageHandler );
       
       // if way description changed, store message
-      if ( recordMessageData && msgData.wayKeyValues != null && !sameData )
+      if ( recordMessageData && msgData.wayKeyValues != null )
       {
         originElement.message = msgData;
         msgData = new MessageData();
@@ -212,6 +211,10 @@ final class OsmPath implements OsmLinkHolder
         int turncost = (int)(cos * rc.expctxWay.getTurncost() + 0.2 ); // e.g. turncost=90 -> 90 degree = 90m penalty
         cost += turncost;
         msgData.linkturncost += turncost;
+        if ( recordMessageData )
+        {
+          msgData.turnangle = (float)rc.calcAngle( lon0, lat0, lon1, lat1, lon2, lat2 );
+        }
       }
 
       // *** penalty for elevation (penalty is for descend! in a way that slow descends give no penalty)
@@ -334,6 +337,7 @@ final class OsmPath implements OsmLinkHolder
       if ( recordMessageData )
       {
         msgData.costfactor = costfactor;
+        msgData.priorityclassifier = rc.expctxWay.getPriorityClassifier();
         msgData.lon = lon2;
         msgData.lat = lat2;
         msgData.ele = ele2;

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

@@ -43,6 +43,10 @@ public final class OsmTrack
 
   private CompactLongMap<OsmPathElementHolder> nodesMap;
 
+  private CompactLongMap<OsmPathElementHolder> detourMap;
+
+  private List<VoiceHint> voiceHints;
+
   public String message = null;
   public ArrayList<String> messageList = null;
 
@@ -53,6 +57,34 @@ public final class OsmTrack
     nodes.add( 0, node );
   }
 
+  public void registerDetourForId( long id, OsmPathElement detour )
+  {
+    if ( detourMap == null )
+    {
+      detourMap = new CompactLongMap<OsmPathElementHolder>();
+    }
+    OsmPathElementHolder nh = new OsmPathElementHolder();
+    nh.node = detour;
+    OsmPathElementHolder h = detourMap.get( id );
+    if ( h != null )
+    {
+      while ( h.nextHolder != null )
+      {
+        h = h.nextHolder;
+      }
+      h.nextHolder = nh;
+    }
+    else
+    {
+      detourMap.fastPut( id, nh );
+    }
+  }
+
+  public void copyDetours( OsmTrack source )
+  {
+    detourMap = new FrozenLongMap<OsmPathElementHolder>( source.detourMap );
+  }
+
   public void buildMap()
   {
     nodesMap = new CompactLongMap<OsmPathElementHolder>();
@@ -226,6 +258,15 @@ public final class OsmTrack
         nodes.add( t.nodes.get( i ) );
       }
     }
+
+    if ( t.voiceHints != null )
+    {
+      for( VoiceHint hint : t.voiceHints )
+      {
+        addVoiceHint( hint );
+      }
+    }
+
     distance += t.distance;
     ascend += t.ascend;
     plainAscend += t.plainAscend;
@@ -269,6 +310,20 @@ public final class OsmTrack
     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-1.3.2\" version=\"1.1\">\n" );
+
+    if ( voiceHints != null )
+    {
+      for( VoiceHint hint:    voiceHints )
+      {
+        sb.append( " <wpt lon=\"" ).append( formatILon( hint.ilon ) ).append( "\" lat=\"" )
+          .append( formatILat( hint.ilat ) ).append( "\">" )
+          .append( "<name>" ).append( hint.message ).append( "</name>" )
+          .append( "<sym>" ).append( hint.symbol ).append( "</sym>" )
+          .append( "<type>" ).append( hint.symbol ).append( "</type>" )
+          .append( "</wpt>\n" );
+      }
+    }
+
     sb.append( " <trk>\n" );
     sb.append( "  <name>" ).append( name ).append( "</name>\n" );
     sb.append( "  <trkseg>\n" );
@@ -276,8 +331,8 @@ public final class OsmTrack
     for ( OsmPathElement n : nodes )
     {
       String sele = n.getSElev() == Short.MIN_VALUE ? "" : "<ele>" + n.getElev() + "</ele>";
-      sb.append( "   <trkpt lon=\"" ).append( formatPos( n.getILon() - 180000000 ) ).append( "\" lat=\"" )
+      sb.append( "   <trkpt lon=\"" ).append( formatILon( n.getILon() ) ).append( "\" lat=\"" )
-          .append( formatPos( n.getILat() - 90000000 ) ).append( "\">" ).append( sele ).append( "</trkpt>\n" );
+          .append( formatILat( n.getILat() ) ).append( "\">" ).append( sele ).append( "</trkpt>\n" );
     }
 
     sb.append( "  </trkseg>\n" );
@@ -322,7 +377,7 @@ public final class OsmTrack
 
     for ( OsmPathElement n : nodes )
     {
-      sb.append( formatPos( n.getILon() - 180000000 ) ).append( "," ).append( formatPos( n.getILat() - 90000000 ) ).append( "\n" );
+      sb.append( formatILon( n.getILon() ) ).append( "," ).append( formatILat( n.getILat() ) ).append( "\n" );
     }
 
     sb.append( "          </coordinates>\n" );
@@ -381,7 +436,7 @@ public final class OsmTrack
     for ( OsmPathElement n : nodes )
     {
       String sele = n.getSElev() == Short.MIN_VALUE ? "" : ", " + n.getElev();
-      sb.append( "          [" ).append( formatPos( n.getILon() - 180000000 ) ).append( ", " ).append( formatPos( n.getILat() - 90000000 ) )
+      sb.append( "          [" ).append( formatILon( n.getILon() ) ).append( ", " ).append( formatILat( n.getILat() ) )
           .append( sele ).append( "],\n" );
     }
     sb.deleteCharAt( sb.lastIndexOf( "," ) );
@@ -395,6 +450,16 @@ public final class OsmTrack
     return sb.toString();
   }
 
+  private static String formatILon( int ilon )
+  {
+    return formatPos(  ilon - 180000000 );
+  }
+
+  private static String formatILat( int ilat )
+  {
+    return formatPos(  ilat - 90000000 );
+  }
+
   private static String formatPos( int p )
   {
     boolean negative = p < 0;
@@ -488,4 +553,69 @@ public final class OsmTrack
     }
     return true;
   }
+
+  private void addVoiceHint( VoiceHint hint )
+  {
+    if ( voiceHints == null )
+    {
+      voiceHints = new ArrayList<VoiceHint>();
+    }
+    voiceHints.add( hint );
+  }
+
+  public void processVoiceHints()
+  {
+    OsmPathElement node = nodes.get( nodes.size() - 1 );
+    List<VoiceHint> inputs = new ArrayList<VoiceHint>();
+    while (node != null)
+    {
+      if ( node.origin != null )
+      {
+        VoiceHint input = new VoiceHint();
+        inputs.add( input );
+        input.ilat = node.origin.getILat();
+        input.ilon = node.origin.getILon();
+        input.goodWay = node.message;
+
+        OsmPathElementHolder detours = detourMap.get( node.origin.getIdFromPos() );
+        if ( detours != null )
+        {
+          OsmPathElementHolder h = detours;
+          while (h != null)
+          {
+            OsmPathElement e = h.node;
+            input.addBadWay( startSection( e, node.origin ) );
+            h = h.nextHolder;
+          }
+        }
+      }
+      node = node.origin;
+    }
+
+    List<VoiceHint> results = VoiceHintProcessor.process( inputs );
+    for( int i=results.size()-1; i >= 0; i-- )
+    {
+      addVoiceHint( results.get(i) );
+    }
+  }
+
+
+  private MessageData startSection( OsmPathElement element, OsmPathElement root )
+  {
+    OsmPathElement e = element;
+    int cnt = 0;
+    while( e != null )
+    {
+      if ( e.origin == root )
+      {
+        return e.message;
+      }
+      e = e.origin;
+      if ( cnt++ == 10000 )
+      {
+        throw new IllegalArgumentException( "ups?" );
+      }
+    }
+    return null;
+  }
 }

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

@@ -296,6 +296,42 @@ public final class RoutingContext implements DistanceChecker
     return 1.-cosp; // don't care to really do acos..
   }
 
+  public double calcAngle( 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 sinp = (dlat1*dlon2 - dlon1*dlat2)/dd;
+    double cosp = (dlat1*dlat2 + dlon1*dlon2)/dd;
+
+    double p;
+    if ( sinp > -0.7 && sinp < 0.7 )
+    {
+      p = Math.asin( sinp )*57.3;
+      if ( cosp < 0. )
+      {
+        p = 180. - p;
+      }
+    }
+    else
+    {
+      p = Math.acos( cosp )*57.3;
+      if ( sinp < 0. )
+      {
+        p = - p;
+      }
+    }
+    if ( p > 180. )
+    {
+      p -= 360.;
+    }
+    return p;
+  }
+
   @Override
   public boolean isWithinRadius( int ilon0, int ilat0, OsmTransferNode firstTransfer, int ilon1, int ilat1 )
   {

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

@@ -930,8 +930,15 @@ public class RoutingEngine extends Thread
             continue;
           }
           OsmPathElement guideNode = guideTrack.nodes.get( gidx );
-          if ( nextNode.getILat() != guideNode.getILat() || nextNode.getILon() != guideNode.getILon() )
+          long nextId = nextNode.getIdFromPos();
+          if ( nextId != guideNode.getIdFromPos() )
           {
+            // not along the guide-track, discard, but register for voice-hint processing
+            OsmPath detour = new OsmPath( currentNode, path, link, refTrack, true, routingContext );
+            if ( detour.cost >= 0. && nextId != startNodeId1 && nextId != startNodeId2 )
+            {
+              guideTrack.registerDetourForId( currentNode.getIdFromPos(), OsmPathElement.create( detour, false ) );
+            }
             continue;
           }
         }
@@ -1109,6 +1116,13 @@ public class RoutingEngine extends Thread
     logInfo( "track-length = " + track.distance );
     logInfo( "filtered ascend = " + track.ascend );
     track.buildMap();
+
+    // for final track..
+    if ( guideTrack != null )
+    {
+      track.copyDetours( guideTrack );
+      track.processVoiceHints();
+    }
     return track;
   }
 

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

@@ -0,0 +1,88 @@
+/**
+ * Container for a voice hint
+ * (both input- and result data for voice hint processing)
+ *
+ * @author ab
+ */
+package btools.router;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class VoiceHint
+{
+  int ilon;
+  int ilat;
+  String message;
+  String symbol;
+  int locusAction;
+  MessageData goodWay;
+  List<MessageData> badWays;
+  
+  public void addBadWay( MessageData badWay )
+  {
+    if ( badWay == null )
+    {
+      return;
+    }
+    if ( badWays == null )
+    {
+      badWays = new ArrayList<MessageData>();
+    }
+    badWays.add( badWay );
+  }
+  
+  public boolean setTurnAngle( float angle )
+  {
+    if ( angle < -165. || angle > 165. )
+    {
+      symbol = "TU";
+      message = "u-turn";
+      locusAction = 12;
+    }
+    else if ( angle < -115. )
+    {
+      symbol = "TSHL";
+      message = "sharp left";
+      locusAction = 5;
+    }
+    else if ( angle < -65. )
+    {
+      symbol = "Left";
+      message = "left";
+      locusAction = 4;
+    }
+    else if ( angle < -15. )
+    {
+      symbol = "TSLL";
+      message = "slight left";
+      locusAction = 3;
+    }
+    else if ( angle < 15. )
+    {
+      symbol = "Straight";
+      message = "straight";
+      locusAction = 1;
+      return false;
+    }
+    else if ( angle < 65. )
+    {
+      symbol = "TSLR";
+      message = "slight right";
+      locusAction = 6;
+    }
+    else if ( angle < 115. )
+    {
+      symbol = "Right";
+      message = "right";
+      locusAction = 7;
+    }
+    else
+    {
+      symbol = "TSHR";
+      message = "sharp right";
+      locusAction = 8;
+    }
+    return true;
+  }
+}

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

@@ -0,0 +1,44 @@
+/**
+ * Processor for Voice Hints
+ *
+ * @author ab
+ */
+package btools.router;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public final class VoiceHintProcessor
+{
+
+  public static List<VoiceHint> process( List<VoiceHint> inputs )
+  {
+    List<VoiceHint> results = new ArrayList<VoiceHint>();
+    for ( VoiceHint input : inputs )
+    {
+//      System.out.println( "***** processing: " + input.ilat + " " + input.ilon + " goodWay=" + input.goodWay );
+      if ( input.badWays != null )
+      {
+        float maxprio = 0.f;
+        for ( MessageData badWay : input.badWays )
+        {
+//          System.out.println( " --> badWay: " + badWay );
+          if ( badWay.priorityclassifier > maxprio )
+          {
+            maxprio = badWay.priorityclassifier;
+          }
+        }
+        if ( maxprio > 0. && maxprio >= input.goodWay.priorityclassifier )
+        {
+          boolean isTurn = input.setTurnAngle( input.goodWay.turnangle );
+          if ( isTurn || input.goodWay.priorityclassifier < maxprio )
+          {
+            results.add( input );
+          }
+        }
+      }
+    }
+    return results;
+  }
+
+}

brouter-expressions/src/main/java/btools/expressions/BExpressionContextWay.java

@@ -13,7 +13,7 @@ import btools.codec.TagValueValidator;
 public final class BExpressionContextWay extends BExpressionContext implements TagValueValidator
 {
   private static String[] buildInVariables =
-	  { "costfactor", "turncost", "uphillcostfactor", "downhillcostfactor", "initialcost", "nodeaccessgranted", "initialclassifier", "trafficsourcedensity", "istrafficbackbone" };
+	  { "costfactor", "turncost", "uphillcostfactor", "downhillcostfactor", "initialcost", "nodeaccessgranted", "initialclassifier", "trafficsourcedensity", "istrafficbackbone", "priorityclassifier" };
 	  
   protected String[] getBuildInVariableNames()
   {
@@ -29,6 +29,7 @@ public final class BExpressionContextWay extends BExpressionContext implements T
   public float getInitialClassifier() { return getBuildInVariable(6); }
   public float getTrafficSourceDensity() { return getBuildInVariable(7); }
   public float getIsTrafficBackbone() { return getBuildInVariable(8); }
+  public float getPriorityClassifier() { return getBuildInVariable(9); }
 
 
   public BExpressionContextWay( BExpressionMetaData meta )