219 lines
6.8 KiB
Java
219 lines
6.8 KiB
Java
package net.osmand.router;
|
|
|
|
import gnu.trove.list.array.TIntArrayList;
|
|
|
|
import java.util.ArrayList;
|
|
import java.util.List;
|
|
|
|
import net.osmand.binary.RouteDataObject;
|
|
import net.osmand.data.QuadPoint;
|
|
import net.osmand.data.QuadRect;
|
|
import net.osmand.data.QuadTree;
|
|
import net.osmand.util.MapUtils;
|
|
|
|
public class PrecalculatedRouteDirection {
|
|
|
|
private TIntArrayList pointsX = new TIntArrayList();
|
|
private TIntArrayList pointsY = new TIntArrayList();
|
|
private float speed;
|
|
private float[] tms;
|
|
private static final int SHIFT = (1 << (31 - 17));
|
|
private static final int[] SHIFTS = new int[]{1 << (31 - 15), 1 << (31 - 13), 1 << (31 - 12),
|
|
1 << (31 - 11), 1 << (31 - 7)};
|
|
|
|
private List<Integer> cachedS = new ArrayList<Integer>();
|
|
|
|
private long startPoint = 0;
|
|
private long endPoint = 0;
|
|
// private DataTileManager<Integer> indexedPoints = new DataTileManager<Integer>(17);
|
|
QuadTree<Integer> quadTree = new QuadTree<Integer>(new QuadRect(0, 0, Integer.MAX_VALUE, Integer.MAX_VALUE),
|
|
8, 0.55f);
|
|
|
|
private PrecalculatedRouteDirection(List<RouteSegmentResult> ls, float avgSpeed) {
|
|
this.speed = avgSpeed;
|
|
init(ls);
|
|
}
|
|
|
|
private PrecalculatedRouteDirection(PrecalculatedRouteDirection parent, int s1, int s2) {
|
|
this.speed = parent.speed;
|
|
tms = new float[s2 - s1 + 1];
|
|
for (int i = s1; i <= s2; i++) {
|
|
pointsX.add(parent.pointsX.get(i));
|
|
pointsY.add(parent.pointsY.get(i));
|
|
// indexedPoints.registerObjectXY(parent.pointsX.get(i), parent.pointsY.get(i), pointsX.size() - 1);
|
|
quadTree.insert(pointsX.size() - 1, parent.pointsX.get(i), parent.pointsY.get(i));
|
|
tms[i - s1] = parent.tms[i] - parent.tms[s2];
|
|
}
|
|
}
|
|
|
|
public static PrecalculatedRouteDirection build(List<RouteSegmentResult> ls, float cutoffDistance, float avgSpeed){
|
|
int begi = 0;
|
|
float d = cutoffDistance;
|
|
for (; begi < ls.size(); begi++) {
|
|
d -= ls.get(begi).getDistance();
|
|
if (d < 0) {
|
|
break;
|
|
}
|
|
}
|
|
int endi = ls.size();
|
|
d = cutoffDistance;
|
|
for (; endi > 0; endi--) {
|
|
d -= ls.get(endi - 1).getDistance();
|
|
if (d < 0) {
|
|
break;
|
|
}
|
|
}
|
|
if(begi < endi) {
|
|
return new PrecalculatedRouteDirection(ls.subList(begi, endi), avgSpeed);
|
|
}
|
|
return null;
|
|
}
|
|
|
|
|
|
private void init(List<RouteSegmentResult> ls) {
|
|
float totaltm = 0;
|
|
List<Float> times = new ArrayList<Float>();
|
|
for (RouteSegmentResult s : ls) {
|
|
boolean plus = s.getStartPointIndex() < s.getEndPointIndex();
|
|
int i = s.getStartPointIndex();
|
|
RouteDataObject obj = s.getObject();
|
|
float routeSpd = s.getDistance()/ s.getRoutingTime() ;
|
|
while (true) {
|
|
int iprev = i;
|
|
i = plus? i + 1 : i -1;
|
|
// MapUtils.measuredDist31 vs BinaryRoutePlanner.squareRootDist
|
|
// use measuredDist31 because we use precise s.getDistance() to calculate routeSpd
|
|
float dist = (float) MapUtils.measuredDist31(obj.getPoint31XTile(iprev), obj.getPoint31YTile(iprev),
|
|
obj.getPoint31XTile(i), obj.getPoint31YTile(i));
|
|
float tm = dist / routeSpd;
|
|
pointsX.add(obj.getPoint31XTile(i));
|
|
pointsY.add(obj.getPoint31YTile(i));
|
|
times.add(tm);
|
|
quadTree.insert(pointsX.size() - 1, obj.getPoint31XTile(i), obj.getPoint31YTile(i));
|
|
// indexedPoints.registerObjectXY();
|
|
totaltm += tm;
|
|
|
|
if (i == s.getEndPointIndex()) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
tms = new float[times.size()];
|
|
float totDec = totaltm;
|
|
for(int i = 0; i < times.size(); i++) {
|
|
totDec -= times.get(i);
|
|
tms[i] = totDec;
|
|
}
|
|
|
|
}
|
|
|
|
public float timeEstimate(int sx31, int sy31, int ex31, int ey31) {
|
|
long l1 = calc(sx31, sy31);
|
|
long l2 = calc(ex31, ey31);
|
|
int x31;
|
|
int y31;
|
|
boolean start;
|
|
if(l1 == startPoint || l1 == endPoint) {
|
|
start = l1 == startPoint;
|
|
x31 = ex31;
|
|
y31 = ey31;
|
|
} else if(l2 == startPoint || l2 == endPoint) {
|
|
start = l2 == startPoint;
|
|
x31 = sx31;
|
|
y31 = sy31;
|
|
} else {
|
|
throw new UnsupportedOperationException();
|
|
}
|
|
int ind = getIndex(x31, y31);
|
|
if(ind == -1) {
|
|
return -1;
|
|
}
|
|
if((ind == 0 && start) ||
|
|
(ind == pointsX.size() - 1 && !start)) {
|
|
return -1;
|
|
}
|
|
float distToPoint = getDeviationDistance(x31, y31, ind);
|
|
float deviationPenalty = distToPoint / speed;
|
|
if(start) {
|
|
return (tms[0] - tms[ind]) + deviationPenalty;
|
|
} else {
|
|
return tms[ind] + deviationPenalty;
|
|
}
|
|
}
|
|
|
|
public float getDeviationDistance(int x31, int y31) {
|
|
int ind = getIndex(x31, y31);
|
|
if(ind == -1) {
|
|
return 0;
|
|
}
|
|
return getDeviationDistance(x31, y31, ind);
|
|
}
|
|
|
|
public float getDeviationDistance(int x31, int y31, int ind) {
|
|
float distToPoint = 0; //BinaryRoutePlanner.squareRootDist(x31, y31, pointsX.get(ind), pointsY.get(ind));
|
|
if(ind < pointsX.size() - 1 && ind != 0) {
|
|
double nx = BinaryRoutePlanner.squareRootDist(x31, y31, pointsX.get(ind + 1), pointsY.get(ind + 1));
|
|
double pr = BinaryRoutePlanner.squareRootDist(x31, y31, pointsX.get(ind - 1), pointsY.get(ind - 1));
|
|
int nind = nx > pr ? ind -1 : ind +1;
|
|
QuadPoint proj = MapUtils.getProjectionPoint31(x31, y31, pointsX.get(ind), pointsY.get(ind), pointsX.get(nind), pointsX.get(nind));
|
|
distToPoint = (float) BinaryRoutePlanner.squareRootDist(x31, y31, (int)proj.x, (int)proj.y) ;
|
|
}
|
|
return distToPoint;
|
|
}
|
|
|
|
public int getIndex(int x31, int y31) {
|
|
int ind = -1;
|
|
cachedS.clear();
|
|
// indexedPoints.getObjects(x31 - SHIFT, y31 - SHIFT, x31 + SHIFT, y31 + SHIFT, cachedS);
|
|
quadTree.queryInBox(new QuadRect(x31 - SHIFT, y31 - SHIFT, x31 + SHIFT, y31 + SHIFT), cachedS);
|
|
if (cachedS.size() == 0) {
|
|
for (int k = 0; k < SHIFTS.length; k++) {
|
|
quadTree.queryInBox(new QuadRect(x31 - SHIFTS[k], y31 - SHIFTS[k], x31 + SHIFTS[k], y31 + SHIFTS[k]), cachedS);
|
|
// indexedPoints.getObjects(x31 - SHIFTS[k], y31 - SHIFTS[k], x31 + SHIFTS[k], y31 + SHIFTS[k],cachedS);
|
|
if (cachedS.size() != 0) {
|
|
break;
|
|
}
|
|
}
|
|
if (cachedS.size() == 0) {
|
|
return -1;
|
|
}
|
|
}
|
|
double minDist = 0;
|
|
for (int i = 0; i < cachedS.size(); i++) {
|
|
Integer n = cachedS.get(i);
|
|
double ds = BinaryRoutePlanner.squareRootDist(x31, y31, pointsX.get(n), pointsY.get(n));
|
|
if (ds < minDist || i == 0) {
|
|
ind = n;
|
|
minDist = ds;
|
|
}
|
|
}
|
|
return ind;
|
|
}
|
|
|
|
private long calc(int x31, int y31) {
|
|
return ((long) x31) << 32l + ((long)y31);
|
|
}
|
|
|
|
public PrecalculatedRouteDirection adopt(RoutingContext ctx) {
|
|
int ind1 = getIndex(ctx.startX, ctx.startY);
|
|
int ind2 = getIndex(ctx.targetX, ctx.targetY);
|
|
if(ind1 == -1) {
|
|
throw new IllegalArgumentException();
|
|
}
|
|
if(ind2 == -1) {
|
|
throw new IllegalArgumentException();
|
|
}
|
|
PrecalculatedRouteDirection routeDirection = new PrecalculatedRouteDirection(this, ind1, ind2);
|
|
routeDirection.startPoint = calc(ctx.startX, ctx.startY);
|
|
// routeDirection.startX31 = ctx.startX;
|
|
// routeDirection.startY31 = ctx.startY;
|
|
routeDirection.endPoint = calc(ctx.targetX, ctx.targetX);
|
|
// routeDirection.endX31 = ctx.targetX;
|
|
// routeDirection.endY31 = ctx.targetY;
|
|
return routeDirection;
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|