From 8077418c98c7dafeee86bebe6220a5ca869ebcff Mon Sep 17 00:00:00 2001
From: Emil Tin <emil@tin.dk>
Date: Sat, 23 Feb 2013 08:45:22 +0100
Subject: [PATCH] use bearing formula for better turn angles

---
 Contractor/EdgeBasedGraphFactory.cpp | 37 +++++++++++++++++++++-------
 features/testbot/turns.feature       | 10 ++++----
 2 files changed, 33 insertions(+), 14 deletions(-)

diff --git a/Contractor/EdgeBasedGraphFactory.cpp b/Contractor/EdgeBasedGraphFactory.cpp
index 13a570e46..04957e7f2 100644
--- a/Contractor/EdgeBasedGraphFactory.cpp
+++ b/Contractor/EdgeBasedGraphFactory.cpp
@@ -382,13 +382,32 @@ unsigned EdgeBasedGraphFactory::GetNumberOfNodes() const {
 /* Get angle of line segment (A,C)->(C,B), atan2 magic, formerly cosine theorem*/
 template<class CoordinateT>
 double EdgeBasedGraphFactory::GetAngleBetweenTwoEdges(const CoordinateT& A, const CoordinateT& C, const CoordinateT& B) const {
-    const int v1x = A.lon - C.lon;
-    const int v1y = A.lat - C.lat;
-    const int v2x = B.lon - C.lon;
-    const int v2y = B.lat - C.lat;
-
-    double angle = (atan2((double)v2y,v2x) - atan2((double)v1y,v1x) )*180/M_PI;
-    while(angle < 0)
-        angle += 360;
-    return angle;
+    double deltaLonCA,deltaLonBC,latA,latB,latC,x,y,a1,a2;
+    
+    latA = (A.lat/100000.)*M_PI/180;
+    latB = (B.lat/100000.)*M_PI/180;
+    latC = (C.lat/100000.)*M_PI/180;
+    deltaLonCA = (C.lon/100000. - A.lon/100000.)*M_PI/180;
+    deltaLonBC = (B.lon/100000. - C.lon/100000.)*M_PI/180;
+    
+    y = sin(deltaLonCA) * cos(latC);
+    x = cos(latA)*sin(latC) - sin(latA)*cos(latC)*cos(deltaLonCA);
+    a1 = atan2(y,x)*180/M_PI;
+    
+    y = sin(deltaLonBC) * cos(latB);
+    x = cos(latC)*sin(latB) - sin(latC)*cos(latB)*cos(deltaLonBC);
+    a2 = atan2(y,x)*180/M_PI;
+    const double angle = a2-a1;
+    
+    //const double v1x = A.lon - C.lon;
+    //const double v1y = A.lat - C.lat;
+    //const double v2x = B.lon - C.lon;
+    //const double v2y = B.lat - C.lat;
+    //const double angle = (atan2(v2y,v2x) - atan2(v1y,v1x) )*180/M_PI;
+    
+    if(angle < 0) {
+        return angle + 360;
+    } else {
+        return angle;
+    }
 }
diff --git a/features/testbot/turns.feature b/features/testbot/turns.feature
index 34a0c9199..5b8883019 100644
--- a/features/testbot/turns.feature
+++ b/features/testbot/turns.feature
@@ -96,14 +96,14 @@ Feature: Turn directions/codes
 		 | g | a | xg,xa | head,slight_right,destination |
 		 | g | c | xg,xc | head,right,destination        |
 		 | g | e | xg,xe | head,sharp_right,destination  |
-	
+	@xx
 	Scenario: Skadestuevej, København
 	https://github.com/DennisOSRM/Project-OSRM/issues/532
 		Given the node locations
 		 | node | lat      | lon      |
-		 | a    | 55.68679 | 12.52360 |
-		 | b    | 55.68745 | 12.52407 |
-		 | c    | 55.68720 | 12.52509 |
+		 | a    | 55.68740 | 12.52430 |
+		 | b    | 55.68745 | 12.52409 |
+		 | c    | 55.68711 | 12.52383 |
 
 		And the ways
 		 | nodes |
@@ -112,4 +112,4 @@ Feature: Turn directions/codes
 
 		When I route I should get
 		 | from | to | route | turns                  |
-		 | a    | c  | ab,bc | head,right,destination |
+		 | a    | c  | ab,bc | head,left,destination |