Update native routing

DataExtractionOSM/src/net/osmand/swing/MapRouterLayer.java

@@ -722,7 +722,7 @@ public class MapRouterLayer implements MapPanelLayer {
 				int sy31 = st.getRoad().getPoint31YTile(st.getSegmentStart());
 //				 FIXME
 				RouteSegmentResult[] searchRoute = ctx.nativeLib.runNativeRouting(sx31, sy31, ex31, ey31, ctx.config);
-				/*List<RouteSegmentResult> searchRoute = */router.searchRoute(ctx, st, e, inters, false);
+//				/*List<RouteSegmentResult> searchRoute = */router.searchRoute(ctx, st, e, inters, false);
 //				this.previousRoute = searchRoute;
 				if (animateRoutingCalculation) {
 					playPauseButton.setVisible(false);

Osmand-kernel/osmand/src/binaryRoutePlanner.cpp

@@ -31,16 +31,6 @@ inline int roadPriorityComparator(float o1DistanceFromStart, float o1DistanceToE
 }
 
 
-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);
@@ -497,9 +487,53 @@ bool processIntersections(RoutingContext* ctx, SEGMENTS_QUEUE& graphSegments,
 	return false;
 }
 
-// FIXME replace with adequate method
+
 SHARED_PTR<RouteSegment> findRouteSegment(int px, int py, RoutingContext* ctx) {
-	return ctx->loadSegmentAround(px, py);
+	vector<SHARED_PTR<RouteDataObject> > dataObjects;
+	ctx->loadTileData(px, py, 16, dataObjects);
+	if (dataObjects.size() == 0) {
+		ctx->loadTileData(px, py, 14, dataObjects);
+	}
+	SHARED_PTR<RouteSegment> road;
+	double sdist = 0;
+	vector<SHARED_PTR<RouteDataObject> >::iterator it = dataObjects.begin();
+	for (; it!= dataObjects.end(); it++) {
+		SHARED_PTR<RouteDataObject> r = *it;
+		if (r->pointsX.size() > 1) {
+			for (int j = 1; j < r->pointsX.size() ; j++) {
+				double mDist = squareRootDist(r->pointsX[j -1 ], r->pointsY[j-1], r->pointsX[j], r->pointsY[j]);
+				int prx = r->pointsX[j];
+				int pry = r->pointsY[j];
+				double projection = calculateProjection31TileMetric(r->pointsX[j -1 ], r->pointsY[j-1], r->pointsX[j], r->pointsY[j],
+						px, py);
+				if (projection < 0) {
+					prx = r->pointsX[j - 1];
+					pry = r->pointsY[j - 1];
+				} else if (projection >= mDist * mDist) {
+					prx = r->pointsX[j ];
+					pry = r->pointsY[j ];
+				} else {
+					double c = projection / (mDist * mDist);
+					prx = (int) ((double)r->pointsX[j - 1] + ((double)r->pointsX[j] -  r->pointsX[j - 1]) * c);
+					pry = (int) ((double)r->pointsY[j - 1] + ((double)r->pointsY[j] -  r->pointsY[j - 1]) * c);
+				}
+				double currentsDist = squareDist31TileMetric(prx, pry, px, py);
+				if (road.get() == NULL || currentsDist < sdist) {
+					// make copy before
+					// r->pointsX.insert(j, prx);
+					// r->pointsY.insert(j, pry);
+					road = SHARED_PTR<RouteSegment>(new RouteSegment(r, j));
+					sdist = currentsDist;
+				}
+			}
+		}
+	}
+	// TODO FIX
+//	if (road.get() != null) {
+//		// re-register the best road because one more point was inserted
+//		ctx->registerRouteDataObject(road.getRoad());
+//	}
+	return road;
 }
 
  bool combineTwoSegmentResult(RouteSegmentResult& toAdd, RouteSegmentResult& previous,

Osmand-kernel/osmand/src/binaryRoutePlanner.h

@@ -265,7 +265,7 @@ struct RoutingConfiguration {
 		return 0;
 	}
 
-// TODO
+// TODO FIX
 	float calculateTurnTime(SHARED_PTR<RouteSegment> segment, int index, SHARED_PTR<RouteSegment> next, int nextIndex) {
 		return 0;
 	}
@@ -417,8 +417,43 @@ struct RoutingContext {
 		timeToLoad.pause();
 	}
 
+	void loadTileData(int x31, int y31, int zoomAround, vector<SHARED_PTR<RouteDataObject> >& dataObjects ){
+		int t = zoomAround - config.zoomToLoad;
+		if(t <= 0) {
+			t = 1;
+		} else {
+			t = 1 << t;
+		}
+		int coordinatesShift = (1 << (31 - config.zoomToLoad));
+		UNORDERED(set)<int64_t> ids;
+		int z  = config.zoomToLoad;
+		for(int i = -t; i <= t; i++) {
+			for(int j = -t; j <= t; j++) {
+				uint32_t xloc = (x31 + i*coordinatesShift) >> (31 - z);
+				uint32_t yloc = (y31+j*coordinatesShift) >> (31 - z);
+				int64_t tileId = (xloc << z) + yloc;
+				loadHeaders(xloc, yloc);
+				vector<SHARED_PTR<RoutingSubregionTile> > subregions = indexedSubregions[tileId];
+				for(int j = 0; j<subregions.size(); j++) {
+					if(subregions[j]->isLoaded()) {
+						UNORDERED(map)<int64_t, SHARED_PTR<RouteSegment> >::iterator s = subregions[j]->routes.begin();
+						while(s != subregions[j]->routes.end()) {
+							SHARED_PTR<RouteSegment> seg = s->second;
+							if(seg.get() != NULL) {
+								if(ids.find(seg->road->id) == ids.end()) {
+									dataObjects.push_back(seg->road);
+									ids.insert(seg->road->id);
+								}
+								seg = seg->next;
+							}
+							s++;
+						}
+					}
+				}
+			}
+		}
+	}
 
-	// FIXME replace with adequate method
 	SHARED_PTR<RouteSegment> loadSegmentAround(int x31, int y31) {
 		timeToLoad.start();
 		SHARED_PTR<RouteSegment> r;

Osmand-kernel/osmand/src/common.cpp

@@ -180,6 +180,29 @@ double getPowZoom(float zoom){
 	}
 }
 
+double convert31YToMeters(int y1, int y2) {
+	// translate into meters
+	return (y1 - y2) * 0.01863f;
+}
+
+double convert31XToMeters(int x1, int x2) {
+	// translate into meters
+	return (x1 - x2) * 0.011f;
+}
+
+
+double calculateProjection31TileMetric(int xA, int yA, int xB, int yB, int xC, int yC) {
+	// Scalar multiplication between (AB, AC)
+	double multiple = convert31XToMeters(xB, xA) * convert31XToMeters(xC, xA) + convert31YToMeters(yB, yA) * convert31YToMeters(yC, yA);
+	return multiple;
+}
+double squareDist31TileMetric(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;
+}
+
 
 double checkLongitude(double longitude) {
 	while (longitude < -180 || longitude > 180) {

Osmand-kernel/osmand/src/common.h

@@ -318,4 +318,10 @@ double getTileNumberY(float zoom, double latitude);
 double getDistance(double lat1, double lon1, double lat2, double lon2);
 double getPowZoom(float zoom);
 
+double calculateProjection31TileMetric(int xA, int yA, int xB, int yB, int xC, int yC);
+double squareDist31TileMetric(int x1, int y1, int x2, int y2) ;
+double convert31YToMeters(int y1, int y2);
+double convert31XToMeters(int y1, int y2);
+
+
 #endif /*_OSMAND_COMMON_H*/