From ddd352096d5a3dd3da5b58e2f06604e97c048990 Mon Sep 17 00:00:00 2001
From: Emil Tin <emil@tin.dk>
Date: Sun, 24 Feb 2013 10:58:56 +0100
Subject: [PATCH] use mercator projection

---
 Contractor/EdgeBasedGraphFactory.cpp | 38 +++++++++++++++++-------
 Util/Geometry.h                      | 43 ++++++++++++++++++++++++++++
 2 files changed, 70 insertions(+), 11 deletions(-)
 create mode 100644 Util/Geometry.h

diff --git a/Contractor/EdgeBasedGraphFactory.cpp b/Contractor/EdgeBasedGraphFactory.cpp
index c816902cb..42c2141bc 100644
--- a/Contractor/EdgeBasedGraphFactory.cpp
+++ b/Contractor/EdgeBasedGraphFactory.cpp
@@ -19,6 +19,8 @@
  */
 
 #include "EdgeBasedGraphFactory.h"
+#include "../DataStructures/MercatorUtil.h"
+#include "../Util/Geometry.h"
 
 template<>
 EdgeBasedGraphFactory::EdgeBasedGraphFactory(int nodes, std::vector<NodeBasedEdge> & inputEdges, std::vector<NodeID> & bn, std::vector<NodeID> & tl, std::vector<_Restriction> & irs, std::vector<NodeInfo> & nI, SpeedProfileProperties sp) : speedProfile(sp), inputNodeInfoList(nI), numberOfTurnRestrictions(irs.size()) {
@@ -382,19 +384,33 @@ unsigned EdgeBasedGraphFactory::GetNumberOfNodes() const {
 // get angle of line segment AC -> CB
 template<class CoordinateT>
 double EdgeBasedGraphFactory::GetAngleBetweenTwoEdges(const CoordinateT& A, const CoordinateT& C, const CoordinateT& B) const {
-    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 latC = (C.lat/100000.)*M_PI/180.;
-    const double scale = cos(latC);                           //scale by length of longitude at latitude C
-    const double a2 = atan2(v2y,v2x*scale)*180/M_PI;
-    const double a1 = atan2(v1y,v1x*scale)*180/M_PI;
-    const double angle = a2-a1;
-
+    const double latA = int2rad(A.lat);
+    const double latB = int2rad(B.lat);
+    const double latC = int2rad(C.lat);
+    
+    const double lonA = int2rad(A.lon);
+    const double lonB = int2rad(B.lon);
+    const double lonC = int2rad(C.lon);
+    
+    const double v1x = lonA - lonC;
+    const double v2x = lonB - lonC;
+    
+    // mercator projection: y=log(tan( pi/4 + lat/2 ))
+    // since log(a)-log(b) = log(a/b), it follows that:
+    // log(tan( M_PI/4 + latA/2 )) - log(tan( M_PI/4 + latC/2 )) =>
+    // log(tan( M_PI/4 + latA/2 )) / tan( M_PI/4 + latC/2 ))
+    // note that latC cannot be at a pole, lest we get a division by zero.
+    
+    const double v1y = log(tan(M_PI/4 + latA/2)/tan(M_PI/4 + latC/2)); 
+    const double v2y = log(tan(M_PI/4 + latB/2)/tan(M_PI/4 + latC/2));
+    
+    const double a2 = atan2(v2y,v2x);
+	const double a1 = atan2(v1y,v1x);
+	const double angle = rad2deg(a2-a1);	
+	
     if(angle < 0) {
         return angle + 360;
     } else {
         return angle;
     }
-}
+}
\ No newline at end of file
diff --git a/Util/Geometry.h b/Util/Geometry.h
new file mode 100644
index 000000000..088c61187
--- /dev/null
+++ b/Util/Geometry.h
@@ -0,0 +1,43 @@
+/*
+    open source routing machine
+    Copyright (C) Dennis Luxen, others 2010
+
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU AFFERO General Public License as published by
+the Free Software Foundation; either version 3 of the License, or
+any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU Affero General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+or see http://www.gnu.org/licenses/agpl.txt.
+ */
+
+
+#ifndef GEOMETRY_H_
+#define GEOMETRY_H_
+
+#include <cmath>
+
+inline double deg2rad(const double d) {
+     return d*M_PI/180;
+}
+
+inline double rad2deg(const double r) {
+    return r*180/M_PI;
+}
+
+inline double int2rad(long i) {
+    return (i/100000.)*M_PI/180;
+}
+
+inline double lat2yrad(double lat) {
+	return log(tan( M_PI/4 + lat/2 ));
+}
+
+#endif /* GEOMETRY_H_ */