Test cpp routing
This commit is contained in:
Victor Shcherb
parent
5fb2bad2fd
commit
c65bd840b2
6 changed files with 704 additions and 3 deletions
|
|
@ -22,6 +22,7 @@ LOCAL_SRC_FILES := \
|
|||
src/renderRules.cpp \
|
||||
src/rendering.cpp \
|
||||
src/binaryRead.cpp \
|
||||
src/binaryRoutePlanner.cpp \
|
||||
src/proto/osmand_index.pb.cpp
|
||||
|
||||
ifdef OSMAND_PROFILE_NATIVE_OPERATIONS
|
||||
|
|
|
|||
627
Osmand-kernel/osmand/src/binaryRoutePlanner.cpp
Normal file
627
Osmand-kernel/osmand/src/binaryRoutePlanner.cpp
Normal file
|
|
@ -0,0 +1,627 @@
|
|||
#include "common.h"
|
||||
#include <queue>
|
||||
#include "binaryRead.h"
|
||||
#include <functional>
|
||||
|
||||
static bool PRINT_TO_CONSOLE_ROUTE_INFORMATION_TO_TEST = true;
|
||||
static const int REVERSE_WAY_RESTRICTION_ONLY = 1024;
|
||||
static const int STANDARD_ROAD_IN_QUEUE_OVERHEAD = 900;
|
||||
|
||||
static const int ROUTE_POINTS = 11;
|
||||
static const float TURN_DEGREE_MIN = 45;
|
||||
static const short RESTRICTION_NO_RIGHT_TURN = 1;
|
||||
static const short RESTRICTION_NO_LEFT_TURN = 2;
|
||||
static const short RESTRICTION_NO_U_TURN = 3;
|
||||
static const short RESTRICTION_NO_STRAIGHT_ON = 4;
|
||||
static const short RESTRICTION_ONLY_RIGHT_TURN = 5;
|
||||
static const short RESTRICTION_ONLY_LEFT_TURN = 6;
|
||||
static const short RESTRICTION_ONLY_STRAIGHT_ON = 7;
|
||||
|
||||
inline int roadPriorityComparator(float o1DistanceFromStart, float o1DistanceToEnd,
|
||||
float o2DistanceFromStart, float o2DistanceToEnd, float heuristicCoefficient) {
|
||||
// f(x) = g(x) + h(x) --- g(x) - distanceFromStart, h(x) - distanceToEnd (not exact)
|
||||
float f1 = o1DistanceFromStart + heuristicCoefficient * o1DistanceToEnd;
|
||||
float f2 = o2DistanceFromStart + heuristicCoefficient * o2DistanceToEnd;
|
||||
if(f1 == f2) {
|
||||
return 0;
|
||||
}
|
||||
return f1 < f2 ? -1 : 1;
|
||||
}
|
||||
struct RouteSegment {
|
||||
public :
|
||||
int segmentStart;
|
||||
SHARED_PTR<RouteDataObject> road;
|
||||
// needed to store intersection of routes
|
||||
SHARED_PTR<RouteSegment> next;
|
||||
|
||||
// search context (needed for searching route)
|
||||
// Initially it should be null (!) because it checks was it segment visited before
|
||||
SHARED_PTR<RouteSegment> parentRoute;
|
||||
int parentSegmentEnd;
|
||||
|
||||
// distance measured in time (seconds)
|
||||
float distanceFromStart;
|
||||
float distanceToEnd;
|
||||
|
||||
RouteSegment(SHARED_PTR<RouteDataObject> road, int segmentStart) : road(road), segmentStart(segmentStart),
|
||||
parentSegmentEnd(0), distanceFromStart(0), distanceToEnd(0){
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
// FIXME
|
||||
const static int ZOOM_TO_LOAD_TILES = 16;
|
||||
struct RoutingSubregionTile {
|
||||
RouteSubregion subregion;
|
||||
// make it without get/set for fast access
|
||||
int access;
|
||||
int loaded;
|
||||
UNORDERED(map)<long long, SHARED_PTR<RouteSegment> > routes;
|
||||
|
||||
RoutingSubregionTile():access(0), loaded(0) {
|
||||
|
||||
}
|
||||
bool isLoaded(){
|
||||
return loaded > 0;
|
||||
}
|
||||
};
|
||||
static long calcRouteId(SHARED_PTR<RouteDataObject> o, int ind) {
|
||||
return (o->id << 10) + ind;
|
||||
}
|
||||
|
||||
struct RoutingContext {
|
||||
int visitedSegments;
|
||||
ElapsedTimer timeToLoad;
|
||||
ElapsedTimer timeToCalculate;
|
||||
int firstRoadDirection;
|
||||
int64_t firstRoadId;
|
||||
|
||||
vector<SHARED_PTR<RouteSegment> > segmentsToVisitNotForbidden;
|
||||
vector<SHARED_PTR<RouteSegment> > segmentsToVisitPrescripted;
|
||||
|
||||
int finalReverseEndSegment;
|
||||
SHARED_PTR<RouteSegment> finalReverseRoute;
|
||||
int finalDirectEndSegment;
|
||||
SHARED_PTR<RouteSegment> finalDirectRoute;
|
||||
UNORDERED(map)<long long, vector<SHARED_PTR<RoutingSubregionTile> > > indexedSubregions;
|
||||
|
||||
// void searchRouteRegion(SearchQuery* q, std::vector<RouteDataObject*>& list, RoutingIndex* rs, RouteSubregion* sub)
|
||||
SHARED_PTR<RouteSegment> loadRouteSegment(int x31, int y31) {
|
||||
timeToLoad.start();
|
||||
int64_t xloc = x31 >> (31 - ZOOM_TO_LOAD_TILES);
|
||||
int64_t yloc = y31 >> (31 - ZOOM_TO_LOAD_TILES);
|
||||
int64_t tileId = (xloc << ZOOM_TO_LOAD_TILES) + yloc;
|
||||
// if (indexedSubregions[tileId] == indexedSubregions.end()) {
|
||||
// SearchQuery q((int) (xloc << zoomToLoad),
|
||||
// (int) ((xloc + 1) << zoomToLoad), (int) (yloc << zoomToLoad), (int) ((yloc + 1) << zoomToLoad));
|
||||
// vector<SHARED_PTR<RoutingSubregionTile> > collection = loadTileHeaders(x31, y31);
|
||||
// indexedSubregions.put(tileId, collection);
|
||||
// }
|
||||
// vector<SHARED_PTR<RoutingSubregionTile> > subregions = indexedSubregions.get(tileId);
|
||||
// if (subregions != null) {
|
||||
// for (RoutingSubregionTile ts : subregions) {
|
||||
// if (!ts.isLoaded()) {
|
||||
// loadSubregionTile(ts, loadOptions == OPTION_IN_MEMORY_LOAD);
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
timeToLoad.pause();
|
||||
return SHARED_PTR<RouteSegment>();
|
||||
}
|
||||
|
||||
bool isInterrupted(){
|
||||
return false;
|
||||
}
|
||||
float getHeuristicCoefficient(){
|
||||
return 1;
|
||||
}
|
||||
|
||||
bool planRouteIn2Directions() {
|
||||
return true;
|
||||
}
|
||||
int getPlanRoadDirection() {
|
||||
return 0;
|
||||
}
|
||||
|
||||
float defineSpeedPriority(SHARED_PTR<RouteDataObject> r) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
float defineSpeed(SHARED_PTR<RouteDataObject> r) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
float getMinDefaultSpeed(){
|
||||
return 40;
|
||||
}
|
||||
float getMaxDefaultSpeed(){
|
||||
return 130;
|
||||
}
|
||||
|
||||
bool isOneWay(SHARED_PTR<RouteDataObject> r) {
|
||||
return true;
|
||||
}
|
||||
float calculateTurnTime(SHARED_PTR<RouteSegment> segment, int index, SHARED_PTR<RouteSegment> next, int nextIndex){
|
||||
return 0;
|
||||
}
|
||||
float defineRoutingObstacle(SHARED_PTR<RouteDataObject> road, int segmentEnd) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool restrictionsAware(){
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
static double convert31YToMeters(int y1, int y2) {
|
||||
// translate into meters
|
||||
return (y1 - y2) * 0.01863f;
|
||||
}
|
||||
|
||||
static double convert31XToMeters(int x1, int x2) {
|
||||
// translate into meters
|
||||
return (x1 - x2) * 0.011f;
|
||||
}
|
||||
|
||||
// translate into meters
|
||||
static double squareRootDist(int x1, int y1, int x2, int y2) {
|
||||
double dy = convert31YToMeters(y1, y2);
|
||||
double dx = convert31XToMeters(x1, x2);
|
||||
return sqrt(dx * dx + dy * dy);
|
||||
// return measuredDist(x1, y1, x2, y2);
|
||||
}
|
||||
|
||||
static double measuredDist(int x1, int y1, int x2, int y2) {
|
||||
return getDistance(get31LatitudeY(y1), get31LongitudeX(x1), get31LatitudeY(y2),
|
||||
get31LongitudeX(x2));
|
||||
}
|
||||
|
||||
static double squareDist(int x1, int y1, int x2, int y2) {
|
||||
// translate into meters
|
||||
double dy = convert31YToMeters(y1, y2);
|
||||
double dx = convert31XToMeters(x1, x2);
|
||||
return dx * dx + dy * dy;
|
||||
}
|
||||
|
||||
static double h(RoutingContext* ctx, float distanceToFinalPoint, SHARED_PTR<RouteSegment> next) {
|
||||
return distanceToFinalPoint / ctx->getMaxDefaultSpeed();
|
||||
|
||||
}
|
||||
static double h(RoutingContext* ctx, int targetEndX, int targetEndY,
|
||||
int startX, int startY) {
|
||||
double distance = squareRootDist(startX, startY, targetEndX, targetEndY);
|
||||
return distance / ctx->getMaxDefaultSpeed();
|
||||
}
|
||||
|
||||
|
||||
struct SegmentsComparator : public std::binary_function<SHARED_PTR<RouteSegment>, SHARED_PTR<RouteSegment>, bool>
|
||||
{
|
||||
RoutingContext* ctx;
|
||||
SegmentsComparator(RoutingContext* c) : ctx(c){
|
||||
|
||||
}
|
||||
bool operator()(const SHARED_PTR<RouteSegment> lhs, const SHARED_PTR<RouteSegment> rhs) const
|
||||
{
|
||||
return roadPriorityComparator(lhs.get()->distanceFromStart, lhs.get()->distanceToEnd, rhs.get()->distanceFromStart, rhs.get()->distanceToEnd, ctx->getHeuristicCoefficient()) < 0;
|
||||
}
|
||||
};
|
||||
struct NonHeuristicSegmentsComparator : public std::binary_function<SHARED_PTR<RouteSegment>, SHARED_PTR<RouteSegment>, bool>
|
||||
{
|
||||
bool operator()(const SHARED_PTR<RouteSegment> lhs, const SHARED_PTR<RouteSegment> rhs) const
|
||||
{
|
||||
return roadPriorityComparator(lhs.get()->distanceFromStart, lhs.get()->distanceToEnd, rhs.get()->distanceFromStart, rhs.get()->distanceToEnd, 0.5) < 0;
|
||||
}
|
||||
};
|
||||
|
||||
typedef UNORDERED(map)<long long, SHARED_PTR<RouteSegment> > VISITED_MAP;
|
||||
typedef priority_queue<SHARED_PTR<RouteSegment>, vector<SHARED_PTR<RouteSegment> >, SegmentsComparator > SEGMENTS_QUEUE;
|
||||
bool processRouteSegment(RoutingContext* ctx, bool reverseWaySearch,
|
||||
SEGMENTS_QUEUE& graphSegments, VISITED_MAP& visitedSegments, int targetEndX, int targetEndY,
|
||||
SHARED_PTR<RouteSegment> segment, VISITED_MAP& oppositeSegments);
|
||||
bool processIntersections(RoutingContext* ctx, SEGMENTS_QUEUE& graphSegments,
|
||||
VISITED_MAP& visitedSegments, VISITED_MAP& oppositeSegments,
|
||||
double distFromStart, double distToFinalPoint,
|
||||
SHARED_PTR<RouteSegment> segment, int segmentEnd, SHARED_PTR<RouteSegment> inputNext,
|
||||
bool reverseWay);
|
||||
|
||||
|
||||
/**
|
||||
* Calculate route between start.segmentEnd and end.segmentStart (using A* algorithm)
|
||||
* return list of segments
|
||||
*/
|
||||
void searchRouteInternal(RoutingContext* ctx, SHARED_PTR<RouteSegment> start, SHARED_PTR<RouteSegment> end, bool leftSideNavigation) {
|
||||
// measure time
|
||||
ctx->visitedSegments = 0;
|
||||
ctx->timeToCalculate.start();
|
||||
// FIXME initial direction
|
||||
// if(ctx.config.initialDirection != null) {
|
||||
// ctx.firstRoadId = (start->road->id << ROUTE_POINTS) + start.getSegmentStart();
|
||||
// double plusDir = start->road->directionRoute(start.segmentStart, true);
|
||||
// double diff = plusDir - ctx.config.initialDirection;
|
||||
// if(Math.abs(MapUtils.alignAngleDifference(diff)) <= Math.PI / 3) {
|
||||
// ctx.firstRoadDirection = 1;
|
||||
// } else if(Math.abs(MapUtils.alignAngleDifference(diff - Math.PI)) <= Math.PI / 3) {
|
||||
// ctx.firstRoadDirection = -1;
|
||||
// }
|
||||
//
|
||||
// }
|
||||
|
||||
SegmentsComparator sgmCmp(ctx);
|
||||
SEGMENTS_QUEUE graphDirectSegments(sgmCmp);
|
||||
SEGMENTS_QUEUE graphReverseSegments(sgmCmp);
|
||||
|
||||
// Set to not visit one segment twice (stores road.id << X + segmentStart)
|
||||
VISITED_MAP visitedDirectSegments ;
|
||||
VISITED_MAP visitedOppositeSegments;
|
||||
|
||||
// FIXME run recalculation
|
||||
bool runRecalculation = false;
|
||||
|
||||
// for start : f(start) = g(start) + h(start) = 0 + h(start) = h(start)
|
||||
int targetEndX = end->road->pointsX[end->segmentStart];
|
||||
int targetEndY = end->road->pointsY[end->segmentStart];
|
||||
int startX = start->road->pointsX[start->segmentStart];
|
||||
int startY = start->road->pointsY[start->segmentStart];
|
||||
float estimatedDistance = (float) h(ctx, targetEndX, targetEndY, startX, startY);
|
||||
end->distanceToEnd = start->distanceToEnd = estimatedDistance;
|
||||
|
||||
graphDirectSegments.push(start);
|
||||
graphReverseSegments.push(end);
|
||||
|
||||
// Extract & analyze segment with min(f(x)) from queue while final segment is not found
|
||||
bool inverse = false;
|
||||
bool init = false;
|
||||
|
||||
NonHeuristicSegmentsComparator nonHeuristicSegmentsComparator;
|
||||
SEGMENTS_QUEUE * graphSegments;
|
||||
if(inverse) {
|
||||
graphSegments = &graphReverseSegments;
|
||||
} else {
|
||||
graphSegments = &graphDirectSegments;
|
||||
}
|
||||
while (graphSegments->size() > 0) {
|
||||
SHARED_PTR<RouteSegment> segment = graphSegments->top();
|
||||
graphSegments->pop();
|
||||
ctx->visitedSegments++;
|
||||
bool routeFound = false;
|
||||
if (!inverse) {
|
||||
routeFound = processRouteSegment(ctx, false, graphDirectSegments, visitedDirectSegments, targetEndX, targetEndY,
|
||||
segment, visitedOppositeSegments);
|
||||
} else {
|
||||
routeFound = processRouteSegment(ctx, true, graphReverseSegments, visitedOppositeSegments, startX, startY, segment,
|
||||
visitedDirectSegments);
|
||||
}
|
||||
if (graphReverseSegments.size() == 0 || graphDirectSegments.size() == 0 || routeFound) {
|
||||
break;
|
||||
}
|
||||
if(runRecalculation) {
|
||||
// nothing to do
|
||||
inverse = false;
|
||||
} else if (!init) {
|
||||
inverse = !inverse;
|
||||
init = true;
|
||||
} else if (ctx->planRouteIn2Directions()) {
|
||||
inverse = !nonHeuristicSegmentsComparator(graphDirectSegments.top(), graphReverseSegments.top());
|
||||
if (graphDirectSegments.size() * 1.3 > graphReverseSegments.size()) {
|
||||
inverse = true;
|
||||
} else if (graphDirectSegments.size() < 1.3 * graphReverseSegments.size()) {
|
||||
inverse = false;
|
||||
}
|
||||
} else {
|
||||
// different strategy : use onedirectional graph
|
||||
inverse = ctx->getPlanRoadDirection() < 0;
|
||||
}
|
||||
if (inverse) {
|
||||
graphSegments = &graphReverseSegments;
|
||||
} else {
|
||||
graphSegments = &graphDirectSegments;
|
||||
}
|
||||
|
||||
// check if interrupted
|
||||
if(ctx->isInterrupted()) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
ctx->timeToCalculate.pause();
|
||||
}
|
||||
bool processRouteSegment(RoutingContext* ctx, bool reverseWaySearch,
|
||||
SEGMENTS_QUEUE& graphSegments, VISITED_MAP& visitedSegments, int targetEndX, int targetEndY,
|
||||
SHARED_PTR<RouteSegment> segment, VISITED_MAP& oppositeSegments) {
|
||||
// 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
|
||||
SHARED_PTR<RouteDataObject> road = segment->road;
|
||||
int middle = segment->segmentStart;
|
||||
double obstaclePlusTime = 0;
|
||||
double obstacleMinusTime = 0;
|
||||
|
||||
// 0. mark route segment as visited
|
||||
long nt = (road->id << ROUTE_POINTS) + middle;
|
||||
// avoid empty segments to connect but mark the point as visited
|
||||
visitedSegments[nt] =SHARED_PTR<RouteSegment>();
|
||||
|
||||
int oneway = ctx->isOneWay(road);
|
||||
bool minusAllowed;
|
||||
bool plusAllowed;
|
||||
if(ctx->firstRoadId == nt) {
|
||||
minusAllowed = ctx->firstRoadDirection <= 0;
|
||||
plusAllowed = ctx->firstRoadDirection >= 0;
|
||||
} else if (!reverseWaySearch) {
|
||||
minusAllowed = oneway <= 0;
|
||||
plusAllowed = oneway >= 0;
|
||||
} else {
|
||||
minusAllowed = oneway >= 0;
|
||||
plusAllowed = oneway <= 0;
|
||||
}
|
||||
|
||||
// +/- diff from middle point
|
||||
int d = plusAllowed ? 1 : -1;
|
||||
if(segment->parentRoute.get() != NULL) {
|
||||
if(plusAllowed && middle < segment->road->pointsX.size() - 1) {
|
||||
obstaclePlusTime = ctx->calculateTurnTime(segment, segment->road->pointsX.size() - 1,
|
||||
segment->parentRoute, segment->parentSegmentEnd);
|
||||
}
|
||||
if(minusAllowed && middle > 0) {
|
||||
obstacleMinusTime = ctx->calculateTurnTime(segment, 0,
|
||||
segment->parentRoute, segment->parentSegmentEnd);
|
||||
}
|
||||
}
|
||||
// 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)
|
||||
double posSegmentDist = 0;
|
||||
double negSegmentDist = 0;
|
||||
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;
|
||||
bool positive = d > 0;
|
||||
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->pointsX.size()) {
|
||||
plusAllowed = false;
|
||||
continue;
|
||||
}
|
||||
// if we found end point break cycle
|
||||
long nts = (road->id << ROUTE_POINTS) + segmentEnd;
|
||||
visitedSegments[nts]=segment;
|
||||
|
||||
// 2. calculate point and try to load neighbor ways if they are not loaded
|
||||
int x = road->pointsX[segmentEnd];
|
||||
int y = road->pointsY[segmentEnd];
|
||||
if(positive) {
|
||||
posSegmentDist += squareRootDist(x, y,
|
||||
road->pointsX[segmentEnd - 1], road->pointsY[segmentEnd - 1]);
|
||||
} else {
|
||||
negSegmentDist += squareRootDist(x, y,
|
||||
road->pointsX[segmentEnd + 1], road->pointsY[segmentEnd + 1]);
|
||||
}
|
||||
|
||||
// 2.1 calculate possible obstacle plus time
|
||||
if(positive) {
|
||||
double obstacle = ctx->defineRoutingObstacle(road, segmentEnd);
|
||||
if (obstacle < 0) {
|
||||
plusAllowed = false;
|
||||
continue;
|
||||
}
|
||||
obstaclePlusTime += obstacle;
|
||||
} else {
|
||||
double obstacle = ctx->defineRoutingObstacle(road, segmentEnd);
|
||||
if (obstacle < 0) {
|
||||
minusAllowed = false;
|
||||
continue;
|
||||
}
|
||||
obstacleMinusTime += obstacle;
|
||||
}
|
||||
// could be expensive calculation
|
||||
// FIXME memory check
|
||||
// int overhead = (ctx.visitedSegments - ctx.relaxedSegments ) *
|
||||
// STANDARD_ROAD_IN_QUEUE_OVERHEAD;
|
||||
// if(overhead > ctx.config.memoryLimitation * 0.95) {
|
||||
// throw new OutOfMemoryError("There is no enough memory " + ctx.config.memoryLimitation/(1<<20) + " Mb");
|
||||
// }
|
||||
SHARED_PTR<RouteSegment> next = ctx->loadRouteSegment(x, y);
|
||||
// 3. get intersected ways
|
||||
if (next.get() != NULL) {
|
||||
// TO-DO U-Turn
|
||||
if((next.get() == segment.get() || next->road->id == road->id) && next->next.get() == NULL) {
|
||||
// simplification if there is no real intersection
|
||||
continue;
|
||||
}
|
||||
// Using A* routing algorithm
|
||||
// g(x) - calculate distance to that point and calculate time
|
||||
|
||||
double priority = ctx->defineSpeedPriority(road);
|
||||
double speed = ctx->defineSpeed(road) * priority;
|
||||
if (speed == 0) {
|
||||
speed = ctx->getMinDefaultSpeed() * priority;
|
||||
}
|
||||
double distOnRoadToPass = positive? posSegmentDist : negSegmentDist;
|
||||
double distStartObstacles = segment->distanceFromStart + ( positive ? obstaclePlusTime : obstacleMinusTime) + distOnRoadToPass / speed;
|
||||
|
||||
double distToFinalPoint = squareRootDist(x, y, targetEndX, targetEndY);
|
||||
bool routeFound = processIntersections(ctx, graphSegments, visitedSegments, oppositeSegments,
|
||||
distStartObstacles, distToFinalPoint, segment, segmentEnd, next, reverseWaySearch);
|
||||
if(routeFound) {
|
||||
return routeFound;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool proccessRestrictions(RoutingContext* ctx, SHARED_PTR<RouteDataObject> road, SHARED_PTR<RouteSegment> inputNext, bool reverseWay) {
|
||||
ctx->segmentsToVisitPrescripted.clear();
|
||||
ctx->segmentsToVisitNotForbidden.clear();
|
||||
bool exclusiveRestriction = false;
|
||||
SHARED_PTR<RouteSegment> next = inputNext;
|
||||
|
||||
if (!reverseWay && road->restrictions.size() == 0) {
|
||||
return false;
|
||||
}
|
||||
if(!ctx->restrictionsAware()) {
|
||||
return false;
|
||||
}
|
||||
while (next.get() != NULL) {
|
||||
int type = -1;
|
||||
if (!reverseWay) {
|
||||
for (int i = 0; i < road->restrictions.size(); i++) {
|
||||
if ((road->restrictions[i] >> 3) == next->road->id) {
|
||||
type = road->restrictions[i] & 7;
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
for (int i = 0; i < next->road->restrictions.size(); i++) {
|
||||
int rt = next->road->restrictions[i] & 7;
|
||||
long restrictedTo = next->road->restrictions[i] >> 3;
|
||||
if (restrictedTo == road->id) {
|
||||
type = rt;
|
||||
break;
|
||||
}
|
||||
|
||||
// Check if there is restriction only to the other than current road
|
||||
if (rt == RESTRICTION_ONLY_RIGHT_TURN || rt == RESTRICTION_ONLY_LEFT_TURN
|
||||
|| rt == RESTRICTION_ONLY_STRAIGHT_ON) {
|
||||
// check if that restriction applies to considered junk
|
||||
SHARED_PTR<RouteSegment> foundNext = inputNext;
|
||||
while (foundNext.get() != NULL) {
|
||||
if (foundNext->road->id == restrictedTo) {
|
||||
break;
|
||||
}
|
||||
foundNext = foundNext->next;
|
||||
}
|
||||
if (foundNext.get() != 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 == RESTRICTION_NO_LEFT_TURN || type == RESTRICTION_NO_RIGHT_TURN
|
||||
|| type == RESTRICTION_NO_STRAIGHT_ON || type == RESTRICTION_NO_U_TURN) {
|
||||
// next = next.next; continue;
|
||||
} else if (type == -1) {
|
||||
// case no restriction
|
||||
ctx->segmentsToVisitNotForbidden.push_back(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 many "in" roads and one "out"
|
||||
// 2. in case we are going forward we have one "in" and many "out"
|
||||
if (!reverseWay) {
|
||||
exclusiveRestriction = true;
|
||||
ctx->segmentsToVisitNotForbidden.clear();
|
||||
ctx->segmentsToVisitPrescripted.push_back(next);
|
||||
} else {
|
||||
ctx->segmentsToVisitNotForbidden.push_back(next);
|
||||
}
|
||||
}
|
||||
next = next->next;
|
||||
}
|
||||
ctx->segmentsToVisitPrescripted.insert(ctx->segmentsToVisitPrescripted.end(), ctx->segmentsToVisitNotForbidden.begin(), ctx->segmentsToVisitNotForbidden.end());
|
||||
return true;
|
||||
}
|
||||
|
||||
bool processIntersections(RoutingContext* ctx, SEGMENTS_QUEUE& graphSegments,
|
||||
VISITED_MAP& visitedSegments, VISITED_MAP& oppositeSegments,
|
||||
double distFromStart, double distToFinalPoint,
|
||||
SHARED_PTR<RouteSegment> segment, int segmentEnd, SHARED_PTR<RouteSegment> inputNext,
|
||||
bool reverseWay) {
|
||||
bool thereAreRestrictions = proccessRestrictions(ctx, segment->road, inputNext, reverseWay);
|
||||
vector<SHARED_PTR<RouteSegment> >::iterator nextIterator;
|
||||
if (thereAreRestrictions) {
|
||||
nextIterator = ctx->segmentsToVisitPrescripted.begin();
|
||||
}
|
||||
// Calculate possible ways to put into priority queue
|
||||
SHARED_PTR<RouteSegment> next = inputNext;
|
||||
bool hasNext = !thereAreRestrictions || nextIterator != ctx->segmentsToVisitPrescripted.end();
|
||||
while (hasNext) {
|
||||
if (thereAreRestrictions) {
|
||||
next = *nextIterator;
|
||||
}
|
||||
long nts = (next->road->id << ROUTE_POINTS) + next->segmentStart;
|
||||
|
||||
// 1. Check if opposite segment found so we can stop calculations
|
||||
if (oppositeSegments[nts].get() != NULL) {
|
||||
// restrictions checked
|
||||
SHARED_PTR<RouteSegment> opposite = oppositeSegments[nts];
|
||||
// additional check if opposite way not the same as current one
|
||||
if (next->segmentStart != segmentEnd ||
|
||||
opposite->road->id != segment->road->id) {
|
||||
if (reverseWay) {
|
||||
ctx->finalReverseEndSegment = segmentEnd;
|
||||
ctx->finalReverseRoute = segment;
|
||||
ctx->finalDirectEndSegment = next->segmentStart;
|
||||
ctx->finalDirectRoute = opposite;
|
||||
} else {
|
||||
ctx->finalDirectEndSegment = segmentEnd;
|
||||
ctx->finalDirectRoute = segment;
|
||||
ctx->finalReverseEndSegment = next->segmentStart;
|
||||
ctx->finalReverseRoute = opposite;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
// road.id could be equal on roundabout, but we should accept them
|
||||
bool alreadyVisited = visitedSegments.find(nts) != visitedSegments.end();
|
||||
if (!alreadyVisited) {
|
||||
double distanceToEnd = h(ctx, distToFinalPoint, next);
|
||||
if (next->parentRoute.get() == NULL
|
||||
|| roadPriorityComparator(next->distanceFromStart, next->distanceToEnd, distFromStart, distanceToEnd,
|
||||
ctx->getHeuristicCoefficient()) > 0) {
|
||||
if (next->parentRoute.get() != NULL) {
|
||||
// already in queue remove it
|
||||
// FIXME remove
|
||||
// if (!graphSegments.remove(next))
|
||||
{
|
||||
// exist in different queue!
|
||||
RouteSegment* cpy = new RouteSegment(next->road, next->segmentStart);
|
||||
next = SHARED_PTR<RouteSegment>(cpy);
|
||||
}
|
||||
}
|
||||
next->distanceFromStart = distFromStart;
|
||||
next->distanceToEnd = distanceToEnd;
|
||||
// put additional information to recover whole route after
|
||||
next->parentRoute = segment;
|
||||
next->parentSegmentEnd = segmentEnd;
|
||||
graphSegments.push(next);
|
||||
}
|
||||
} else {
|
||||
// the segment was already visited! We need to follow better route if it exists
|
||||
// that is very strange situation and almost exception (it can happen when we underestimate distnceToEnd)
|
||||
if (distFromStart < next->distanceFromStart && next->road->id != segment->road->id) {
|
||||
next->distanceFromStart = distFromStart;
|
||||
next->parentRoute = segment;
|
||||
next->parentSegmentEnd = segmentEnd;
|
||||
}
|
||||
}
|
||||
|
||||
// iterate to next road
|
||||
if (thereAreRestrictions) {
|
||||
nextIterator++;
|
||||
hasNext = nextIterator != ctx->segmentsToVisitPrescripted.end();
|
||||
} else {
|
||||
next = next->next;
|
||||
hasNext = next.get() != NULL;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
6
Osmand-kernel/osmand/src/binaryRoutePlanner.h
Normal file
6
Osmand-kernel/osmand/src/binaryRoutePlanner.h
Normal file
|
|
@ -0,0 +1,6 @@
|
|||
#ifndef _OSMAND_BINARY_ROUTE_PLANNER_H
|
||||
#define _OSMAND_BINARY_ROUTE_PLANNER_H
|
||||
#include "common.h"
|
||||
#include "binaryRead.h"
|
||||
|
||||
#endif /*_OSMAND_BINARY_ROUTE_PLANNER_H*/
|
||||
|
|
@ -3,7 +3,6 @@
|
|||
#include <SkPath.h>
|
||||
#include <SkBitmap.h>
|
||||
#include <SkImageDecoder.h>
|
||||
|
||||
#include "osmand_log.h"
|
||||
|
||||
#if defined(_WIN32)
|
||||
|
|
@ -15,6 +14,7 @@
|
|||
# include <time.h>
|
||||
#endif
|
||||
|
||||
|
||||
TextDrawInfo::TextDrawInfo(std::string itext)
|
||||
: text(itext)
|
||||
, drawOnPath(false)
|
||||
|
|
@ -268,4 +268,12 @@ double getTileNumberY(float zoom, double latitude) {
|
|||
return result;
|
||||
}
|
||||
|
||||
|
||||
double getDistance(double lat1, double lon1, double lat2, double lon2) {
|
||||
double R = 6371; // km
|
||||
double dLat = toRadians(lat2 - lat1);
|
||||
double dLon = toRadians(lon2 - lon1);
|
||||
double a = sin(dLat / 2) * sin(dLat / 2)
|
||||
+ cos(toRadians(lat1)) * cos(toRadians(lat2)) * sin(dLon / 2) * sin(dLon / 2);
|
||||
double c = 2 * atan2(sqrt(a), sqrt(1 - a));
|
||||
return R * c * 1000;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -50,6 +50,16 @@
|
|||
#endif
|
||||
#define UNORDERED(cls) UNORDERED_NAMESPACE::UNORDERED_##cls
|
||||
|
||||
#if defined(ANDROID)
|
||||
# include "shared_ptr.h"
|
||||
# define SHARED_PTR my_shared_ptr
|
||||
#elif defined(WINDOWS)
|
||||
#else
|
||||
# include <tr1/memory>
|
||||
# define SHARED_PTR std::tr1::shared_ptr
|
||||
#endif
|
||||
|
||||
|
||||
// Better don't do this
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -303,7 +313,7 @@ double get31LongitudeX(int tileX);
|
|||
double get31LatitudeY(int tileY);
|
||||
double getTileNumberX(float zoom, double longitude);
|
||||
double getTileNumberY(float zoom, double latitude);
|
||||
|
||||
double getDistance(double lat1, double lon1, double lat2, double lon2);
|
||||
double getPowZoom(float zoom);
|
||||
|
||||
#endif /*_OSMAND_COMMON_H*/
|
||||
|
|
|
|||
49
Osmand-kernel/osmand/src/shared_ptr.h
Normal file
49
Osmand-kernel/osmand/src/shared_ptr.h
Normal file
|
|
@ -0,0 +1,49 @@
|
|||
// Simple shared_ptr implementation based on http://stackoverflow.com/questions/1512520/decent-shared-ptr-implementation-that-does-not-require-a-massive-library
|
||||
template <typename contained>
|
||||
class my_shared_ptr {
|
||||
public:
|
||||
my_shared_ptr() : ptr_(NULL), ref_count_(NULL) { }
|
||||
|
||||
my_shared_ptr(contained * p)
|
||||
: ptr_(p), ref_count_(p ? new int : NULL)
|
||||
{ inc_ref(); }
|
||||
|
||||
my_shared_ptr(const my_shared_ptr& rhs)
|
||||
: ptr_(rhs.ptr_), ref_count_(rhs.ref_count_)
|
||||
{ inc_ref(); }
|
||||
|
||||
~my_shared_ptr() {
|
||||
if(ref_count_ && 0 == dec_ref()) { delete ptr_; delete ref_count_; }
|
||||
}
|
||||
contained * get() { return ptr_; }
|
||||
const contained * get() const { return ptr_; }
|
||||
|
||||
void swap(my_shared_ptr& rhs) // throw()
|
||||
{
|
||||
std::swap(ptr_, rhs.ptr_);
|
||||
std::swap(ref_count_, rhs.ref_count_);
|
||||
}
|
||||
|
||||
my_shared_ptr& operator=(const my_shared_ptr& rhs) {
|
||||
my_shared_ptr tmp(rhs);
|
||||
this->swap(tmp);
|
||||
return *this;
|
||||
}
|
||||
|
||||
contained * operator->() {
|
||||
return this->ptr_;
|
||||
}
|
||||
|
||||
// operator->, operator*, operator void*, use_count
|
||||
private:
|
||||
void inc_ref() {
|
||||
if(ref_count_) { ++(*ref_count_); }
|
||||
}
|
||||
|
||||
int dec_ref() {
|
||||
return --(*ref_count_);
|
||||
}
|
||||
|
||||
contained * ptr_;
|
||||
int * ref_count_;
|
||||
};
|
||||
Loading…
Reference in a new issue