diff --git a/brouter-core/src/main/java/btools/router/RoutingEngine.java b/brouter-core/src/main/java/btools/router/RoutingEngine.java index 105ed09..edf013d 100644 --- a/brouter-core/src/main/java/btools/router/RoutingEngine.java +++ b/brouter-core/src/main/java/btools/router/RoutingEngine.java @@ -20,6 +20,8 @@ import btools.mapaccess.OsmLink; import btools.mapaccess.OsmLinkHolder; import btools.mapaccess.OsmNode; import btools.mapaccess.OsmNodePairSet; +import btools.util.CheapAngleMeter; +import btools.util.CheapRuler; import btools.util.CompactLongMap; import btools.util.SortedHeap; import btools.util.StackSampler; @@ -587,6 +589,22 @@ public class RoutingEngine extends Thread { matchedWaypoints.add(mwp); } matchWaypointsToNodes(matchedWaypoints); + if (routingContext.startDirection != null) { + // add a nogo not to turn back + double angle = CheapAngleMeter.normalize(180 + routingContext.startDirection); + int[] np = CheapRuler.destination(matchedWaypoints.get(0).crosspoint.ilon, matchedWaypoints.get(0).crosspoint.ilat, 10, angle); + OsmNodeNamed n = new OsmNodeNamed(); + n.name = "nogo8"; + n.ilon = np[0]; + n.ilat = np[1]; + n.isNogo = true; + n.radius = 8; + n.nogoWeight = 9999; + if (routingContext.nogopoints == null) { + routingContext.nogopoints = new ArrayList<>(); + } + routingContext.nogopoints.add(n); + } routingContext.checkMatchedWaypointAgainstNogos(matchedWaypoints); diff --git a/brouter-util/src/main/java/btools/util/CheapRuler.java b/brouter-util/src/main/java/btools/util/CheapRuler.java index 7a7dc8f..bdd9360 100644 --- a/brouter-util/src/main/java/btools/util/CheapRuler.java +++ b/brouter-util/src/main/java/btools/util/CheapRuler.java @@ -77,4 +77,21 @@ public final class CheapRuler { double dlat = (ilat1 - ilat2) * kxky[1]; return Math.sqrt(dlat * dlat + dlon * dlon); // in m } + + public static int[] destination(int lon1, int lat1, double distance, double angle) { + + double[] lonlat2m = CheapRuler.getLonLatToMeterScales(lat1); + double lon2m = lonlat2m[0]; + double lat2m = lonlat2m[1]; + angle = 90. - angle; + double st = Math.sin(angle * Math.PI / 180.); + double ct = Math.cos(angle * Math.PI / 180.); + + int lon2 = (int) (0.5 + lon1 + ct * distance / lon2m); + int lat2 = (int) (0.5 + lat1 + st * distance / lat2m); + int[] ret = new int[2]; + ret[0] = lon2; + ret[1] = lat2; + return ret; + } }