private void visitAllStartSegments(
final RoutingContext ctx,
RouteSegment start,
PriorityQueue<RouteSegment> graphDirectSegments,
TLongObjectHashMap<RouteSegment> visitedSegments,
int startX,
int startY)
throws IOException {
// mark as visited code seems to be duplicated
long nt = (start.road.getId() << 8l) + start.segmentStart;
visitedSegments.put(nt, start);
graphDirectSegments.add(start);
loadRoutes(
ctx,
(startX >> (31 - ctx.getZoomToLoadTileWithRoads())),
(startY >> (31 - ctx.getZoomToLoadTileWithRoads())));
long ls = (((long) startX) << 31) + (long) startY;
RouteSegment startNbs = ctx.routes.get(ls);
while (startNbs != null) { // startNbs.road.id >> 1, start.road.id >> 1
if (startNbs.road.getId() != start.road.getId()) {
startNbs.parentRoute = start;
startNbs.parentSegmentEnd = start.segmentStart;
startNbs.distanceToEnd = start.distanceToEnd;
// duplicated to be sure start is added
nt = (startNbs.road.getId() << 8l) + startNbs.segmentStart;
visitedSegments.put(nt, startNbs);
graphDirectSegments.add(startNbs);
}
startNbs = startNbs.next;
}
}
public void loadRoutes(final RoutingContext ctx, int tileX, int tileY) throws IOException {
int tileC = (tileX << ctx.getZoomToLoadTileWithRoads()) + tileY;
if (ctx.loadedTiles.contains(tileC)) {
return;
}
long now = System.nanoTime();
int zoomToLoad = 31 - ctx.getZoomToLoadTileWithRoads();
SearchFilter searchFilter =
new BinaryMapIndexReader.SearchFilter() {
@Override
public boolean accept(TIntArrayList types, MapIndex index) {
for (int j = 0; j < types.size(); j++) {
int wholeType = types.get(j);
TagValuePair pair = index.decodeType(wholeType);
if (pair != null) {
int t = wholeType & 3;
if (t == MapRenderingTypes.POINT_TYPE) {
if (ctx.getRouter().acceptPoint(pair)) {
return true;
}
} else if (t == MapRenderingTypes.POLYLINE_TYPE) {
if (ctx.getRouter().acceptLine(pair)) {
return true;
}
}
}
}
return false;
}
};
SearchRequest<BinaryMapDataObject> request =
BinaryMapIndexReader.buildSearchRequest(
tileX << zoomToLoad,
(tileX + 1) << zoomToLoad,
tileY << zoomToLoad,
(tileY + 1) << zoomToLoad,
15,
searchFilter);
for (BinaryMapIndexReader r : map) {
r.searchMapIndex(request);
for (BinaryMapDataObject o : request.getSearchResults()) {
BinaryMapDataObject old = ctx.idObjects.get(o.getId());
// sometimes way are presented only partially in one index
if (old != null && old.getPointsLength() >= o.getPointsLength()) {
continue;
}
ctx.idObjects.put(o.getId(), o);
for (int j = 0; j < o.getPointsLength(); j++) {
long l = (((long) o.getPoint31XTile(j)) << 31) + (long) o.getPoint31YTile(j);
RouteSegment segment = new RouteSegment();
segment.road = o;
segment.segmentEnd = segment.segmentStart = j;
if (ctx.routes.get(l) != null) {
segment.next = ctx.routes.get(l);
}
ctx.routes.put(l, segment);
}
}
ctx.loadedTiles.add(tileC);
ctx.timeToLoad += (System.nanoTime() - now);
}
}
private RouteSegment processIntersectionsWithWays(
RoutingContext ctx,
PriorityQueue<RouteSegment> graphSegments,
TLongObjectHashMap<RouteSegment> visitedSegments,
TLongObjectHashMap<RouteSegment> oppositeSegments,
double distOnRoadToPass,
double distToFinalPoint,
RouteSegment segment,
BinaryMapDataObject road,
boolean firstOfSegment,
int segmentEnd,
RouteSegment inputNext,
boolean reverseWay) {
// This variables can be in routing context
// initialize temporary lists to calculate not forbidden ways at way intersections
ArrayList<RouteSegment> segmentsToVisitPrescripted = new ArrayList<RouteSegment>(5);
ArrayList<RouteSegment> segmentsToVisitNotForbidden = new ArrayList<RouteSegment>(5);
// collect time for obstacles
double obstaclesTime = 0;
boolean exclusiveRestriction = false;
// 3.1 calculate time for obstacles (bumps, traffic_signals, level_crossing)
if (firstOfSegment) {
RouteSegment possibleObstacle = inputNext;
while (possibleObstacle != null) {
obstaclesTime +=
ctx.getRouter().defineObstacle(possibleObstacle.road, possibleObstacle.segmentStart);
possibleObstacle = possibleObstacle.next;
}
}
// 3.2 calculate possible ways to put into priority queue
// for debug next.road.getId() >> 1 == 33911427 && road.getId() >> 1 == 33911442
RouteSegment next = inputNext;
while (next != null) {
long nts = (next.road.getId() << 8l) + next.segmentStart;
boolean oppositeConnectionFound =
oppositeSegments.containsKey(nts) && oppositeSegments.get(nts) != null;
boolean processRoad = true;
if (ctx.isUseStrategyOfIncreasingRoadPriorities()) {
double roadPriority = ctx.getRouter().getRoadPriorityHeuristicToIncrease(segment.road);
double nextRoadPriority = ctx.getRouter().getRoadPriorityHeuristicToIncrease(segment.road);
if (nextRoadPriority < roadPriority) {
processRoad = false;
}
}
/* next.road.getId() >> 1 (3) != road.getId() >> 1 (3) - used that line for debug with osm map */
// road.id could be equal on roundabout, but we should accept them
if ((!visitedSegments.contains(nts) && processRoad) || oppositeConnectionFound) {
int type = -1;
if (!reverseWay) {
for (int i = 0; i < road.getRestrictionCount(); i++) {
if (road.getRestriction(i) == next.road.getId()) {
type = road.getRestrictionType(i);
break;
}
}
} else {
for (int i = 0; i < next.road.getRestrictionCount(); i++) {
if (next.road.getRestriction(i) == road.getId()) {
type = next.road.getRestrictionType(i);
break;
}
// Check if there is restriction only to the current road
if (next.road.getRestrictionType(i) == MapRenderingTypes.RESTRICTION_ONLY_RIGHT_TURN
|| next.road.getRestrictionType(i) == MapRenderingTypes.RESTRICTION_ONLY_LEFT_TURN
|| next.road.getRestrictionType(i)
== MapRenderingTypes.RESTRICTION_ONLY_STRAIGHT_ON) {
// check if that restriction applies to considered junk
RouteSegment foundNext = inputNext;
while (foundNext != null
&& foundNext.getRoad().getId() != next.road.getRestriction(i)) {
foundNext = foundNext.next;
}
if (foundNext != null) {
type = REVERSE_WAY_RESTRICTION_ONLY; // special constant
}
}
}
}
if (type == REVERSE_WAY_RESTRICTION_ONLY) {
// next = next.next; continue;
} else if (type == -1 && exclusiveRestriction) {
// next = next.next; continue;
} else if (type == MapRenderingTypes.RESTRICTION_NO_LEFT_TURN
|| type == MapRenderingTypes.RESTRICTION_NO_RIGHT_TURN
|| type == MapRenderingTypes.RESTRICTION_NO_STRAIGHT_ON
|| type == MapRenderingTypes.RESTRICTION_NO_U_TURN) {
// next = next.next; continue;
} else {
// no restriction can go out
if (oppositeConnectionFound) {
RouteSegment oppSegment = oppositeSegments.get(nts);
oppSegment.segmentEnd = next.segmentStart;
return oppSegment;
}
double distanceToEnd = distToFinalPoint / ctx.getRouter().getMaxDefaultSpeed();
if (ctx.isUseDynamicRoadPrioritising()) {
double priority = ctx.getRouter().getRoadPriorityToCalculateRoute(next.road);
distanceToEnd /= priority;
}
// Using A* routing algorithm
// g(x) - calculate distance to that point and calculate time
double speed = ctx.getRouter().defineSpeed(road);
if (speed == 0) {
speed = ctx.getRouter().getMinDefaultSpeed();
}
double distanceFromStart = segment.distanceFromStart + distOnRoadToPass / speed;
// calculate turn time
distanceFromStart += ctx.getRouter().calculateTurnTime(segment, next, segmentEnd);
// add obstacles time
distanceFromStart += obstaclesTime;
// segment.getRoad().getId() >> 1
if (next.parentRoute == null
|| ctx.roadPriorityComparator(
next.distanceFromStart, next.distanceToEnd, distanceFromStart, distanceToEnd)
> 0) {
next.distanceFromStart = distanceFromStart;
next.distanceToEnd = distanceToEnd;
if (next.parentRoute != null) {
// already in queue remove it
graphSegments.remove(next);
}
// put additional information to recover whole route after
next.parentRoute = segment;
next.parentSegmentEnd = segmentEnd;
if (type == -1) {
// case no restriction
segmentsToVisitNotForbidden.add(next);
} else {
// case exclusive restriction (only_right, only_straight, ...)
// 1. in case we are going backward we should not consider only_restriction
// as exclusive because we have main "in" roads and one "out"
// 2. in case we are going forward we have one "in" and many "out"
if (!reverseWay) {
exclusiveRestriction = true;
segmentsToVisitNotForbidden.clear();
segmentsToVisitPrescripted.add(next);
} else {
segmentsToVisitNotForbidden.add(next);
}
}
}
}
}
next = next.next;
}
// add all allowed route segments to priority queue
for (RouteSegment s : segmentsToVisitNotForbidden) {
graphSegments.add(s);
}
for (RouteSegment s : segmentsToVisitPrescripted) {
graphSegments.add(s);
}
return null;
}
private RoutePair processRouteSegment(
final RoutingContext ctx,
RouteSegment end,
boolean reverseWaySearch,
PriorityQueue<RouteSegment> graphSegments,
TLongObjectHashMap<RouteSegment> visitedSegments,
int targetEndX,
int targetEndY,
RouteSegment segment,
TLongObjectHashMap<RouteSegment> oppositeSegments)
throws IOException {
// Always start from segmentStart (!), not from segmentEnd
// It makes difference only for the first start segment
// Middle point will always be skipped from observation considering already visited
final BinaryMapDataObject road = segment.road;
final int middle = segment.segmentStart;
int middlex = road.getPoint31XTile(middle);
int middley = road.getPoint31YTile(middle);
// 0. mark route segment as visited
long nt = (road.getId() << 8l) + middle;
// avoid empty segments to connect but mark the point as visited
visitedSegments.put(nt, null);
if (oppositeSegments.contains(nt) && oppositeSegments.get(nt) != null) {
segment.segmentEnd = middle;
RouteSegment opposite = oppositeSegments.get(nt);
opposite.segmentEnd = middle;
return new RoutePair(segment, opposite);
}
boolean oneway = ctx.getRouter().isOneWay(road);
boolean minusAllowed = !oneway || reverseWaySearch;
boolean plusAllowed = !oneway || !reverseWaySearch;
// +/- diff from middle point
int d = plusAllowed ? 1 : -1;
// Go through all point of the way and find ways to continue
// ! Actually there is small bug when there is restriction to move forward on way (it doesn't
// take into account)
while (minusAllowed || plusAllowed) {
// 1. calculate point not equal to middle
// (algorithm should visit all point on way if it is not oneway)
int segmentEnd = middle + d;
if (!minusAllowed && d > 0) {
d++;
} else if (!plusAllowed && d < 0) {
d--;
} else {
if (d <= 0) {
d = -d + 1;
} else {
d = -d;
}
}
if (segmentEnd < 0) {
minusAllowed = false;
continue;
}
if (segmentEnd >= road.getPointsLength()) {
plusAllowed = false;
continue;
}
// if we found end point break cycle
long nts = (road.getId() << 8l) + segmentEnd;
if (oppositeSegments.contains(nts) && oppositeSegments.get(nt) != null) {
segment.segmentEnd = segmentEnd;
RouteSegment opposite = oppositeSegments.get(nts);
opposite.segmentEnd = segmentEnd;
return new RoutePair(segment, opposite);
}
visitedSegments.put(nts, segment);
// 2. calculate point and try to load neighbor ways if they are not loaded
int x = road.getPoint31XTile(segmentEnd);
int y = road.getPoint31YTile(segmentEnd);
loadRoutes(
ctx,
(x >> (31 - ctx.getZoomToLoadTileWithRoads())),
(y >> (31 - ctx.getZoomToLoadTileWithRoads())));
long l = (((long) x) << 31) + (long) y;
RouteSegment next = ctx.routes.get(l);
// 3. get intersected ways
if (next != null) {
double distOnRoadToPass = squareRootDist(x, y, middlex, middley);
double distToFinalPoint = squareRootDist(x, y, targetEndX, targetEndY);
RouteSegment foundIntersection =
processIntersectionsWithWays(
ctx,
graphSegments,
visitedSegments,
oppositeSegments,
distOnRoadToPass,
distToFinalPoint,
segment,
road,
d == 0,
segmentEnd,
next,
reverseWaySearch);
if (foundIntersection != null) {
segment.segmentEnd = segmentEnd;
return new RoutePair(segment, foundIntersection);
}
}
}
return null;
}
/**
* Calculate route between start.segmentEnd and end.segmentStart (using A* algorithm) return list
* of segments
*/
public List<RouteSegmentResult> searchRoute(
final RoutingContext ctx, RouteSegment start, RouteSegment end) throws IOException {
// measure time
ctx.timeToLoad = 0;
ctx.visitedSegments = 0;
long startNanoTime = System.nanoTime();
// Initializing priority queue to visit way segments
Comparator<RouteSegment> segmentsComparator =
new Comparator<RouteSegment>() {
@Override
public int compare(RouteSegment o1, RouteSegment o2) {
return ctx.roadPriorityComparator(
o1.distanceFromStart, o1.distanceToEnd, o2.distanceFromStart, o2.distanceToEnd);
}
};
Comparator<RouteSegment> nonHeuristicSegmentsComparator =
new Comparator<RouteSegment>() {
@Override
public int compare(RouteSegment o1, RouteSegment o2) {
return roadPriorityComparator(
o1.distanceFromStart,
o1.distanceToEnd,
o2.distanceFromStart,
o2.distanceToEnd,
0.5);
}
};
PriorityQueue<RouteSegment> graphDirectSegments =
new PriorityQueue<RouteSegment>(50, segmentsComparator);
PriorityQueue<RouteSegment> graphReverseSegments =
new PriorityQueue<RouteSegment>(50, segmentsComparator);
// Set to not visit one segment twice (stores road.id << X + segmentStart)
TLongObjectHashMap<RouteSegment> visitedDirectSegments = new TLongObjectHashMap<RouteSegment>();
TLongObjectHashMap<RouteSegment> visitedOppositeSegments =
new TLongObjectHashMap<RouteSegment>();
int targetEndX = end.road.getPoint31XTile(end.segmentStart);
int targetEndY = end.road.getPoint31YTile(end.segmentStart);
int startX = start.road.getPoint31XTile(start.segmentStart);
int startY = start.road.getPoint31YTile(start.segmentStart);
// for start : f(start) = g(start) + h(start) = 0 + h(start) = h(start)
start.distanceToEnd =
squareRootDist(startX, startY, targetEndX, targetEndY)
/ ctx.getRouter().getMaxDefaultSpeed();
end.distanceToEnd = start.distanceToEnd;
// because first point of the start is not visited do the same as in cycle but only for one
// point
// it matters when start point is intersection of different roads
// add start segment to priority queue
visitAllStartSegments(ctx, start, graphDirectSegments, visitedDirectSegments, startX, startY);
visitAllStartSegments(
ctx, end, graphReverseSegments, visitedOppositeSegments, targetEndX, targetEndY);
// final segment before end
RouteSegment finalDirectRoute = null;
RouteSegment finalReverseRoute = null;
// Extract & analyze segment with min(f(x)) from queue while final segment is not found
boolean inverse = false;
PriorityQueue<RouteSegment> graphSegments =
inverse ? graphReverseSegments : graphDirectSegments;
while (!graphSegments.isEmpty()) {
RouteSegment segment = graphSegments.poll();
ctx.visitedSegments++;
// for debug purposes
if (ctx.visitor != null) {
ctx.visitor.visitSegment(segment);
}
if (!inverse) {
RoutePair pair =
processRouteSegment(
ctx,
end,
false,
graphDirectSegments,
visitedDirectSegments,
targetEndX,
targetEndY,
segment,
visitedOppositeSegments);
if (pair != null) {
finalDirectRoute = pair.a;
finalReverseRoute = pair.b;
break;
}
} else {
RoutePair pair =
processRouteSegment(
ctx,
start,
true,
graphReverseSegments,
visitedOppositeSegments,
startX,
startY,
segment,
visitedDirectSegments);
if (pair != null) {
finalReverseRoute = pair.a;
finalDirectRoute = pair.b;
break;
}
}
if (graphReverseSegments.isEmpty() || graphDirectSegments.isEmpty()) {
break;
}
if (ctx.planRouteIn2Directions()) {
inverse =
nonHeuristicSegmentsComparator.compare(
graphDirectSegments.peek(), graphReverseSegments.peek())
> 0;
// make it more simmetrical with dynamic prioritizing it makes big sense
// inverse = !inverse;
} else {
// different strategy : use onedirectional graph
inverse = !ctx.getPlanRoadDirection().booleanValue();
}
graphSegments = inverse ? graphReverseSegments : graphDirectSegments;
}
// 4. Route is found : collect all segments and prepare result
return prepareResult(ctx, start, end, startNanoTime, finalDirectRoute, finalReverseRoute);
}
public RouteSegment findRouteSegment(double lat, double lon, RoutingContext ctx)
throws IOException {
double tileX = MapUtils.getTileNumberX(ctx.getZoomToLoadTileWithRoads(), lon);
double tileY = MapUtils.getTileNumberY(ctx.getZoomToLoadTileWithRoads(), lat);
loadRoutes(ctx, (int) tileX, (int) tileY);
RouteSegment road = null;
double sdist = 0;
int px = MapUtils.get31TileNumberX(lon);
int py = MapUtils.get31TileNumberY(lat);
for (BinaryMapDataObject r : ctx.values()) {
if (r.getPointsLength() > 1) {
double priority = ctx.getRouter().getRoadPriorityToCalculateRoute(r);
for (int j = 1; j < r.getPointsLength(); j++) {
double mDist =
squareRootDist(
r.getPoint31XTile(j),
r.getPoint31YTile(j),
r.getPoint31XTile(j - 1),
r.getPoint31YTile(j - 1));
double projection =
calculateProjection(
r.getPoint31XTile(j - 1),
r.getPoint31YTile(j - 1),
r.getPoint31XTile(j),
r.getPoint31YTile(j),
px,
py,
mDist);
double currentsDist;
if (projection
< 0) { // TODO: first 2 and last 2 points of a route should be only near and not based
// on road priority (I.E. a motorway road node unreachable near my house)
currentsDist =
squareDist(r.getPoint31XTile(j - 1), r.getPoint31YTile(j - 1), px, py)
/ (priority * priority);
} else if (projection > mDist) {
currentsDist =
squareDist(r.getPoint31XTile(j), r.getPoint31YTile(j), px, py)
/ (priority * priority);
} else {
currentsDist =
calculatesquareDistance(
r.getPoint31XTile(j - 1),
r.getPoint31YTile(j - 1),
r.getPoint31XTile(j),
r.getPoint31YTile(j),
px,
py,
mDist)
/ (priority * priority);
}
if (road == null || currentsDist < sdist) {
road = new RouteSegment();
road.road = r;
road.segmentStart =
j
- 1; // TODO: first 2 and last 2 segments should be based on projection. my
// start/finish point S/F, fake point P between j-1 & j -> SP, PJ; should
// end at finish point: JP,PF
road.segmentEnd = j;
sdist = currentsDist;
}
}
}
}
return road;
}