private void relaxNode(final Node n, PriorityQueue<Node> pendingNodes) {
NodeData nData = nodeData.get(n.getId());
double currTime = nData.getTime();
double currCost = nData.getCost();
for (Link l : n.getOutLinks().values()) {
Node nn = l.getToNode();
NodeData nnData = nodeData.get(nn.getId());
if (nnData == null) {
nnData = new NodeData();
this.nodeData.put(nn.getId(), nnData);
}
double visitCost =
currCost + tcFunction.getLinkTravelDisutility(l, currTime, PERSON, VEHICLE);
double visitTime = currTime + ttFunction.getLinkTravelTime(l, currTime, PERSON, VEHICLE);
if (visitCost < nnData.getCost()) {
pendingNodes.remove(nn);
nnData.visit(n.getId(), visitCost, visitTime);
additionalComputationsHook(l, currTime);
pendingNodes.add(nn);
}
}
}
@Override
public Path calcLeastCostPath(
Node fromNode, Node toNode, double starttime, Person person, Vehicle vehicle) {
List<Node> nodes = new ArrayList<>();
List<Link> links = new ArrayList<>();
double travelTime = starttime;
double travelCost = 0;
Node currentNode = fromNode;
if (networkPainter != null) {
networkPainter.addForegroundPixel(fromNode.getId());
networkPainter.addForegroundPixel(toNode.getId());
}
while (currentNode != toNode) {
Object[] outLinks = currentNode.getOutLinks().values().toArray();
// come up witha set of costs based on how much nearer (eucl) the link brings you to your
// destination,
// use them as probabilities of selecting the link
double[] cumulativeWeights = new double[outLinks.length];
double[] weights = new double[outLinks.length];
double[] dists = new double[outLinks.length];
double minCost = Double.POSITIVE_INFINITY;
double maxCost = 0;
Link selection = null;
for (int i = 0; i < outLinks.length; i++) {
double linkCost =
CoordUtils.calcEuclideanDistance(
toNode.getCoord(), ((Link) outLinks[i]).getToNode().getCoord());
if (((Link) outLinks[i])
.getToNode()
.equals(toNode)) { // found destination, stop mucking about
selection = (Link) outLinks[i];
break;
}
dists[i] = linkCost;
minCost = Math.min(minCost, linkCost);
maxCost = Math.max(maxCost, linkCost);
}
for (int i = 0; i < outLinks.length; i++) {
double linkCost = dists[i] - minCost;
if (links.contains(outLinks[i]) || nodes.contains(((Link) outLinks[i]).getToNode())) {
linkCost = 1e6 * (maxCost - minCost);
}
linkCost = Math.exp(-beta * linkCost);
linkCost = linkCost == 1 ? 2 : linkCost;
cumulativeWeights[i] = i > 0 ? cumulativeWeights[i - 1] + linkCost : linkCost;
weights[i] = linkCost;
}
if (selection == null) {
// sample if the destination has not been found
double sampleProb =
MatsimRandom.getRandom().nextDouble() * cumulativeWeights[cumulativeWeights.length - 1];
for (int i = 0; i < outLinks.length; i++) {
if (cumulativeWeights[i] >= sampleProb) {
selection = (Link) outLinks[i];
links.add(selection);
nodes.add(currentNode);
travelCost +=
travelCosts.getLinkTravelDisutility(selection, travelTime, person, vehicle);
travelTime += travelTimes.getLinkTravelTime(selection, travelTime, person, vehicle);
break;
}
}
}
if (networkPainter != null) {
networkPainter.addForegroundLine(selection.getId());
try {
Thread.sleep(200);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
currentNode = selection.getToNode();
}
if (networkPainter != null) {
try {
Thread.sleep(500);
networkPainter.clearForeground();
} catch (InterruptedException e) {
e.printStackTrace();
}
}
return new Path(nodes, links, travelTime, travelCost);
}