236 lines
7.7 KiB
Java
236 lines
7.7 KiB
Java
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
|
|
* already check for allowed access according to the current routing profile
|
|
* <p>
|
|
* It matches these geometries against the list of waypoints to find the best
|
|
* match for each waypoint
|
|
*/
|
|
public final class WaypointMatcherImpl implements WaypointMatcher {
|
|
private static final int MAX_POINTS = 5;
|
|
|
|
private List<MatchedWaypoint> waypoints;
|
|
private OsmNodePairSet islandPairs;
|
|
|
|
private int lonStart;
|
|
private int latStart;
|
|
private int lonTarget;
|
|
private int latTarget;
|
|
private boolean anyUpdate;
|
|
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) {
|
|
// todo: bounding-box pre-filter
|
|
|
|
double[] lonlat2m = CheapRuler.getLonLatToMeterScales((lat1 + lat2) >> 1);
|
|
double dlon2m = lonlat2m[0];
|
|
double dlat2m = lonlat2m[1];
|
|
|
|
double dx = (lon2 - lon1) * dlon2m;
|
|
double dy = (lat2 - lat1) * dlat2m;
|
|
double d = Math.sqrt(dy * dy + dx * dx);
|
|
|
|
if (d == 0.)
|
|
return;
|
|
|
|
//for ( MatchedWaypoint mwp : waypoints )
|
|
for (int i = 0; i < waypoints.size(); i++) {
|
|
MatchedWaypoint mwp = waypoints.get(i);
|
|
|
|
if (mwp.direct &&
|
|
(i == 0 ||
|
|
waypoints.get(i - 1).direct)
|
|
) {
|
|
if (mwp.crosspoint == null) {
|
|
mwp.crosspoint = new OsmNode();
|
|
mwp.crosspoint.ilon = mwp.waypoint.ilon;
|
|
mwp.crosspoint.ilat = mwp.waypoint.ilat;
|
|
mwp.hasUpdate = true;
|
|
anyUpdate = true;
|
|
}
|
|
continue;
|
|
}
|
|
|
|
OsmNode wp = mwp.waypoint;
|
|
double x1 = (lon1 - wp.ilon) * dlon2m;
|
|
double y1 = (lat1 - wp.ilat) * dlat2m;
|
|
double x2 = (lon2 - wp.ilon) * dlon2m;
|
|
double y2 = (lat2 - wp.ilat) * dlat2m;
|
|
double r12 = x1 * x1 + y1 * y1;
|
|
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) {
|
|
double s1 = x1 * dx + y1 * dy;
|
|
double s2 = x2 * dx + y2 * dy;
|
|
|
|
if (s1 < 0.) {
|
|
s1 = -s1;
|
|
s2 = -s2;
|
|
}
|
|
if (s2 > 0.) {
|
|
radius = Math.sqrt(s1 < s2 ? r12 : r22);
|
|
if (radius > mwp.radius)
|
|
continue;
|
|
}
|
|
// new match for that waypoint
|
|
mwp.radius = radius; // shortest distance to way
|
|
mwp.hasUpdate = true;
|
|
anyUpdate = true;
|
|
// calculate crosspoint
|
|
if (mwp.crosspoint == null)
|
|
mwp.crosspoint = new OsmNode();
|
|
if (s2 < 0.) {
|
|
double wayfraction = -s2 / (d * d);
|
|
double xm = x2 - wayfraction * dx;
|
|
double ym = y2 - wayfraction * dy;
|
|
mwp.crosspoint.ilon = (int) (xm / dlon2m + wp.ilon);
|
|
mwp.crosspoint.ilat = (int) (ym / dlat2m + wp.ilat);
|
|
} else if (s1 > s2) {
|
|
mwp.crosspoint.ilon = lon2;
|
|
mwp.crosspoint.ilat = lat2;
|
|
} else {
|
|
mwp.crosspoint.ilon = lon1;
|
|
mwp.crosspoint.ilat = lat1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
@Override
|
|
public boolean start(int ilonStart, int ilatStart, int ilonTarget, int ilatTarget) {
|
|
if (islandPairs.size() > 0) {
|
|
long n1 = ((long) ilonStart) << 32 | ilatStart;
|
|
long n2 = ((long) ilonTarget) << 32 | ilatTarget;
|
|
if (islandPairs.hasPair(n1, n2)) {
|
|
return false;
|
|
}
|
|
}
|
|
lonLast = lonStart = ilonStart;
|
|
latLast = latStart = ilatStart;
|
|
lonTarget = ilonTarget;
|
|
latTarget = ilatTarget;
|
|
anyUpdate = false;
|
|
return true;
|
|
}
|
|
|
|
@Override
|
|
public void transferNode(int ilon, int ilat) {
|
|
checkSegment(lonLast, latLast, ilon, ilat);
|
|
lonLast = ilon;
|
|
latLast = ilat;
|
|
}
|
|
|
|
@Override
|
|
public void end() {
|
|
checkSegment(lonLast, latLast, lonTarget, latTarget);
|
|
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;
|
|
|
|
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);
|
|
|
|
}
|
|
|
|
|
|
}
|