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

add routines for misplaced pts

Browse source
This commit is contained in:
afischerdev 2023-01-09 18:25:16 +01:00
parent d2297c0c52
commit b98a576fe3
2 changed files with 216 additions and 11 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

19
brouter-core/src/main/java/btools/router/OsmTrack.java
View file

@ -48,7 +48,7 @@ public final class OsmTrack {
public List<OsmNodeNamed> pois = new ArrayList<OsmNodeNamed>();
private static class OsmPathElementHolder {
public static class OsmPathElementHolder {
public OsmPathElement node;
public OsmPathElementHolder nextHolder;
}
@ -928,6 +928,12 @@ public final class OsmTrack {
return true;
}
public OsmPathElementHolder getFromDetourMap(long id) {
if (detourMap == null)
return null;
return detourMap.get(id);
}
public void prepareSpeedProfile(RoutingContext rc) {
// sendSpeedProfile = rc.keyValues != null && rc.keyValues.containsKey( "vmax" );
}
@ -987,6 +993,17 @@ public final class OsmTrack {
return nodes.get(nodes.size() - 1).getTime();
}
public void removeVoiceHint(int i) {
if (voiceHints != null) {
VoiceHint remove = null;
for (VoiceHint vh : voiceHints.list) {
if (vh.indexInTrack == i)
remove = vh;
}
if (remove != null)
voiceHints.list.remove(remove);
}
}
private MessageData startSection(OsmPathElement element, OsmPathElement root) {
OsmPathElement e = element;

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

@ -16,6 +16,7 @@ import btools.mapaccess.OsmLink;
import btools.mapaccess.OsmLinkHolder;
import btools.mapaccess.OsmNode;
import btools.mapaccess.OsmNodePairSet;
import btools.util.CompactLongMap;
import btools.util.SortedHeap;
import btools.util.StackSampler;
@ -32,6 +33,8 @@ public class RoutingEngine extends Thread {
private int MAXNODES_ISLAND_CHECK = 500;
private OsmNodePairSet islandNodePairs = new OsmNodePairSet(MAXNODES_ISLAND_CHECK);
private int MAX_STEPS_CHECK = 10;
protected OsmTrack foundTrack = new OsmTrack();
private OsmTrack foundRawTrack = null;
private int alternativeIndex = 0;
@ -358,15 +361,26 @@ public class RoutingEngine extends Thread {
}
OsmTrack seg;
int wptIndex;
if (routingContext.inverseRouting) {
routingContext.inverseDirection = true;
seg = searchTrack(matchedWaypoints.get(i + 1), matchedWaypoints.get(i), null, refTracks[i]);
routingContext.inverseDirection = false;
wptIndex = i + 1;
} else {
seg = searchTrack(matchedWaypoints.get(i), matchedWaypoints.get(i + 1), i == matchedWaypoints.size() - 2 ? nearbyTrack : null, refTracks[i]);
wptIndex = i;
}
if (seg == null)
return null;
boolean changed = false;
if (routingContext.correctMisplacedViaPoints && !matchedWaypoints.get(i).direct) {
changed = snappPathConnection(totaltrack, seg, routingContext.inverseRouting ? matchedWaypoints.get(i + 1) : matchedWaypoints.get(i));
}
// if (wptIndex > 0)
// matchedWaypoints.get(wptIndex).indexInTrack = totaltrack.nodes.size() - 1;
if (seg == null) return null;
totaltrack.appendTrack(seg);
lastTracks[i] = seg;
}
@ -375,6 +389,172 @@ public class RoutingEngine extends Thread {
return totaltrack;
}
// check for way back on way point
private boolean snappPathConnection(OsmTrack tt, OsmTrack t, MatchedWaypoint startWp) {
if (!startWp.name.startsWith("via"))
return false;
int ourSize = tt.nodes.size();
if (ourSize > 0) {
OsmPathElement testPoint = tt.nodes.get(ourSize - 1);
if (routingContext.poipoints != null) {
for (OsmNodeNamed node : routingContext.poipoints) {
int lon0 = tt.nodes.get(ourSize - 2).getILon();
int lat0 = tt.nodes.get(ourSize - 2).getILat();
int lon1 = startWp.crosspoint.ilon;
int lat1 = startWp.crosspoint.ilat;
int lon2 = node.ilon;
int lat2 = node.ilat;
double angle3 = routingContext.anglemeter.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2);
int dist = node.calcDistance(startWp.crosspoint);
if (dist < routingContext.waypointCatchingRange)
return false;
}
}
ArrayList<OsmPathElement> removeBackList = new ArrayList<>();
ArrayList<OsmPathElement> removeForeList = new ArrayList<>();
ArrayList<Integer> removeVoiceHintList = new ArrayList<>();
int lon0,
lat0,
lon1,
lat1,
lon2,
lat2;
OsmPathElement last = null;
OsmPathElement lastJunction = null;
CompactLongMap<OsmTrack.OsmPathElementHolder> lastJunctions = new CompactLongMap<>();
OsmPathElement newJunction = null;
OsmPathElement newTarget = null;
OsmPathElement tmpback = null;
OsmPathElement tmpfore = null;
int indexback = ourSize - 1;
int indexfore = 0;
int stop = (indexback - MAX_STEPS_CHECK > 1 ? indexback - MAX_STEPS_CHECK : 1);
double wayDistance = 0;
double nextDist = 0;
while (indexback >= 1 && indexback >= stop && indexfore < t.nodes.size()) {
int junctions = 0;
tmpback = tt.nodes.get(indexback);
tmpfore = t.nodes.get(indexfore);
int dist = tmpback.calcDistance(tmpfore);
if (1 == 1) {
OsmTrack.OsmPathElementHolder detours = tt.getFromDetourMap(tmpback.getIdFromPos());
OsmTrack.OsmPathElementHolder h = detours;
while (h != null) {
junctions++;
lastJunctions.put(h.node.getIdFromPos(), h);
h = h.nextHolder;
}
}
if (dist == 1 && indexfore > 0) {
if (indexfore == 1) {
removeBackList.add(tt.nodes.get(tt.nodes.size() - 1)); // last and first should be equal, so drop only on second also equal
removeForeList.add(t.nodes.get(0));
removeBackList.add(tmpback);
removeForeList.add(tmpfore);
removeVoiceHintList.add(tt.nodes.size() - 1);
removeVoiceHintList.add(indexback);
} else {
removeBackList.add(tmpback);
removeForeList.add(tmpfore);
removeVoiceHintList.add(indexback);
}
nextDist = t.nodes.get(indexfore - 1).calcDistance(tmpfore);
wayDistance += nextDist;
}
if (dist > 1 || indexback == 1) {
if (removeBackList.size() != 0) {
// recover last - should be the cross point
removeBackList.remove(removeBackList.get(removeBackList.size() - 1));
removeForeList.remove(removeForeList.get(removeForeList.size() - 1));
break;
} else {
return false;
}
}
indexback--;
indexfore++;
}
if (routingContext.correctMisplacedViaPointsDistance > 0 &&
wayDistance > routingContext.correctMisplacedViaPointsDistance) {
removeVoiceHintList.clear();
removeBackList.clear();
removeForeList.clear();
return false;
}
// time hold
float atime = 0;
float aenergy = 0;
if (removeForeList.size() > 1) {
atime = t.nodes.get(removeForeList.size() - 2).getTime();
aenergy = t.nodes.get(removeForeList.size() - 2).getEnergy();
}
for (OsmPathElement e : removeBackList) {
tt.nodes.remove(e);
}
for (OsmPathElement e : removeForeList) {
t.nodes.remove(e);
}
for (Integer e : removeVoiceHintList) {
tt.removeVoiceHint(e);
}
removeVoiceHintList.clear();
removeBackList.clear();
removeForeList.clear();
if (atime > 0f) {
for (OsmPathElement e : t.nodes) {
e.setTime(e.getTime() - atime);
e.setEnergy(e.getEnergy() - aenergy);
}
}
if (t.nodes.size() < 2)
return true;
if (tt.nodes.size() < 1)
return true;
if (tt.nodes.size() == 1) {
last = tt.nodes.get(0);
} else {
last = tt.nodes.get(tt.nodes.size() - 2);
}
newJunction = t.nodes.get(0);
newTarget = t.nodes.get(1);
setNewVoiceHint(t, last, lastJunctions, newJunction, newTarget);
return true;
}
return false;
}
private void setNewVoiceHint(OsmTrack t, OsmPathElement last, CompactLongMap<OsmTrack.OsmPathElementHolder> lastJunctiona, OsmPathElement newJunction, OsmPathElement newTarget) {
if (last == null || newJunction == null || newTarget == null)
return;
int lon0,
lat0,
lon1,
lat1,
lon2,
lat2;
lon0 = last.getILon();
lat0 = last.getILat();
lon1 = newJunction.getILon();
lat1 = newJunction.getILat();
lon2 = newTarget.getILon();
lat2 = newTarget.getILat();
// get a new angle
double angle = routingContext.anglemeter.calcAngle(lon0, lat0, lon1, lat1, lon2, lat2);
newTarget.message.turnangle = (float) angle;
}
// geometric position matching finding the nearest routable way-section
private void matchWaypointsToNodes(List<MatchedWaypoint> unmatchedWaypoints) {
resetCache(false);
@ -386,7 +566,8 @@ public class RoutingEngine extends Thread {
try {
boolean calcBeeline = startWp.direct;
if (!calcBeeline) return searchRoutedTrack(startWp, endWp, nearbyTrack, refTrack);
if (!calcBeeline)
return searchRoutedTrack(startWp, endWp, nearbyTrack, refTrack);
// we want a beeline-segment
OsmPath path = routingContext.createPath(new OsmLink(null, startWp.crosspoint));
@ -399,7 +580,10 @@ public class RoutingEngine extends Thread {
private OsmTrack searchRoutedTrack(MatchedWaypoint startWp, MatchedWaypoint endWp, OsmTrack nearbyTrack, OsmTrack refTrack) {
OsmTrack track = null;
double[] airDistanceCostFactors = new double[]{routingContext.pass1coefficient, routingContext.pass2coefficient};
double[] airDistanceCostFactors = new double[]{
routingContext.pass1coefficient,
routingContext.pass2coefficient
};
boolean isDirty = false;
IllegalArgumentException dirtyMessage = null;
@ -526,10 +710,14 @@ public class RoutingEngine extends Thread {
double minradius = 1e10;
for (OsmLink link = n1.firstlink; link != null; link = link.getNext(n1)) {
OsmNode nextNode = link.getTarget(n1);
if (nextNode.isHollow()) continue; // border node?
if (nextNode.firstlink == null) continue; // don't care about dead ends
if (nextNode == n1) continue; // ?
if (nextNode != n2) continue; // just that link
if (nextNode.isHollow())
continue; // border node?
if (nextNode.firstlink == null)
continue; // don't care about dead ends
if (nextNode == n1)
continue; // ?
if (nextNode != n2)
continue; // just that link
wp.radius = 1.5;
OsmPath testPath = routingContext.createPath(startPath, link, null, guideTrack != null);
@ -686,7 +874,6 @@ public class RoutingEngine extends Thread {
if (directWeaving && nodesCache.hasHollowLinkTargets(path.getTargetNode())) {
if (!memoryPanicMode) {
if (!nodesCache.nodesMap.isInMemoryBounds(openSet.getSize(), false)) {
// System.out.println( "collecting..." );
int nodesBefore = nodesCache.nodesMap.nodesCreated;
int pathsBefore = openSet.getSize();
@ -901,7 +1088,8 @@ public class RoutingEngine extends Thread {
routingContext.setWaypoint(endPos, true);
}
OsmPath testPath = routingContext.createPath(otherPath, link, refTrack, guideTrack != null);
if (testPath.cost >= 0 && (bestPath == null || testPath.cost < bestPath.cost)) {
if (testPath.cost >= 0 && (bestPath == null || testPath.cost < bestPath.cost) &&
(testPath.sourceNode.getIdFromPos() != testPath.targetNode.getIdFromPos())) {
bestPath = testPath;
}
} finally {
@ -962,7 +1150,7 @@ public class RoutingEngine extends Thread {
OsmPathElement element = OsmPathElement.create(path, false);
// for final track, cut endnode
if (guideTrack != null) {
if (guideTrack != null && element.origin != null) {
element = element.origin;
}