Skip to content

Commit

Permalink
Merge pull request #3726 from moia-oss/turnRestrictionsLeastCostPathTree
Browse files Browse the repository at this point in the history 
Use path iterators for constructing paths outside of least cost path tree
  • Loading branch information
@nkuehnel
nkuehnel authored Feb 13, 2025
2 parents 64838f1 + 98ec498 commit c0a6c24
Show file tree
Hide file tree
Showing 2 changed files with 83 additions and 51 deletions.
60 changes: 11 additions & 49 deletions contribs/dvrp/src/main/java/org/matsim/contrib/dvrp/path/OneToManyPathCalculator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,8 +67,6 @@ class OneToManyPathCalculator {
this.forwardSearch = forwardSearch;
this.fromLink = fromLink;
this.startTime = startTime;

verifyParallelLinks();
}

void calculateDijkstraTree(Collection<Link> toLinks) {
Expand Down Expand Up @@ -127,7 +125,7 @@ Path createPath(Node toNode) {
return null;
}
var nodes = constructNodeSequence(dijkstraTree, toNode, forwardSearch);
var links = constructLinkSequence(nodes);
var links = constructLinkSequence(dijkstraTree, toNode, forwardSearch);
double cost = dijkstraTree.getCost(toNodeIndex);
return new Path(nodes, links, travelTime, cost);
}
Expand All @@ -145,32 +143,23 @@ private List<Node> constructNodeSequence(LeastCostPathTree dijkstraTree, Node to
ArrayList<Node> nodes = new ArrayList<>();
nodes.add(toNode);

LeastCostPathTree.PathIterator pathIterator = dijkstraTree.getComingFromIterator(toNode);
while (pathIterator.hasNext()) {
nodes.add(pathIterator.next());
}
LeastCostPathTree.PathIterator pathIterator = dijkstraTree.getNodePathIterator(toNode);
pathIterator.forEachRemaining(nodes::add);

if (forward) {
Collections.reverse(nodes);
}
return nodes;
}

private List<Link> constructLinkSequence(List<Node> nodes) {
List<Link> links = new ArrayList<>(nodes.size() - 1);
Node prevNode = nodes.get(0);
for (int i = 1; i < nodes.size(); i++) {
Node nextNode = nodes.get(i);
for (Link link : prevNode.getOutLinks().values()) {
//FIXME this method will not work properly if there are many prevNode -> nextNode links
//TODO save link idx in tree OR pre-check: at most 1 arc per each node pair OR choose faster/better link
// sh, 26/07/2023, added a check further below to increase awareness
if (link.getToNode() == nextNode) {
links.add(link);
break;
}
}
prevNode = nextNode;
private List<Link> constructLinkSequence(LeastCostPathTree dijkstraTree, Node toNode, boolean forward) {
ArrayList<Link> links = new ArrayList<>();

LeastCostPathTree.LinkPathIterator pathIterator = dijkstraTree.getLinkPathIterator(toNode);
pathIterator.forEachRemaining(links::add);

if (forward) {
Collections.reverse(links);
}
return links;
}
Expand All @@ -189,31 +178,4 @@ private double getFirstAndLastLinkTT(Link fromLink, Link toLink, double pathTrav
VrpPaths.getLastLinkTT(travelTime, fromLink, time);
return FIRST_LINK_TT + lastLinkTT;
}

private final static Logger logger = LogManager.getLogger(OneToManyPathCalculator.class);
private static int parallelLinksWarningCount = 0;

private void verifyParallelLinks() {
if (parallelLinksWarningCount < 20) {
for (Node prevNode : nodeMap.values()) {
Set<Integer> candidates = new HashSet<>();

for (Link link : prevNode.getOutLinks().values()) {
if (!candidates.add(link.getToNode().getId().index())) {
logger.warn(
"Found parallel links between nodes {} and {}. This may lead to problems in path calculation.",
prevNode.getId().toString(), link.getToNode().getId().toString());

if (parallelLinksWarningCount > 20) {
logger.warn("Consider using NetworkSegmentDoubleLinks.run on your network");
logger.warn("Only showing 20 of these warnings ...");
return;
}

parallelLinksWarningCount++;
}
}
}
}
}
}
74 changes: 72 additions & 2 deletions matsim/src/main/java/org/matsim/core/router/speedy/LeastCostPathTree.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
* implementation NOT thread-safe.
*
* @author mrieser / Simunto, sponsored by SBB Swiss Federal Railways
* @author hrewald, nkuehnel / MOIA turn restriction adjustments
*/
public class LeastCostPathTree {

Expand All @@ -31,6 +32,8 @@ public class LeastCostPathTree {
private final TravelDisutility td;
private final double[] data; // 3 entries per node: time, cost, distance
private final int[] comingFrom;
private final int[] fromLink;
private final int[] comingFromLink;
private final SpeedyGraph.LinkIterator outLI;
private final SpeedyGraph.LinkIterator inLI;
private final NodeMinHeap pq;
Expand All @@ -41,6 +44,8 @@ public LeastCostPathTree(SpeedyGraph graph, TravelTime tt, TravelDisutility td)
this.td = td;
this.data = new double[graph.nodeCount * 3];
this.comingFrom = new int[graph.nodeCount];
this.fromLink = new int[graph.nodeCount];
this.comingFromLink = new int[graph.linkCount];
this.pq = new NodeMinHeap(graph.nodeCount, this::getCost, this::setCost);
this.outLI = graph.getOutLinkIterator();
this.inLI = graph.getInLinkIterator();
Expand All @@ -53,6 +58,7 @@ public void calculate(int startNode, double startTime, Person person, Vehicle ve
public void calculate(int startNode, double startTime, Person person, Vehicle vehicle, StopCriterion stopCriterion) {
Arrays.fill(this.data, Double.POSITIVE_INFINITY);
Arrays.fill(this.comingFrom, -1);
Arrays.fill(this.fromLink, -1);

setData(startNode, 0, startTime, 0);

Expand Down Expand Up @@ -86,18 +92,32 @@ public void calculate(int startNode, double startTime, Person person, Vehicle ve
this.pq.decreaseKey(toNode, newCost);
setData(toNode, newCost, newTime, currDistance + link.getLength());
this.comingFrom[toNode] = nodeIdx;
this.fromLink[toNode] = linkIdx;
}
} else {
setData(toNode, newCost, newTime, currDistance + link.getLength());
this.pq.insert(toNode);
this.comingFrom[toNode] = nodeIdx;
this.fromLink[toNode] = linkIdx;
}
}
}

if (graph.hasTurnRestrictions()) {
consolidateColoredNodes();
}

Arrays.fill(this.comingFromLink, -1);
for (int i = 0; i < graph.nodeCount; i++) {
Node node = graph.getNode(i);
if(node != null) {
this.outLI.reset(i);
while (this.outLI.next()) {
int previousLinkIdx = fromLink[i];
this.comingFromLink[outLI.getLinkIndex()] = previousLinkIdx;
}
}
}
}

public void calculateBackwards(int arrivalNode, double arrivalTime, Person person, Vehicle vehicle) {
Expand All @@ -107,6 +127,7 @@ public void calculateBackwards(int arrivalNode, double arrivalTime, Person perso
public void calculateBackwards(int arrivalNode, double arrivalTime, Person person, Vehicle vehicle, StopCriterion stopCriterion) {
Arrays.fill(this.data, Double.POSITIVE_INFINITY);
Arrays.fill(this.comingFrom, -1);
Arrays.fill(this.fromLink, -1);

setData(arrivalNode, 0, arrivalTime, 0);

Expand Down Expand Up @@ -140,18 +161,32 @@ public void calculateBackwards(int arrivalNode, double arrivalTime, Person perso
this.pq.decreaseKey(fromNode, newCost);
setData(fromNode, newCost, newTime, currDistance + link.getLength());
this.comingFrom[fromNode] = nodeIdx;
this.fromLink[fromNode] = linkIdx;
}
} else {
setData(fromNode, newCost, newTime, currDistance + link.getLength());
this.pq.insert(fromNode);
this.comingFrom[fromNode] = nodeIdx;
this.fromLink[fromNode] = linkIdx;
}
}
}

if (graph.hasTurnRestrictions()) {
consolidateColoredNodes();
}

Arrays.fill(this.comingFromLink, -1);
for (int i = 0; i < graph.nodeCount; i++) {
Node node = graph.getNode(i);
if(node != null) {
this.inLI.reset(i);
while (this.inLI.next()) {
int previousLinkIdx = fromLink[i];
this.comingFromLink[inLI.getLinkIndex()] = previousLinkIdx;
}
}
}
}

