From 90d574c04ec5776102f6bb20c99a0ffcd043dc6f Mon Sep 17 00:00:00 2001
From: Victor Shcherb <victor.shcherb@gmail.com>
Date: Sun, 3 Jul 2011 19:02:21 +0200
Subject: [PATCH] Tweak car routing

---
 .../net/osmand/router/BinaryRoutePlanner.java |  6 ++---
 .../src/net/osmand/router/CarRouter.java      | 22 +++++++++++++++----
 2 files changed, 21 insertions(+), 7 deletions(-)

diff --git a/DataExtractionOSM/src/net/osmand/router/BinaryRoutePlanner.java b/DataExtractionOSM/src/net/osmand/router/BinaryRoutePlanner.java
index 4e23c71d1c..4c9ce79178 100644
--- a/DataExtractionOSM/src/net/osmand/router/BinaryRoutePlanner.java
+++ b/DataExtractionOSM/src/net/osmand/router/BinaryRoutePlanner.java
@@ -32,7 +32,7 @@ public class BinaryRoutePlanner {
 	
 	private final static boolean PRINT_TO_CONSOLE_ROUTE_INFORMATION_TO_TEST = true;
 	private final BinaryMapIndexReader[] map;
-	private static int DEFAULT_HEURISTIC_COEFFICIENT = 3;
+	private static int DEFAULT_HEURISTIC_COEFFICIENT = 1;
 	
 	private static final Log log = LogUtil.getLog(BinaryRoutePlanner.class);
 	
@@ -302,7 +302,7 @@ public class BinaryRoutePlanner {
 		if (firstOfSegment) {
 			RouteSegment possibleObstacle = next;
 			while (possibleObstacle != null) {
-				/*obstaclesTime += */ctx.router.defineObstacle(possibleObstacle.road, possibleObstacle.segmentStart);
+				obstaclesTime += ctx.router.defineObstacle(possibleObstacle.road, possibleObstacle.segmentStart);
 				possibleObstacle = possibleObstacle.next;
 			}
 		}
@@ -310,7 +310,7 @@ public class BinaryRoutePlanner {
 		// 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 */
+			/* next.road.getId() >> 3 (1) != road.getId() >> 3 (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;
diff --git a/DataExtractionOSM/src/net/osmand/router/CarRouter.java b/DataExtractionOSM/src/net/osmand/router/CarRouter.java
index bebc5f356a..89d00be2f5 100644
--- a/DataExtractionOSM/src/net/osmand/router/CarRouter.java
+++ b/DataExtractionOSM/src/net/osmand/router/CarRouter.java
@@ -135,13 +135,27 @@ public class CarRouter extends VehicleRouter {
 	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;
+		// that addition highly affects to trunk roads !(prefer trunk/motorway)
+		if (end && start) {
+			if (next.road.getPointsLength() > 1) {
+				int x = segment.road.getPoint31XTile(segmentEnd);
+				int y = segment.road.getPoint31XTile(segmentEnd);
+				int prevSegmentEnd = segmentEnd - 1;
+				if(prevSegmentEnd < 0){
+					prevSegmentEnd = segmentEnd + 1;
+				}
+				int px = segment.road.getPoint31XTile(prevSegmentEnd);
+				int py = segment.road.getPoint31XTile(prevSegmentEnd);
+				double a1 = Math.atan2(y - py, x - px);
+				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;
 		} else {
-			return 25;
+			return 15;
 		}
 	}