public void cleanNogolist(List<OsmNodeNamed> waypoints) {
if (nogopoints == null) return;
List<OsmNodeNamed> nogos = new ArrayList<OsmNodeNamed>();
for (OsmNodeNamed nogo : nogopoints) {
int radiusInMeter = (int) (nogo.radius * 111894.);
boolean goodGuy = true;
for (OsmNodeNamed wp : waypoints) {
if (wp.calcDistance(nogo) < radiusInMeter) {
goodGuy = false;
break;
}
}
if (goodGuy) nogos.add(nogo);
}
nogopoints = nogos.isEmpty() ? null : nogos;
}
public static void prepareNogoPoints(List<OsmNodeNamed> nogos) {
for (OsmNodeNamed nogo : nogos) {
String s = nogo.name;
int idx = s.indexOf(' ');
if (idx > 0) s = s.substring(0, idx);
int ir = 20; // default radius
if (s.length() > 4) {
try {
ir = Integer.parseInt(s.substring(4));
} catch (Exception e) {
/* ignore */
}
}
nogo.radius = ir / 111894.; // 6378000. / 57.;
}
}
public int calcDistance(int lon1, int lat1, int lon2, int lat2) {
double l = (lat2 - 90000000) * 0.00000001234134;
double l2 = l * l;
double l4 = l2 * l2;
coslat = 1. - l2 + l4 / 6.;
double coslat6 = coslat * 0.000001;
double dx = (lon2 - lon1) * coslat6;
double dy = (lat2 - lat1) * 0.000001;
double d = Math.sqrt(dy * dy + dx * dx);
shortestmatch = false;
if (nogopoints != null && !nogopoints.isEmpty() && d > 0.) {
for (int ngidx = 0; ngidx < nogopoints.size(); ngidx++) {
OsmNodeNamed nogo = nogopoints.get(ngidx);
double x1 = (lon1 - nogo.ilon) * coslat6;
double y1 = (lat1 - nogo.ilat) * 0.000001;
double x2 = (lon2 - nogo.ilon) * coslat6;
double y2 = (lat2 - nogo.ilat) * 0.000001;
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 < nogo.radius) // 20m
{
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 > nogo.radius) continue; // 20m ^ 2
}
if (nogo.isNogo) nogomatch = true;
else {
shortestmatch = true;
nogo.radius = radius; // shortest distance to way
// calculate remaining distance
if (s2 < 0.) {
wayfraction = -s2 / (d * d);
double xm = x2 - wayfraction * dx;
double ym = y2 - wayfraction * dy;
ilonshortest = (int) (xm / coslat6 + nogo.ilon);
ilatshortest = (int) (ym / 0.000001 + nogo.ilat);
} else if (s1 > s2) {
wayfraction = 0.;
ilonshortest = lon2;
ilatshortest = lat2;
} else {
wayfraction = 1.;
ilonshortest = lon1;
ilatshortest = lat1;
}
// here it gets nasty: there can be nogo-points in the list
// *after* the shortest distance point. In case of a shortest-match
// we use the reduced way segment for nogo-matching, in order not
// to cut our escape-way if we placed a nogo just in front of where we are
if (isEndpoint) {
wayfraction = 1. - wayfraction;
lon2 = ilonshortest;
lat2 = ilatshortest;
} else {
nogomatch = false;
lon1 = ilonshortest;
lat1 = ilatshortest;
}
dx = (lon2 - lon1) * coslat6;
dy = (lat2 - lat1) * 0.000001;
d = Math.sqrt(dy * dy + dx * dx);
}
}
}
}
double dd = d * 111894.7368; // 6378000. / 57.;
return (int) (dd + 1.0);
}