private void consolidateColoredNodes() {
Expand All @@ -169,6 +204,7 @@ private void consolidateColoredNodes() {
if (coloredCost < uncoloredCost) {
setData(uncoloredIndex, coloredCost, getTimeRaw(i), getDistance(i));
this.comingFrom[uncoloredIndex] = this.comingFrom[i];
this.fromLink[uncoloredIndex] = this.fromLink[i];
}
}
}
Expand Down Expand Up @@ -206,10 +242,14 @@ private void setData(int nodeIndex, double cost, double time, double distance) {
this.data[index + 2] = distance;
}

public PathIterator getComingFromIterator(Node node) {
public PathIterator getNodePathIterator(Node node) {
return new PathIterator(node);
}

public LinkPathIterator getLinkPathIterator(Node node) {
return new LinkPathIterator(node);
}

public interface StopCriterion {

boolean stop(int nodeIndex, double arrivalTime, double travelCost, double distance, double departureTime);
Expand Down Expand Up @@ -253,7 +293,7 @@ public PathIterator(Node startNode) {
}

@Override
public Node next() throws NoSuchElementException {
public Node next() {
current = comingFrom[current];
if (current < 0) {
throw new NoSuchElementException();
Expand All @@ -266,4 +306,34 @@ public boolean hasNext() {
return comingFrom[current] >= 0;
}
}

// by not exposing internal indices to the outside we ensure that only uncolored nodes are returned. nkuehnel Feb'25
public final class LinkPathIterator implements Iterator<Link> {

private boolean firstStep = true;

private int current;

public LinkPathIterator(Node startNode) {
current = fromLink[startNode.getId().index()];
}

@Override
public Link next() {
if(firstStep) {
firstStep = false;
return graph.getLink(current);
}
current = comingFromLink[current];
if (current < 0) {
throw new NoSuchElementException();
}
return graph.getLink(current);
}

@Override
public boolean hasNext() {
return current >= 0 && (comingFromLink[current] >= 0 || firstStep);
}
}
}

0 comments on commit c0a6c24

Please sign in to comment.