public OsmTrack findRoute(OsmPos startPos, OsmPos endPos, String startTime, int alternativeIdx)
throws Exception {
OsmTrack track = null;
start = graph.matchNodeForPosition(startPos, rc.expctxWay);
if (start == null) throw new IllegalArgumentException("unmatched start: " + startPos);
end = graph.matchNodeForPosition(endPos, rc.expctxWay);
if (end == null) throw new IllegalArgumentException("unmatched end: " + endPos);
// SimpleDateFormat df = new SimpleDateFormat( "dd.MM.yyyy-HH:mm" );
// time0 = df.parse(startTime).getTime();
time0 = System.currentTimeMillis() + (long) (rc.starttimeoffset * 60000L);
long minutes0 = (time0 + 59999L) / 60000L;
time0 = minutes0 * 60000L;
OffsetSet finishedOffsets = OffsetSet.emptySet();
OsmLinkP startLink = new OsmLinkP(null, start);
ScheduledTrip startTrip = new ScheduledTrip(OffsetSet.fullSet(), startLink, null, null);
openSet.add(0, startTrip);
for (; ; ) {
if (re.isTerminated()) {
throw new RuntimeException("operation terminated");
}
if (linksProcessed + linksReProcessed > 5000000) {
throw new RuntimeException("5 Million links limit reached");
}
// get cheapest trip from heap
ScheduledTrip trip = openSet.popLowestKeyValue();
if (trip == null) {
break;
}
OsmLinkP currentLink = trip.link;
OsmNodeP currentNode = trip.getTargetNode();
if (currentNode == null) {
System.out.println("ups: " + trip);
continue;
}
if (currentLink.isVirgin()) {
linksProcessed++;
} else {
linksReProcessed++;
}
// check global closure
OffsetSet offsets = finishedOffsets.filter(trip.offsets);
if (offsets == null) continue;
// check local closure for links:
offsets =
currentLink.filterAndClose(offsets, trip.arrival, currentLink instanceof ScheduledLink);
if (offsets == null) continue;
// check for arrival
if (currentNode == end) {
for (int offset = 0; offset < trip.offsets.size(); offset++) {
if (trip.offsets.contains(offset)) {
track = compileTrip(trip, offset);
System.out.println("---- begin route ------ (cost " + track.cost + ")");
for (String s : track.iternity) System.out.println(s);
System.out.println("---- end route ------");
break; // + plus more offsets..
}
}
finishedOffsets = finishedOffsets.add(offsets);
if (solutionCount++ >= alternativeIdx) return track;
}
for (OsmLinkP link = currentNode.getFirstLink();
link != null;
link = link.getNext(currentNode)) {
addNextTripsForLink(trip, currentNode, currentLink, link, offsets, 0);
}
}
return track;
}
private void addNextTripsForLink(
ScheduledTrip trip,
OsmNodeP currentNode,
OsmLinkP currentLink,
OsmLinkP link,
OffsetSet offsets,
int level) {
if (link == currentLink) {
return; // just reverse, ignore
}
OsmNodeP node = link.getTarget(currentNode);
if (node == null) {
System.out.println("ups2: " + link);
return;
}
// calc distance and check nogos
rc.nogomatch = false;
int distance = rc.calcDistance(currentNode.ilon, currentNode.ilat, node.ilon, node.ilat);
if (rc.nogomatch) {
return;
}
if (link instanceof ScheduledLink) {
// System.out.println( "next trip for link: " + link + " at offset " + offsets );
ScheduledLink slink = (ScheduledLink) link;
ScheduledLine line = slink.line;
// line change delay
long delay = 0L;
if (currentLink instanceof ScheduledLink) {
delay =
((ScheduledLink) currentLink).line == line
? 0L
: (long) (rc.changetime * 1000.); // 3 minutes
}
long changePenalty = delay > 0 ? 60000L : 0L;
List<ScheduledDeparture> nextDepartures =
line.getScheduledDepartures(slink.indexInLine, time0 + trip.arrival + delay, offsets);
for (ScheduledDeparture nextDeparture : nextDepartures) {
ScheduledTrip nextTrip = new ScheduledTrip(nextDeparture.offsets, link, currentNode, trip);
long waitTime = nextDeparture.waitTime + delay;
long rideTime = nextDeparture.rideTime;
nextTrip.cost =
trip.cost
+ (int)
((rideTime + changePenalty + waitTime * rc.waittimeadjustment)
* rc.cost1speed
/ 3600.); // 160ms / meter = 22km/h
nextTrip.departure = trip.arrival + waitTime;
nextTrip.arrival = nextTrip.departure + rideTime;
addToOpenSet(nextTrip);
// System.out.println( "found: " + nextTrip );
}
} else if (link.isWayLink()) {
// get costfactor
rc.expctxWay.evaluate(link.isReverse(currentNode), link.descriptionBitmap, null);
// *** penalty for distance
float costfactor = rc.expctxWay.getCostfactor();
if (costfactor > 9999.) {
return;
}
int waycost = (int) (distance * costfactor + 0.5f);
// *** add initial cost if factor changed
float costdiff = costfactor - trip.lastcostfactor;
if (costdiff > 0.0005 || costdiff < -0.0005) {
waycost += (int) rc.expctxWay.getInitialcost();
}
if (node.getNodeDecsription() != null) {
rc.expctxNode.evaluate(
rc.expctxWay.getNodeAccessGranted() != 0., node.getNodeDecsription(), null);
float initialcost = rc.expctxNode.getInitialcost();
if (initialcost >= 1000000.) {
return;
}
waycost += (int) initialcost;
}
// *** penalty for turning angles
if (trip.originNode != null) {
// penalty proportional to direction change
double cos =
rc.calcCosAngle(
trip.originNode.ilon,
trip.originNode.ilat,
currentNode.ilon,
currentNode.ilat,
node.ilon,
node.ilat);
int turncost =
(int)
(cos * rc.expctxWay.getTurncost()
+ 0.2); // e.g. turncost=90 -> 90 degree = 90m penalty
waycost += turncost;
}
ScheduledTrip nextTrip = new ScheduledTrip(offsets, link, currentNode, trip);
// *** penalty for elevation
short ele2 = node.selev;
short ele1 = trip.selev;
int elefactor = 250000;
if (ele2 == Short.MIN_VALUE) ele2 = ele1;
nextTrip.selev = ele2;
if (ele1 != Short.MIN_VALUE) {
nextTrip.ehbd = trip.ehbd + (ele1 - ele2) * elefactor - distance * rc.downhillcutoff;
nextTrip.ehbu = trip.ehbu + (ele2 - ele1) * elefactor - distance * rc.uphillcutoff;
}
if (nextTrip.ehbd > rc.elevationpenaltybuffer) {
int excess = nextTrip.ehbd - rc.elevationpenaltybuffer;
int reduce = distance * rc.elevationbufferreduce;
if (reduce > excess) {
reduce = excess;
}
excess = nextTrip.ehbd - rc.elevationmaxbuffer;
if (reduce < excess) {
reduce = excess;
}
nextTrip.ehbd -= reduce;
if (rc.downhillcostdiv > 0) {
int elevationCost = reduce / rc.downhillcostdiv;
waycost += elevationCost;
}
} else if (nextTrip.ehbd < 0) {
nextTrip.ehbd = 0;
}
if (nextTrip.ehbu > rc.elevationpenaltybuffer) {
int excess = nextTrip.ehbu - rc.elevationpenaltybuffer;
int reduce = distance * rc.elevationbufferreduce;
if (reduce > excess) {
reduce = excess;
}
excess = nextTrip.ehbu - rc.elevationmaxbuffer;
if (reduce < excess) {
reduce = excess;
}
nextTrip.ehbu -= reduce;
if (rc.uphillcostdiv > 0) {
int elevationCost = reduce / rc.uphillcostdiv;
waycost += elevationCost;
}
} else if (nextTrip.ehbu < 0) {
nextTrip.ehbu = 0;
}
nextTrip.lastcostfactor = costfactor;
nextTrip.cost = trip.cost + (int) (waycost * rc.additionalcostfactor + 0.5);
nextTrip.departure = trip.arrival;
nextTrip.arrival =
nextTrip.departure + (long) (waycost * 3600. / rc.cost1speed); // 160ms / meter = 22km/h
addToOpenSet(nextTrip);
} else // connecting link
{
ScheduledTrip nextTrip = new ScheduledTrip(offsets, link, currentNode, trip);
long delay = (long) (rc.buffertime * 1000.); // 2 min
nextTrip.cost = trip.cost + (int) (delay * rc.waittimeadjustment * rc.cost1speed / 3600.);
nextTrip.departure = trip.arrival;
nextTrip.arrival = nextTrip.departure + delay;
addToOpenSet(nextTrip);
}
}
private OsmTrack compileTrip(ScheduledTrip trip, int offset) {
OsmTrack track = new OsmTrack();
track.iternity = new ArrayList<String>();
ScheduledTrip current = trip;
ScheduledLine lastLine = new ScheduledLine();
ScheduledLine dummyLine = new ScheduledLine();
List<ScheduledTrip> list = new ArrayList<ScheduledTrip>();
int distance = 0;
ScheduledTrip itrip = null;
String profile = extractProfile(rc.localFunction);
OsmNodeP nextNode = null;
while (current != null) {
System.out.println("trip=" + current);
OsmNodeP node = current.getTargetNode();
OsmPathElement pe = new OsmPathElement(node.ilon, node.ilat, node.selev, null);
track.addNode(pe);
if (nextNode != null) {
distance += node.calcDistance(nextNode);
}
boolean isScheduled = current.link instanceof ScheduledLink;
boolean isConnection = current.link.descriptionBitmap == null && !isScheduled;
ScheduledLine line =
isScheduled ? ((ScheduledLink) current.link).line : isConnection ? dummyLine : null;
if (line != lastLine && !isConnection) {
itrip = new ScheduledTrip();
itrip.departure = current.departure;
itrip.arrival = current.arrival;
itrip.originNode = current.originNode;
itrip.link = current.link;
if (isScheduled && list.size() > 0) {
list.get(list.size() - 1).originNode = current.getTargetNode();
}
list.add(itrip);
} else if (itrip != null && !isConnection) {
itrip.departure = current.departure;
itrip.originNode = current.originNode;
}
lastLine = line;
current = current.origin;
nextNode = node;
}
track.distance = distance;
track.cost = trip.cost;
for (int i = list.size() - 1; i >= 0; i--) {
current = list.get(i);
String lineName = profile;
boolean isScheduled = current.link instanceof ScheduledLink;
if (isScheduled) {
lineName = ((ScheduledLink) current.link).line.name;
}
String stationName = "*position*";
if (current.originNode instanceof StationNode) {
stationName = ((StationNode) current.originNode).name;
}
String nextStationName = "*position*";
if (i > 0 && list.get(i - 1).originNode instanceof StationNode) {
nextStationName = ((StationNode) list.get(i - 1).originNode).name;
}
{
Date d0 = new Date(time0 + 60000L * offset + current.departure);
Date d1 = new Date(time0 + 60000L * offset + current.arrival);
if (track.iternity.size() > 0) track.iternity.add("");
track.iternity.add("depart: " + d0 + " " + stationName);
track.iternity.add(" --- " + lineName + " ---");
track.iternity.add("arrive: " + d1 + " " + nextStationName);
}
}
return track;
}
private void addToOpenSet(ScheduledTrip nextTrip) {
int distance = nextTrip.getTargetNode().calcDistance(end);
nextTrip.adjustedCost = nextTrip.cost + (int) (distance * rc.pass1coefficient + 0.5);
openSet.add(nextTrip.adjustedCost, nextTrip);
}
