@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);
}
public static void main(String[] args) {
Scenario scenario;
scenario = ScenarioUtils.createScenario(ConfigUtils.createConfig());
new NetworkReaderMatsimV2(scenario.getNetwork()).readFile(args[0]);
Network network = scenario.getNetwork();
Network net = NetworkUtils.createNetwork();
Set<String> modes = new HashSet<>();
modes.add(TransportMode.car);
new TransportModeNetworkFilter(network).filter(net, modes);
new NetworkCleaner().run(net);
MatsimRandom.reset(123);
DrunkDudePathfinder stochasticRouter =
new DrunkDudePathfinder(
net,
new TravelDisutility() {
@Override
public double getLinkTravelDisutility(
Link link, double time, Person person, Vehicle vehicle) {
return link.getLength() / link.getFreespeed();
}
@Override
public double getLinkMinimumTravelDisutility(Link link) {
return link.getLength() / link.getFreespeed();
}
},
new TravelTime() {
@Override
public double getLinkTravelTime(
Link link, double time, Person person, Vehicle vehicle) {
return link.getLength() / link.getFreespeed();
}
},
0.5);
SimpleNetworkPainter networkPainter = new SimpleNetworkPainter(400, 400);
networkPainter.setNetworkTransformation(network);
stochasticRouter.setNetworkPainter(networkPainter);
// stochasticRouter.calcLeastCostPath(network.getNodes().get(Id.createNodeId("11_311")),
// network.getNodes().get(Id.createNodeId("18_566")), 3600, null, null);
Set<Node> nodeSet = new HashSet<>();
nodeSet.addAll(net.getNodes().values());
Node[] nodesArray = nodeSet.toArray(new Node[nodeSet.size()]);
long currentTimeMillis = -System.currentTimeMillis();
for (int i = 0; i < 1e3; i++) {
int[] ints = Sample.sampleMfromN(2, nodesArray.length);
System.out.println(nodesArray[ints[0]] + " " + nodesArray[ints[1]]);
stochasticRouter.calcLeastCostPath(
nodesArray[ints[0]], nodesArray[ints[1]], 3600, null, null);
}
currentTimeMillis += System.currentTimeMillis();
System.out.println(currentTimeMillis);
Dijkstra dijkstra =
new Dijkstra(
net,
new TravelDisutility() {
@Override
public double getLinkTravelDisutility(
Link link, double time, Person person, Vehicle vehicle) {
return link.getLength() / link.getFreespeed();
}
@Override
public double getLinkMinimumTravelDisutility(Link link) {
return link.getLength() / link.getFreespeed();
}
},
new TravelTime() {
@Override
public double getLinkTravelTime(
Link link, double time, Person person, Vehicle vehicle) {
return link.getLength() / link.getFreespeed();
}
},
null);
currentTimeMillis = -System.currentTimeMillis();
for (int i = 0; i < 1e3; i++) {
int[] ints = Sample.sampleMfromN(2, nodesArray.length);
dijkstra.calcLeastCostPath(nodesArray[ints[0]], nodesArray[ints[1]], 3600, null, null);
}
currentTimeMillis += System.currentTimeMillis();
System.out.println(currentTimeMillis);
}