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

add direction on matching wpts

Browse source
This commit is contained in:
afischerdev 2023-02-26 12:36:56 +01:00
parent b735cd3e4e
commit 16a5ebe737
2 changed files with 117 additions and 6 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

97
brouter-mapaccess/src/main/java/btools/mapaccess/WaypointMatcherImpl.java
View file

@ -1,9 +1,12 @@
package btools.mapaccess;
import java.util.List;
import java.util.Collections;
import java.util.Comparator;
import btools.codec.WaypointMatcher;
import btools.util.CheapRuler;
import btools.util.CheapAngleMeter;
/**
* the WaypointMatcher is feeded by the decoder with geoemtries of ways that are
@ -13,6 +16,8 @@ import btools.util.CheapRuler;
* match for each waypoint
*/
public final class WaypointMatcherImpl implements WaypointMatcher {
private static final int MAX_POINTS = 5;
private List<MatchedWaypoint> waypoints;
private OsmNodePairSet islandPairs;
@ -24,12 +29,37 @@ public final class WaypointMatcherImpl implements WaypointMatcher {
private int lonLast;
private int latLast;
private Comparator<MatchedWaypoint> comparator;
public WaypointMatcherImpl(List<MatchedWaypoint> waypoints, double maxDistance, OsmNodePairSet islandPairs) {
this.waypoints = waypoints;
this.islandPairs = islandPairs;
MatchedWaypoint last = null;
for (MatchedWaypoint mwp : waypoints) {
mwp.radius = maxDistance;
if (last != null && mwp.directionToNext == -1) {
last.directionToNext = CheapAngleMeter.getDirection(last.waypoint.ilon, last.waypoint.ilat, mwp.waypoint.ilon, mwp.waypoint.ilat);
}
last = mwp;
}
// last point has no angle so we are looking back
int lastidx = waypoints.size() - 2;
if (lastidx < 0) {
last.directionToNext = -1;
} else {
last.directionToNext = CheapAngleMeter.getDirection(last.waypoint.ilon, last.waypoint.ilat, waypoints.get(lastidx).waypoint.ilon, waypoints.get(lastidx).waypoint.ilat);
}
// sort result list
comparator = new Comparator<MatchedWaypoint>() {
@Override
public int compare(MatchedWaypoint mw1, MatchedWaypoint mw2) {
int cmpDist = Double.compare(mw1.radius, mw2.radius);
if (cmpDist != 0) return cmpDist;
return Double.compare(mw1.directionDiff, mw2.directionDiff);
}
};
}
private void checkSegment(int lon1, int lat1, int lon2, int lat2) {
@ -46,8 +76,8 @@ public final class WaypointMatcherImpl implements WaypointMatcher {
if (d == 0.)
return;
int len = waypoints.size();
for (int i = 0; i < len; i++) {
//for ( MatchedWaypoint mwp : waypoints )
for (int i = 0; i < waypoints.size(); i++) {
MatchedWaypoint mwp = waypoints.get(i);
if (mwp.direct &&
@ -65,7 +95,6 @@ public final class WaypointMatcherImpl implements WaypointMatcher {
}
OsmNode wp = mwp.waypoint;
double x1 = (lon1 - wp.ilon) * dlon2m;
double y1 = (lat1 - wp.ilat) * dlat2m;
double x2 = (lon2 - wp.ilon) * dlon2m;
@ -74,7 +103,7 @@ public final class WaypointMatcherImpl implements WaypointMatcher {
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) {
if (radius <= mwp.radius) {
double s1 = x1 * dx + y1 * dy;
double s2 = x2 * dx + y2 * dy;
@ -141,11 +170,67 @@ public final class WaypointMatcherImpl implements WaypointMatcher {
if (anyUpdate) {
for (MatchedWaypoint mwp : waypoints) {
if (mwp.hasUpdate) {
double angle = CheapAngleMeter.getDirection(lonStart, latStart, lonTarget, latTarget);
double diff = CheapAngleMeter.getDifferenceFromDirection(mwp.directionToNext, angle);
mwp.hasUpdate = false;
mwp.node1 = new OsmNode(lonStart, latStart);
mwp.node2 = new OsmNode(lonTarget, latTarget);
MatchedWaypoint mw = new MatchedWaypoint();
mw.waypoint = new OsmNode();
mw.waypoint.ilon = mwp.waypoint.ilon;
mw.waypoint.ilat = mwp.waypoint.ilat;
mw.crosspoint = new OsmNode();
mw.crosspoint.ilon = mwp.crosspoint.ilon;
mw.crosspoint.ilat = mwp.crosspoint.ilat;
mw.node1 = new OsmNode(lonStart, latStart);
mw.node2 = new OsmNode(lonTarget, latTarget);
mw.name = mwp.name + "_w_" + mwp.crosspoint.hashCode();
mw.radius = mwp.radius;
mw.directionDiff = diff;
mw.directionToNext = mwp.directionToNext;
updateWayList(mwp.wayNearest, mw);
// revers
angle = CheapAngleMeter.getDirection(lonTarget, latTarget, lonStart, latStart);
diff = CheapAngleMeter.getDifferenceFromDirection(mwp.directionToNext, angle);
mw = new MatchedWaypoint();
mw.waypoint = new OsmNode();
mw.waypoint.ilon = mwp.waypoint.ilon;
mw.waypoint.ilat = mwp.waypoint.ilat;
mw.crosspoint = new OsmNode();
mw.crosspoint.ilon = mwp.crosspoint.ilon;
mw.crosspoint.ilat = mwp.crosspoint.ilat;
mw.node1 = new OsmNode(lonTarget, latTarget);
mw.node2 = new OsmNode(lonStart, latStart);
mw.name = mwp.name + "_w2_" + mwp.crosspoint.hashCode();
mw.radius = mwp.radius;
mw.directionDiff = diff;
mw.directionToNext = mwp.directionToNext;
updateWayList(mwp.wayNearest, mw);
MatchedWaypoint way = mwp.wayNearest.get(0);
mwp.crosspoint.ilon = way.crosspoint.ilon;
mwp.crosspoint.ilat = way.crosspoint.ilat;
mwp.node1 = new OsmNode(way.node1.ilon, way.node1.ilat);
mwp.node2 = new OsmNode(way.node2.ilon, way.node2.ilat);
mwp.directionDiff = way.directionDiff;
mwp.radius = way.radius;
}
}
}
}
// check limit of list size (avoid long runs)
void updateWayList(List<MatchedWaypoint> ways, MatchedWaypoint mw) {
ways.add(mw);
// use only shortest distances by smallest direction difference
Collections.sort(ways, comparator);
if (ways.size() > MAX_POINTS) ways.remove(MAX_POINTS);
}
}

26
brouter-util/src/main/java/btools/util/CheapAngleMeter.java
View file

@ -46,4 +46,30 @@ public final class CheapAngleMeter {
}
return offset + sinp * (57.4539 + s2 * (9.57565 + s2 * (4.30904 + s2 * 2.56491)));
}
public static double getAngle(int lon1, int lat1, int lon2, int lat2) {
double res = 0;
double xdiff = lat2 - lat1;
double ydiff = lon2 - lon1;
res = Math.toDegrees(Math.atan2(ydiff, xdiff));
return res;
}
public static double getDirection(int lon1, int lat1, int lon2, int lat2) {
double res = getAngle(lon1, lat1, lon2, lat2);
return normalize(res);
}
public static double normalize(double a) {
return a >= 360 ? a - (360 * (int) (a / 360))
: a < 0 ? a - (360 * ((int) (a / 360) - 1)) : a;
}
public static double getDifferenceFromDirection(double b1, double b2) {
double r = (b2 - b1) % 360.0;
if (r < -180.0) r += 360.0;
if (r >= 180.0) r -= 360.0;
return Math.abs(r);
}
}