diff --git a/brouter-core/src/main/java/btools/router/RoutingEngine.java b/brouter-core/src/main/java/btools/router/RoutingEngine.java index cb678aa..d90714c 100644 --- a/brouter-core/src/main/java/btools/router/RoutingEngine.java +++ b/brouter-core/src/main/java/btools/router/RoutingEngine.java @@ -50,7 +50,7 @@ public class RoutingEngine extends Thread { private int engineMode = 0; - private int MAX_STEPS_CHECK = 250; + private int MAX_STEPS_CHECK = 500; private int ROUNDTRIP_DEFAULT_DIRECTIONADD = 45; @@ -205,15 +205,15 @@ public class RoutingEngine extends Thread { onn.name = "to"; waypoints.add(onn); } else { - waypoints.get(waypoints.size()-1).name = "via" + (waypoints.size()-1) + "_center"; + waypoints.get(waypoints.size() - 1).name = "via" + (waypoints.size() - 1) + "_center"; List newpoints = new ArrayList<>(); - for (int i = waypoints.size()-2; i >= 0; i--) { + for (int i = waypoints.size() - 2; i >= 0; i--) { // System.out.println("back " + waypoints.get(i)); OsmNodeNamed onn = new OsmNodeNamed(new OsmNode(waypoints.get(i).ilon, waypoints.get(i).ilat)); onn.name = "via"; newpoints.add(onn); } - newpoints.get(newpoints.size()-1).name = "to"; + newpoints.get(newpoints.size() - 1).name = "to"; waypoints.addAll(newpoints); } } @@ -405,24 +405,24 @@ public class RoutingEngine extends Thread { } } int otherIdx = 0; - if (minIdx == t.nodes.size()-1) { - otherIdx = minIdx-1; + if (minIdx == t.nodes.size() - 1) { + otherIdx = minIdx - 1; } else { - otherIdx = minIdx+1; + otherIdx = minIdx + 1; } int otherdist = t.nodes.get(otherIdx).calcDistance(listOne.get(0).crosspoint); int minSElev = t.nodes.get(minIdx).getSElev(); int otherSElev = t.nodes.get(otherIdx).getSElev(); int diffSElev = 0; diffSElev = otherSElev - minSElev; - double diff = (double) mindist/(mindist + otherdist) * diffSElev; + double diff = (double) mindist / (mindist + otherdist) * diffSElev; OsmNodeNamed n = new OsmNodeNamed(listOne.get(0).crosspoint); n.name = wpt1.name; n.selev = minIdx != -1 ? (short) (minSElev + (int) diff) : Short.MIN_VALUE; if (engineMode == BROUTER_ENGINEMODE_GETINFO) { - n.nodeDescription = (start1 != null && start1.firstlink!=null ? start1.firstlink.descriptionBitmap : null); + n.nodeDescription = (start1 != null && start1.firstlink != null ? start1.firstlink.descriptionBitmap : null); t.pois.add(n); //t.message = "get_info"; //t.messageList.add(t.message); @@ -484,10 +484,10 @@ public class RoutingEngine extends Thread { wpt1.name = "roundtrip"; routingContext.useDynamicDistance = true; - double searchRadius = (routingContext.roundtripDistance == null ? 1500 :routingContext.roundtripDistance); - double direction = (routingContext.startDirection == null ? -1 :routingContext.startDirection); - if (direction == -1) direction = (int) (Math.random()*360); - double directionAdd = (routingContext.roundtripDirectionAdd == null ? ROUNDTRIP_DEFAULT_DIRECTIONADD :routingContext.roundtripDirectionAdd); + double searchRadius = (routingContext.roundtripDistance == null ? 1500 : routingContext.roundtripDistance); + double direction = (routingContext.startDirection == null ? -1 : routingContext.startDirection); + if (direction == -1) direction = (int) (Math.random() * 360); + double directionAdd = (routingContext.roundtripDirectionAdd == null ? ROUNDTRIP_DEFAULT_DIRECTIONADD : routingContext.roundtripDirectionAdd); if (routingContext.allowSamewayback) { int[] pos = CheapRuler.destination(waypoints.get(0).ilon, waypoints.get(0).ilat, searchRadius, direction); @@ -496,41 +496,11 @@ public class RoutingEngine extends Thread { wpt2.name = "rt1_" + direction; OsmNodeNamed onn = new OsmNodeNamed(new OsmNode(pos[0], pos[1])); - onn.name = "via1"; + onn.name = "rt1"; waypoints.add(onn); } else { - int[] pos = CheapRuler.destination(waypoints.get(0).ilon, waypoints.get(0).ilat, searchRadius, direction-directionAdd); - MatchedWaypoint wpt2 = new MatchedWaypoint(); - wpt2.waypoint = new OsmNode(pos[0], pos[1]); - wpt2.name = "via1_" + (int) (direction-directionAdd); - - OsmNodeNamed onn = new OsmNodeNamed(new OsmNode(pos[0], pos[1])); - onn.name = "via1"; - waypoints.add(onn); - - - pos = CheapRuler.destination(waypoints.get(0).ilon, waypoints.get(0).ilat, searchRadius, direction+directionAdd); - MatchedWaypoint wpt3 = new MatchedWaypoint(); - wpt3.waypoint = new OsmNode(pos[0], pos[1]); - wpt3.name = "via2_" + (int) (direction+directionAdd); - - onn = new OsmNodeNamed(new OsmNode(pos[0], pos[1])); - onn.name = "via2"; - waypoints.add(onn); - - pos = CheapRuler.destination(waypoints.get(0).ilon, waypoints.get(0).ilat, searchRadius/2, direction); - OsmNodeNamed n = new OsmNodeNamed(); - n.name = "nogo" + (int) (searchRadius/3); - n.ilon = pos[0]; - n.ilat = pos[1]; - n.isNogo = true; - n.radius = (int) (searchRadius/3); - n.nogoWeight = Double.NaN; - routingContext.setWaypoint(n, false); - - onn = new OsmNodeNamed(waypoints.get(0)); - onn.name = "to_rt"; - waypoints.add(onn); + //buildPointsFromAngle(waypoints, direction, directionAdd, searchRadius, true); + buildPointsFromCircle(waypoints, direction, searchRadius, 5); } routingContext.waypointCatchingRange = 1000; @@ -546,6 +516,50 @@ public class RoutingEngine extends Thread { } + void buildPointsFromAngle(List waypoints, double startAngle, double addAngle, double searchRadius, boolean withNogoCenter) { + int[] pos = CheapRuler.destination(waypoints.get(0).ilon, waypoints.get(0).ilat, searchRadius, startAngle - addAngle); + OsmNodeNamed onn = new OsmNodeNamed(new OsmNode(pos[0], pos[1])); + onn.name = "rt1"; + waypoints.add(onn); + + pos = CheapRuler.destination(waypoints.get(0).ilon, waypoints.get(0).ilat, searchRadius, startAngle + addAngle); + onn = new OsmNodeNamed(new OsmNode(pos[0], pos[1])); + onn.name = "rt2"; + waypoints.add(onn); + + onn = new OsmNodeNamed(waypoints.get(0)); + onn.name = "to_rt"; + waypoints.add(onn); + + if (withNogoCenter) { // add a nogo area + pos = CheapRuler.destination(waypoints.get(0).ilon, waypoints.get(0).ilat, searchRadius/2, startAngle); + OsmNodeNamed n = new OsmNodeNamed(); + n.name = "nogo" + (int) (searchRadius/3); + n.ilon = pos[0]; + n.ilat = pos[1]; + n.isNogo = true; + n.radius = (int) (searchRadius/3); + n.nogoWeight = Double.NaN; + routingContext.setWaypoint(n, false); + } + } + + void buildPointsFromCircle(List waypoints, double startAngle, double searchRadius, int points) { + //startAngle -= 90; + for (int i = 1; i < points; i++) { + double anAngle = 90 - (180.0 * i / points); + int[] pos = CheapRuler.destination(waypoints.get(0).ilon, waypoints.get(0).ilat, searchRadius, startAngle - anAngle); + OsmNodeNamed onn = new OsmNodeNamed(new OsmNode(pos[0], pos[1])); + onn.name = "rt" + i; + waypoints.add(onn); + } + + OsmNodeNamed onn = new OsmNodeNamed(waypoints.get(0)); + onn.name = "to_rt"; + waypoints.add(onn); + } + + private void postElevationCheck(OsmTrack track) { OsmPathElement lastPt = null; OsmPathElement startPt = null; @@ -738,21 +752,21 @@ public class RoutingEngine extends Thread { // add extra waypoints from the last broken round for (OsmNodeNamed wp : extraWaypoints) { if (wp.direct) hasDirectRouting = true; - if (wp.name.startsWith("from")) { + if (wp.name.startsWith("from")) { waypoints.add(1, wp); waypoints.get(0).direct = true; nUnmatched++; } else { - waypoints.add(waypoints.size()-1, wp); - waypoints.get(waypoints.size()-2).direct = true; + waypoints.add(waypoints.size() - 1, wp); + waypoints.get(waypoints.size() - 2).direct = true; nUnmatched++; } } extraWaypoints = null; } - if (lastTracks.length < waypoints.size()-1) { - refTracks = new OsmTrack[waypoints.size()-1]; // used ways for alternatives - lastTracks = new OsmTrack[waypoints.size()-1]; + if (lastTracks.length < waypoints.size() - 1) { + refTracks = new OsmTrack[waypoints.size() - 1]; // used ways for alternatives + lastTracks = new OsmTrack[waypoints.size() - 1]; hasDirectRouting = true; } for (OsmNodeNamed wp : waypoints) { @@ -787,13 +801,14 @@ public class RoutingEngine extends Thread { int startSize = matchedWaypoints.size(); matchWaypointsToNodes(matchedWaypoints); if (startSize < matchedWaypoints.size()) { - refTracks = new OsmTrack[matchedWaypoints.size()-1]; // used ways for alternatives - lastTracks = new OsmTrack[matchedWaypoints.size()-1]; + refTracks = new OsmTrack[matchedWaypoints.size() - 1]; // used ways for alternatives + lastTracks = new OsmTrack[matchedWaypoints.size() - 1]; hasDirectRouting = true; } for (MatchedWaypoint mwp : matchedWaypoints) { - if (hasInfo() && matchedWaypoints.size() != nUnmatched) logInfo("new wp=" + mwp.waypoint + " " + mwp.crosspoint + (mwp.direct ? " direct" : "")); + if (hasInfo() && matchedWaypoints.size() != nUnmatched) + logInfo("new wp=" + mwp.waypoint + " " + mwp.crosspoint + (mwp.direct ? " direct" : "")); } routingContext.checkMatchedWaypointAgainstNogos(matchedWaypoints); @@ -823,9 +838,9 @@ public class RoutingEngine extends Thread { matchedWaypoints.add(nearbyTrack.endPoint); } } else { - if (lastTracks.length < matchedWaypoints.size()-1) { - refTracks = new OsmTrack[matchedWaypoints.size()-1]; // used ways for alternatives - lastTracks = new OsmTrack[matchedWaypoints.size()-1]; + if (lastTracks.length < matchedWaypoints.size() - 1) { + refTracks = new OsmTrack[matchedWaypoints.size() - 1]; // used ways for alternatives + lastTracks = new OsmTrack[matchedWaypoints.size() - 1]; hasDirectRouting = true; } } @@ -884,7 +899,7 @@ public class RoutingEngine extends Thread { // check for way back on way point private boolean snappPathConnection(OsmTrack tt, OsmTrack t, MatchedWaypoint startWp) { - if (!startWp.name.startsWith("via")) + if (!startWp.name.startsWith("via") && !startWp.name.startsWith("rt")) return false; int ourSize = tt.nodes.size(); @@ -977,7 +992,7 @@ public class RoutingEngine extends Thread { indexfore++; if (routingContext.correctMisplacedViaPointsDistance > 0 && - wayDistance > routingContext.correctMisplacedViaPointsDistance) break; + wayDistance > routingContext.correctMisplacedViaPointsDistance) break; } @@ -1188,7 +1203,8 @@ public class RoutingEngine extends Thread { range = -MAX_DYNAMIC_RANGE; List tmp = new ArrayList<>(); for (MatchedWaypoint mwp : unmatchedWaypoints) { - if (mwp.crosspoint == null || mwp.radius >= routingContext.waypointCatchingRange) tmp.add(mwp); + if (mwp.crosspoint == null || mwp.radius >= routingContext.waypointCatchingRange) + tmp.add(mwp); } ok = nodesCache.matchWaypointsToNodes(tmp, range, islandNodePairs); } @@ -1199,56 +1215,53 @@ public class RoutingEngine extends Thread { } } // add beeline points when not already done - if (useDynamicDistance && !useNodePoints) { + if (useDynamicDistance && !useNodePoints && engineMode != BROUTER_ENGINEMODE_ROUNDTRIP) { List waypoints = new ArrayList<>(); for (int i = 0; i < unmatchedWaypoints.size(); i++) { MatchedWaypoint wp = unmatchedWaypoints.get(i); if (wp.waypoint.calcDistance(wp.crosspoint) > routingContext.waypointCatchingRange) { - if (engineMode != BROUTER_ENGINEMODE_ROUNDTRIP) { - MatchedWaypoint nmw = new MatchedWaypoint(); - if (i == 0) { - OsmNodeNamed onn = new OsmNodeNamed(wp.waypoint); - onn.name = "from"; - nmw.waypoint = onn; - nmw.name = onn.name; - nmw.crosspoint = new OsmNode(wp.waypoint.ilon, wp.waypoint.ilat); - nmw.direct = true; - onn = new OsmNodeNamed(wp.crosspoint); - onn.name = wp.name + "_add"; - wp.waypoint = onn; - waypoints.add(nmw); - wp.name = wp.name + "_add"; - waypoints.add(wp); - } else { - OsmNodeNamed onn = new OsmNodeNamed(wp.crosspoint); - onn.name = wp.name + "_add"; - nmw.waypoint = onn; - nmw.crosspoint = new OsmNode(wp.crosspoint.ilon, wp.crosspoint.ilat); - nmw.node1 = new OsmNode(wp.node1.ilon, wp.node1.ilat); - nmw.node2 = new OsmNode(wp.node2.ilon, wp.node2.ilat); - nmw.direct = true; - if (wp.name != null) nmw.name = wp.name; - waypoints.add(nmw); - wp.name = wp.name + "_add"; - waypoints.add(wp); - if (wp.name.startsWith("via")) { - wp.direct = true; - MatchedWaypoint emw = new MatchedWaypoint(); - OsmNodeNamed onn2 = new OsmNodeNamed(wp.crosspoint); - onn2.name = wp.name + "_2"; - emw.name = onn2.name; - emw.waypoint = onn2; - emw.crosspoint = new OsmNode(nmw.crosspoint.ilon, nmw.crosspoint.ilat); - emw.node1 = new OsmNode(nmw.node1.ilon, nmw.node1.ilat); - emw.node2 = new OsmNode(nmw.node2.ilon, nmw.node2.ilat); - emw.direct = false; - waypoints.add(emw); - } - wp.crosspoint = new OsmNode(wp.waypoint.ilon, wp.waypoint.ilat); - } - } else { + MatchedWaypoint nmw = new MatchedWaypoint(); + if (i == 0) { + OsmNodeNamed onn = new OsmNodeNamed(wp.waypoint); + onn.name = "from"; + nmw.waypoint = onn; + nmw.name = onn.name; + nmw.crosspoint = new OsmNode(wp.waypoint.ilon, wp.waypoint.ilat); + nmw.direct = true; + onn = new OsmNodeNamed(wp.crosspoint); + onn.name = wp.name + "_add"; + wp.waypoint = onn; + waypoints.add(nmw); + wp.name = wp.name + "_add"; waypoints.add(wp); + } else { + OsmNodeNamed onn = new OsmNodeNamed(wp.crosspoint); + onn.name = wp.name + "_add"; + nmw.waypoint = onn; + nmw.crosspoint = new OsmNode(wp.crosspoint.ilon, wp.crosspoint.ilat); + nmw.node1 = new OsmNode(wp.node1.ilon, wp.node1.ilat); + nmw.node2 = new OsmNode(wp.node2.ilon, wp.node2.ilat); + nmw.direct = true; + + if (wp.name != null) nmw.name = wp.name; + waypoints.add(nmw); + wp.name = wp.name + "_add"; + waypoints.add(wp); + if (wp.name.startsWith("via")) { + wp.direct = true; + MatchedWaypoint emw = new MatchedWaypoint(); + OsmNodeNamed onn2 = new OsmNodeNamed(wp.crosspoint); + onn2.name = wp.name + "_2"; + emw.name = onn2.name; + emw.waypoint = onn2; + emw.crosspoint = new OsmNode(nmw.crosspoint.ilon, nmw.crosspoint.ilat); + emw.node1 = new OsmNode(nmw.node1.ilon, nmw.node1.ilat); + emw.node2 = new OsmNode(nmw.node2.ilon, nmw.node2.ilat); + emw.direct = false; + waypoints.add(emw); + } + wp.crosspoint = new OsmNode(wp.waypoint.ilon, wp.waypoint.ilat); } } else { waypoints.add(wp); @@ -1257,6 +1270,16 @@ public class RoutingEngine extends Thread { unmatchedWaypoints.clear(); unmatchedWaypoints.addAll(waypoints); } + + // round trip cross point to way point + if (engineMode == BROUTER_ENGINEMODE_ROUNDTRIP) { + for (int i = 1; i < unmatchedWaypoints.size()-1; i++) { + MatchedWaypoint wp = unmatchedWaypoints.get(i); + wp.waypoint.ilon = wp.crosspoint.ilon; + wp.waypoint.ilat = wp.crosspoint.ilat; + } + } + } private OsmTrack searchTrack(MatchedWaypoint startWp, MatchedWaypoint endWp, OsmTrack nearbyTrack, OsmTrack refTrack) {