Refactoring routing a bit

This commit is contained in:
Victor Shcherb 2011-07-03 17:32:24 +02:00
parent 3bbc1e49ac
commit dfca6a94d3
5 changed files with 255 additions and 252 deletions

View file

@ -144,16 +144,17 @@ public class BicycleRouter extends VehicleRouter {
return 9;
}
public double calculateTurnTime(int middley, int middlex, int x, int y, RouteSegment segment, RouteSegment next, int j) {
boolean lineAreNotConnected = j < segment.road.getPointsLength() - 1 || next.segmentStart != 0;
if (lineAreNotConnected) {
return 5;
} else {
if (next.road.getPointsLength() > 1) {
public double calculateTurnTime(RouteSegment segment, RouteSegment next, int segmentEnd) {
boolean end = (segmentEnd == segment.road.getPointsLength() - 1 || segmentEnd == 0);
boolean start = next.segmentStart == 0;
if (end) {
if(!start){
return 5;
}
return 0;
} else {
return 5;
}
return 0;
}
}

View file

@ -32,7 +32,7 @@ public class BinaryRoutePlanner {
private final static boolean PRINT_TO_CONSOLE_ROUTE_INFORMATION_TO_TEST = true;
private final BinaryMapIndexReader[] map;
private int HEURISTIC_COEFFICIENT = 3;
private static int DEFAULT_HEURISTIC_COEFFICIENT = 3;
private static final Log log = LogUtil.getLog(BinaryRoutePlanner.class);
@ -142,26 +142,19 @@ public class BinaryRoutePlanner {
return road;
}
public int roadPriorityComparator(double o1DistanceFromStart, double o1DistanceToEnd,
double o2DistanceFromStart, double o2DistanceToEnd) {
// f(x) = g(x) + h(x) --- g(x) - distanceFromStart, h(x) - distanceToEnd (not exact)
return Double.compare(o1DistanceFromStart + HEURISTIC_COEFFICIENT * o1DistanceToEnd,
o2DistanceFromStart + HEURISTIC_COEFFICIENT * o2DistanceToEnd);
}
// TODO write unit tests
// TODO add information about turns
// TODO think about u-turn
// TODO fix roundabout
// TODO fix roundabout (?)
// TODO access
// TODO bicycle router (?)
// TODO fastest/shortest way
/**
* Calculate route between start.segmentEnd and end.segmentStart (using A* algorithm)
* return list of segments
*/
public List<RouteSegmentResult> searchRoute(RoutingContext ctx, RouteSegment start, RouteSegment end) throws IOException {
public List<RouteSegmentResult> searchRoute(final RoutingContext ctx, RouteSegment start, RouteSegment end) throws IOException {
// measure time
ctx.timeToLoad = 0;
@ -172,24 +165,21 @@ public class BinaryRoutePlanner {
Comparator<RouteSegment> segmentsComparator = new Comparator<RouteSegment>(){
@Override
public int compare(RouteSegment o1, RouteSegment o2) {
return roadPriorityComparator(o1.distanceFromStart, o1.distanceToEnd, o2.distanceFromStart, o2.distanceToEnd);
return ctx.roadPriorityComparator(o1.distanceFromStart, o1.distanceToEnd, o2.distanceFromStart, o2.distanceToEnd);
}
};
PriorityQueue<RouteSegment> graphSegments = new PriorityQueue<RouteSegment>(50, segmentsComparator);
// 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);
// Set to not visit one segment twice (stores road.id << X + segmentStart)
TLongHashSet visitedSegments = new TLongHashSet();
int endX = end.road.getPoint31XTile(end.segmentEnd);
int endY = end.road.getPoint31YTile(end.segmentEnd);
int targetEndX = end.road.getPoint31XTile(end.segmentEnd);
int targetEndY = end.road.getPoint31YTile(end.segmentEnd);
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, endX, endY) / ctx.router.getMaxDefaultSpeed();
start.distanceToEnd = squareRootDist(startX, startY, targetEndX, targetEndY) / ctx.router.getMaxDefaultSpeed();
// add start segment to priority queue
graphSegments.add(start);
@ -240,8 +230,6 @@ public class BinaryRoutePlanner {
if(end.road.getId() == road.getId() && end.segmentStart == middle){
finalRoute = segment;
}
// collect time for obstacles
double obstaclesTime = 0;
// Go through all point of the way and find ways to continue
while(finalRoute == null && ((!oneway && minus) || plus)) {
@ -281,97 +269,12 @@ public class BinaryRoutePlanner {
// 3. get intersected ways
RouteSegment next = ctx.routes.get(l);
if (next != null) {
segmentsToVisitPrescripted.clear();
segmentsToVisitNotForbidden.clear();
boolean exclusiveRestriction = false;
// 3.1 calculate time for obstacles (bumps, traffic_signals, level_crossing)
if (d != 0) {
RouteSegment possibleObstacle = next;
while (possibleObstacle != null) {
ctx.router.defineObstacle(possibleObstacle.road, possibleObstacle.segmentStart);
possibleObstacle = possibleObstacle.next;
}
}
// 3.2 calculate possible ways to put into priority queue
while(next != null){
long nts = (next.road.getId() << 8l) + next.segmentStart;
/* next.road.id >> 1 != road.id >> 1 - used that line for debug with osm map */
// road.id could be equal on roundabout, but we should accept them
if(!visitedSegments.contains(nts)){
int type = -1;
for(int i = 0; i< road.getRestrictionCount(); i++){
if(road.getRestriction(i) == next.road.getId()){
type = road.getRestrictionType(i);
break;
}
}
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 {
int x = road.getPoint31XTile(j);
int y = road.getPoint31YTile(j);
// Using A* routing algorithm
// g(x) - calculate distance to that point and calculate time
double speed = ctx.router.defineSpeed(road);
if(speed == 0){
speed = ctx.router.getMinDefaultSpeed();
}
double distanceFromStart = segment.distanceFromStart + squareRootDist(x, y, middlex, middley) / speed;
// calculate turn time
distanceFromStart += ctx.router.calculateTurnTime(middley, middlex, x, y, segment, next, j);
// add obstacles time
distanceFromStart += obstaclesTime;
double distanceToEnd = squareRootDist(x, y, endX, endY) / ctx.router.getMaxDefaultSpeed();
if(next.parentRoute == null ||
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 = j;
if(type == -1){
// case no restriction
segmentsToVisitNotForbidden.add(next);
} else {
// case exclusive restriction (only_right, only_straight, ...)
exclusiveRestriction = true;
segmentsToVisitNotForbidden.clear();
segmentsToVisitPrescripted.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);
}
int x = road.getPoint31XTile(j);
int y = road.getPoint31YTile(j);
double distOnRoadToPass = squareRootDist(x, y, middlex, middley);
double distToFinalPoint = squareRootDist(x, y, targetEndX, targetEndY);
processIntersectionsWithWays(ctx, graphSegments, visitedSegments, distOnRoadToPass, distToFinalPoint,
segment, road, d == 0, j, next);
}
}
}
@ -383,6 +286,99 @@ public class BinaryRoutePlanner {
private void processIntersectionsWithWays(RoutingContext ctx, PriorityQueue<RouteSegment> graphSegments,
TLongHashSet visitedSegments, double distOnRoadToPass, double distToFinalPoint,
RouteSegment segment, BinaryMapDataObject road, boolean firstOfSegment, int segmentEnd, RouteSegment next) {
// 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 = next;
while (possibleObstacle != null) {
obstaclesTime += ctx.router.defineObstacle(possibleObstacle.road, possibleObstacle.segmentStart);
possibleObstacle = possibleObstacle.next;
}
}
// 3.2 calculate possible ways to put into priority queue
while (next != null) {
long nts = (next.road.getId() << 8l) + next.segmentStart;
/* next.road.id >> 1 != road.id >> 1 - used that line for debug with osm map */
// road.id could be equal on roundabout, but we should accept them
if (!visitedSegments.contains(nts)) {
int type = -1;
for (int i = 0; i < road.getRestrictionCount(); i++) {
if (road.getRestriction(i) == next.road.getId()) {
type = road.getRestrictionType(i);
break;
}
}
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 {
double distanceToEnd = distToFinalPoint / ctx.router.getMaxDefaultSpeed();
// Using A* routing algorithm
// g(x) - calculate distance to that point and calculate time
double speed = ctx.router.defineSpeed(road);
if (speed == 0) {
speed = ctx.router.getMinDefaultSpeed();
}
double distanceFromStart = segment.distanceFromStart + distOnRoadToPass / speed;
// calculate turn time
distanceFromStart += ctx.router.calculateTurnTime(segment, next, segmentEnd);
// add obstacles time
distanceFromStart += obstaclesTime;
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, ...)
exclusiveRestriction = true;
segmentsToVisitNotForbidden.clear();
segmentsToVisitPrescripted.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);
}
}
private List<RouteSegmentResult> prepareResult(RoutingContext ctx, RouteSegment start, RouteSegment end, long startNanoTime,
RouteSegment finalRoute) {
List<RouteSegmentResult> result = new ArrayList<RouteSegmentResult>();
@ -518,21 +514,32 @@ public class BinaryRoutePlanner {
public static class RoutingContext {
TLongObjectMap<BinaryMapDataObject> idObjects = new TLongObjectHashMap<BinaryMapDataObject>();
TLongObjectMap<RouteSegment> routes = new TLongObjectHashMap<RouteSegment>();
// parameters of routing
public int heuristicCoefficient = DEFAULT_HEURISTIC_COEFFICIENT;
public VehicleRouter router = new CarRouter();
//
TLongObjectMap<BinaryMapDataObject> idObjects = new TLongObjectHashMap<BinaryMapDataObject>();
TLongObjectMap<RouteSegment> routes = new TLongObjectHashMap<RouteSegment>();
TIntSet loadedTiles = new TIntHashSet();
// set collection to not null to monitor visited ways
public RouteSegmentVisitor visitor = null;
// debug information
long timeToLoad = 0;
long timeToCalculate = 0;
int visitedSegments = 0;
// callback of processing segments
public RouteSegmentVisitor visitor = null;
public Collection<BinaryMapDataObject> values(){
return idObjects.valueCollection();
}
public int roadPriorityComparator(double o1DistanceFromStart, double o1DistanceToEnd,
double o2DistanceFromStart, double o2DistanceToEnd) {
// f(x) = g(x) + h(x) --- g(x) - distanceFromStart, h(x) - distanceToEnd (not exact)
return Double.compare(o1DistanceFromStart + heuristicCoefficient * o1DistanceToEnd,
o2DistanceFromStart + heuristicCoefficient * o2DistanceToEnd);
}
}
public static class RouteSegment {

View file

@ -9,145 +9,140 @@ import net.osmand.osm.MapRenderingTypes;
import net.osmand.router.BinaryRoutePlanner.RouteSegment;
public class CarRouter extends VehicleRouter {
// no distinguish for speed in city/outside city (for now)
private Map<String, Double> autoNotDefinedValues = new LinkedHashMap<String, Double>();
private Map<String, Double> autoPriorityValues = new LinkedHashMap<String, Double>();
{
autoNotDefinedValues.put("motorway", 110d);
autoNotDefinedValues.put("motorway_link", 80d);
autoNotDefinedValues.put("trunk", 100d);
autoNotDefinedValues.put("trunk_link", 80d);
autoNotDefinedValues.put("primary", 65d);
autoNotDefinedValues.put("primary_link", 45d);
autoNotDefinedValues.put("secondary", 50d);
autoNotDefinedValues.put("secondary_link", 40d);
autoNotDefinedValues.put("tertiary", 35d);
autoNotDefinedValues.put("tertiary_link", 30d);
autoNotDefinedValues.put("residential", 30d);
autoNotDefinedValues.put("road", 30d);
autoNotDefinedValues.put("service", 20d);
autoNotDefinedValues.put("unclassified", 20d);
autoNotDefinedValues.put("track", 20d);
autoNotDefinedValues.put("path", 20d);
autoNotDefinedValues.put("living_street", 20d);
autoPriorityValues.put("motorway", 1.5);
autoPriorityValues.put("motorway_link", 1.0);
autoPriorityValues.put("trunk", 1.5);
autoPriorityValues.put("trunk_link", 1d);
autoPriorityValues.put("primary", 1.3d);
autoPriorityValues.put("primary_link", 1d);
autoPriorityValues.put("secondary", 1.0d);
autoPriorityValues.put("secondary_link", 1.0d);
autoPriorityValues.put("tertiary", 1.0d);
autoPriorityValues.put("tertiary_link", 1.0d);
autoPriorityValues.put("residential", 0.8d);
autoPriorityValues.put("service", 0.6d);
autoPriorityValues.put("unclassified", 0.4d);
autoPriorityValues.put("road", 0.4d);
autoPriorityValues.put("track", 0.1d);
autoPriorityValues.put("path", 0.1d);
autoPriorityValues.put("living_street", 0.5d);
}
public boolean acceptLine(TagValuePair pair){
if(pair.tag.equals("highway")){
return autoNotDefinedValues.containsKey(pair.value);
}
return false;
}
public boolean acceptPoint(TagValuePair pair){
if(pair.tag.equals("traffic_calming")){
return true;
} else if(pair.tag.equals("highway") && pair.value.equals("traffic_signals")){
return true;
} else if(pair.tag.equals("highway") && pair.value.equals("speed_camera")){
return true;
} else if(pair.tag.equals("railway") && pair.value.equals("crossing")){
return true;
} else if(pair.tag.equals("railway") && pair.value.equals("level_crossing")){
return true;
}
return false;
}
public boolean isOneWay(int highwayAttributes){
return MapRenderingTypes.isOneWayWay(highwayAttributes) ||
MapRenderingTypes.isRoundabout(highwayAttributes);
}
/**
* return delay in seconds
*/
public double defineObstacle(BinaryMapDataObject road, int point) {
if ((road.getTypes()[0] & 3) == MapRenderingTypes.POINT_TYPE) {
// possibly not only first type needed ?
TagValuePair pair = road.getTagValue(0);
if (pair != null) {
if(pair.tag.equals("highway") && pair.value.equals("traffic_signals")){
return 20;
} else if(pair.tag.equals("railway") && pair.value.equals("crossing")){
return 25;
} else if(pair.tag.equals("railway") && pair.value.equals("level_crossing")){
return 25;
}
}
}
return 0;
}
/**
* return speed in m/s
*/
public double defineSpeed(BinaryMapDataObject road) {
TagValuePair pair = road.getTagValue(0);
double speed = MapRenderingTypes.getMaxSpeedIfDefined(road.getHighwayAttributes()) / 3.6d;
boolean highway = "highway".equals(pair.tag);
double priority = highway && autoPriorityValues.containsKey(pair.value) ? autoPriorityValues.get(pair.value) : 1d;
if(speed == 0 && highway) {
Double value = autoNotDefinedValues.get(pair.value);
if(value == null){
value = 50d;
}
speed = value / 3.6d;
}
return speed * priority;
}
/**
* Used for A* routing to calculate g(x)
* @return minimal speed at road
*/
public double getMinDefaultSpeed() {
return 9;
}
/**
* Used for A* routing to predict h(x) : it should be < (!) any g(x)
* @return maximum speed to calculate shortest distance
*/
public double getMaxDefaultSpeed() {
return 30;
}
public double calculateTurnTime(int middley, int middlex, int x, int y, RouteSegment segment, RouteSegment next, int j) {
boolean lineAreNotConnected = j < segment.road.getPointsLength() - 1 || next.segmentStart != 0;
if(lineAreNotConnected){
return 25;
} else {
if (next.road.getPointsLength() > 1) {
double a1 = Math.atan2(y - middley, x - middlex);
double a2 = Math.atan2(y - next.road.getPoint31YTile(1), x - next.road.getPoint31XTile(1));
double diff = Math.abs(a1 - a2);
if (diff > Math.PI / 2 && diff < 3 * Math.PI / 2) {
return 25;
}
}
}
return 0;
}
// no distinguish for speed in city/outside city (for now)
private Map<String, Double> autoNotDefinedValues = new LinkedHashMap<String, Double>();
private Map<String, Double> autoPriorityValues = new LinkedHashMap<String, Double>();
{
autoNotDefinedValues.put("motorway", 110d);
autoNotDefinedValues.put("motorway_link", 80d);
autoNotDefinedValues.put("trunk", 100d);
autoNotDefinedValues.put("trunk_link", 80d);
autoNotDefinedValues.put("primary", 65d);
autoNotDefinedValues.put("primary_link", 45d);
autoNotDefinedValues.put("secondary", 50d);
autoNotDefinedValues.put("secondary_link", 40d);
autoNotDefinedValues.put("tertiary", 35d);
autoNotDefinedValues.put("tertiary_link", 30d);
autoNotDefinedValues.put("residential", 30d);
autoNotDefinedValues.put("road", 30d);
autoNotDefinedValues.put("service", 20d);
autoNotDefinedValues.put("unclassified", 20d);
autoNotDefinedValues.put("track", 20d);
autoNotDefinedValues.put("path", 20d);
autoNotDefinedValues.put("living_street", 20d);
autoPriorityValues.put("motorway", 1.5);
autoPriorityValues.put("motorway_link", 1.0);
autoPriorityValues.put("trunk", 1.5);
autoPriorityValues.put("trunk_link", 1d);
autoPriorityValues.put("primary", 1.3d);
autoPriorityValues.put("primary_link", 1d);
autoPriorityValues.put("secondary", 1.0d);
autoPriorityValues.put("secondary_link", 1.0d);
autoPriorityValues.put("tertiary", 1.0d);
autoPriorityValues.put("tertiary_link", 1.0d);
autoPriorityValues.put("residential", 0.8d);
autoPriorityValues.put("service", 0.6d);
autoPriorityValues.put("unclassified", 0.4d);
autoPriorityValues.put("road", 0.4d);
autoPriorityValues.put("track", 0.1d);
autoPriorityValues.put("path", 0.1d);
autoPriorityValues.put("living_street", 0.5d);
}
public boolean acceptLine(TagValuePair pair) {
if (pair.tag.equals("highway")) {
return autoNotDefinedValues.containsKey(pair.value);
}
return false;
}
public boolean acceptPoint(TagValuePair pair) {
if (pair.tag.equals("traffic_calming")) {
return true;
} else if (pair.tag.equals("highway") && pair.value.equals("traffic_signals")) {
return true;
} else if (pair.tag.equals("highway") && pair.value.equals("speed_camera")) {
return true;
} else if (pair.tag.equals("railway") && pair.value.equals("crossing")) {
return true;
} else if (pair.tag.equals("railway") && pair.value.equals("level_crossing")) {
return true;
}
return false;
}
public boolean isOneWay(int highwayAttributes) {
return MapRenderingTypes.isOneWayWay(highwayAttributes) || MapRenderingTypes.isRoundabout(highwayAttributes);
}
/**
* return delay in seconds
*/
public double defineObstacle(BinaryMapDataObject road, int point) {
if ((road.getTypes()[0] & 3) == MapRenderingTypes.POINT_TYPE) {
// possibly not only first type needed ?
TagValuePair pair = road.getTagValue(0);
if (pair != null) {
if (pair.tag.equals("highway") && pair.value.equals("traffic_signals")) {
return 20;
} else if (pair.tag.equals("railway") && pair.value.equals("crossing")) {
return 25;
} else if (pair.tag.equals("railway") && pair.value.equals("level_crossing")) {
return 25;
}
}
}
return 0;
}
/**
* return speed in m/s
*/
public double defineSpeed(BinaryMapDataObject road) {
TagValuePair pair = road.getTagValue(0);
double speed = MapRenderingTypes.getMaxSpeedIfDefined(road.getHighwayAttributes()) / 3.6d;
boolean highway = "highway".equals(pair.tag);
double priority = highway && autoPriorityValues.containsKey(pair.value) ? autoPriorityValues.get(pair.value) : 1d;
if (speed == 0 && highway) {
Double value = autoNotDefinedValues.get(pair.value);
if (value == null) {
value = 50d;
}
speed = value / 3.6d;
}
return speed * priority;
}
/**
* Used for A* routing to calculate g(x)
*
* @return minimal speed at road
*/
public double getMinDefaultSpeed() {
return 9;
}
/**
* Used for A* routing to predict h(x) : it should be < (!) any g(x)
*
* @return maximum speed to calculate shortest distance
*/
public double getMaxDefaultSpeed() {
return 30;
}
public double calculateTurnTime(RouteSegment segment, RouteSegment next, int segmentEnd) {
boolean end = (segmentEnd == segment.road.getPointsLength() - 1 || segmentEnd == 0);
boolean start = next.segmentStart == 0;
if (end) {
if(!start){
return 15;
}
return 0;
} else {
return 25;
}
}
}

View file

@ -144,7 +144,7 @@ public class PedestrianRouter extends VehicleRouter {
return 2;
}
public double calculateTurnTime(int middley, int middlex, int x, int y, RouteSegment segment, RouteSegment next, int j) {
public double calculateTurnTime(RouteSegment segment, RouteSegment next, int j) {
return 0;
}

View file

@ -58,5 +58,5 @@ public abstract class VehicleRouter {
/**
* Calculate turn time
*/
public abstract double calculateTurnTime(int middley, int middlex, int x, int y, RouteSegment segment, RouteSegment next, int j) ;
public abstract double calculateTurnTime(RouteSegment segment, RouteSegment next, int segmentEnd) ;
}