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

added nogo for heading calc

Browse source
This commit is contained in:
afischerdev 2024-01-19 10:16:42 +01:00
parent bf07e2e6d2
commit ae7411d4a0
2 changed files with 35 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -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);

17
brouter-util/src/main/java/btools/util/CheapRuler.java
View file

@ -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;
}
}