turn penalties, k*angle^2 with left/right bias

Contractor/EdgeBasedGraphFactory.cpp

@@ -28,7 +28,7 @@
 #include "EdgeBasedGraphFactory.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, boost::property_tree::ptree speedProfile, std::string & srtm) : inputNodeInfoList(nI), numberOfTurnRestrictions(irs.size()), trafficSignalPenalty(0) {
+EdgeBasedGraphFactory::EdgeBasedGraphFactory(int nodes, std::vector<NodeBasedEdge> & inputEdges, std::vector<NodeID> & bn, std::vector<NodeID> & tl, std::vector<_Restriction> & irs, std::vector<NodeInfo> & nI, boost::property_tree::ptree speedProfile, std::string & srtm) : inputNodeInfoList(nI), numberOfTurnRestrictions(irs.size()), trafficSignalPenalty(0), turnPenalty(0), turnBias(1.0) {
     BOOST_FOREACH(_Restriction & restriction, irs) {
         std::pair<NodeID, NodeID> restrictionSource = std::make_pair(restriction.fromNode, restriction.viaNode);
         unsigned index;
@@ -73,6 +73,21 @@ EdgeBasedGraphFactory::EdgeBasedGraphFactory(int nodes, std::vector<NodeBasedEdg
             std::string value = v.second.get<std::string>("");
             takeMinimumOfSpeeds = (v.second.get<std::string>("") == "yes");
         }
+        if("turnPenalty" ==  v.first) {
+            try {
+                turnPenalty = boost::lexical_cast<int>(v.second.get<std::string>(""));
+            } catch(boost::bad_lexical_cast &) {
+                turnPenalty = 0;
+            }
+        }
+        if("turnBias" ==  v.first) {
+            std::string value = v.second.get<std::string>("");
+            try {
+                turnBias = boost::lexical_cast<float>(v.second.get<std::string>(""));
+            } catch(boost::bad_lexical_cast &) {
+                turnBias = 1;
+            }
+        }
     }
 
 //    INFO("traffic signal penalty: " << trafficSignalPenalty << ", U-Turn penalty: " << uturnPenalty << ", takeMinimumOfSpeeds=" << (takeMinimumOfSpeeds ? "yes" : "no"));
@@ -236,11 +251,12 @@ void EdgeBasedGraphFactory::Run() {
                         if(_trafficLights.find(v) != _trafficLights.end()) {
                             distance += trafficSignalPenalty;
                         }
-                        short turnInstruction = AnalyzeTurn(u, v, w);
+						unsigned penalty = 0;
+                        short turnInstruction = AnalyzeTurn(u, v, w, penalty);
+                        //distance += heightPenalty;
                         if(turnInstruction == TurnInstructions.UTurn)
                             distance += uturnPenalty;
-                        //distance += heightPenalty;
+						distance += penalty;
-                        //distance += ComputeTurnPenalty(u, v, w);
                         assert(edgeData1.edgeBasedNodeID != edgeData2.edgeBasedNodeID);
                         EdgeBasedEdge newEdge(edgeData1.edgeBasedNodeID, edgeData2.edgeBasedNodeID, v,  edgeData2.nameID, distance, true, false, turnInstruction);
                         edgeBasedEdges.push_back(newEdge);
@@ -260,7 +276,10 @@ void EdgeBasedGraphFactory::Run() {
     INFO("Generated " << edgeBasedNodes.size() << " edge based nodes");
 }
 
-short EdgeBasedGraphFactory::AnalyzeTurn(const NodeID u, const NodeID v, const NodeID w) const {
+short EdgeBasedGraphFactory::AnalyzeTurn(const NodeID u, const NodeID v, const NodeID w, unsigned& penalty) const {
+	double angle = GetAngleBetweenTwoEdges(inputNodeInfoList[u], inputNodeInfoList[v], inputNodeInfoList[w]);
+	penalty = ComputeTurnPenalty( angle );
+
 	if(u == w) {
         return TurnInstructions.UTurn;
     }
@@ -297,10 +316,22 @@ short EdgeBasedGraphFactory::AnalyzeTurn(const NodeID u, const NodeID v, const N
     if( (data1.nameID == data2.nameID) && (0 == data1.nameID) && (_nodeBasedGraph->GetOutDegree(v) <= 2) )
         return TurnInstructions.NoTurn;
 
-    double angle = GetAngleBetweenTwoEdges(inputNodeInfoList[u], inputNodeInfoList[v], inputNodeInfoList[w]);
     return TurnInstructions.GetTurnDirectionOfInstruction(angle);
 }
 
+unsigned EdgeBasedGraphFactory::ComputeTurnPenalty(const float angle) const {
+	unsigned out;
+	float a = angle-180;
+	float k = turnPenalty/(90.0*90.0);
+	if( a>=0 ) {
+		out = k*a*a*turnBias;		//right turn
+	} else {
+		out = k*a*a/turnBias;		//left turn
+	}
+	//INFO("angle " << a << ", out "<< out);
+	return out;
+}
+
 unsigned EdgeBasedGraphFactory::GetNumberOfNodes() const {
     return _nodeBasedGraph->GetNumberOfEdges();
 }

Contractor/EdgeBasedGraphFactory.h

@@ -117,6 +117,8 @@ private:
     unsigned numberOfTurnRestrictions;
     unsigned trafficSignalPenalty;
     unsigned uturnPenalty;
+    unsigned turnPenalty;
+    double turnBias;
     bool takeMinimumOfSpeeds;
 
 public:
@@ -127,7 +129,8 @@ public:
     template< class ImportEdgeT >
     void GetEdgeBasedEdges( std::vector< ImportEdgeT >& edges );
     void GetEdgeBasedNodes( std::vector< EdgeBasedNode> & nodes);
-    short AnalyzeTurn(const NodeID u, const NodeID v, const NodeID w) const;
+    short AnalyzeTurn(const NodeID u, const NodeID v, const NodeID w, unsigned& penalty) const;
+	unsigned ComputeTurnPenalty(const float angle) const;
    unsigned GetNumberOfNodes() const;
 };
 

speedprofiles/bicycle.ini

@@ -1,28 +1,34 @@
 [bicycle]
 	accessTag				= bicycle
-	defaultSpeed	= 17
 	obeyOneways				= yes
 	useRestrictions 		= yes
 	obeyBollards			= no
+	takeMinimumOfSpeeds 	= yes
+	excludeFromGrid 		= ferry
+	
+	defaultSpeed			= 17
+	trafficSignalPenalty 	= 10
+	turnPenalty				= 50
+	turnBias				= 1.4
 	
 	cycleway				= 19
 	primary         		= 19
 	primary_link    		= 19
-	secondary       = 17
+	secondary       		= 18
-	secondary_link  = 17
+	secondary_link  		= 18
-	tertiary        = 15
+	tertiary        		= 17
-	residential     = 15
+	residential     		= 16
-	unclassified    = 15
+	unclassified    		= 16
-	living_street   = 13
+	road		    		= 16
-	road		    = 13
+	living_street   		= 15
-	service         = 12
+	service         		= 15
-	track			= 12
+	track					= 13
-	path			= 12
+	path					= 13
-	footway			= 10
+	footway					= 5
 	pedestrian				= 5
 	pier					= 5
+	platform				= 5
 	steps					= 3
+	
 	ferry					= 5
 
-	excludeFromGrid = ferry
-	trafficSignalPenalty = 10