Fix Radius Calculation for points collinear in latidue

src/extractor/edge_based_graph_factory.cpp

@@ -2,12 +2,12 @@
 #include "extractor/edge_based_graph_factory.hpp"
 #include "util/coordinate.hpp"
 #include "util/coordinate_calculation.hpp"
-#include "util/percent.hpp"
+#include "util/exception.hpp"
 #include "util/integer_range.hpp"
 #include "util/lua_util.hpp"
+#include "util/percent.hpp"
 #include "util/simple_logger.hpp"
 #include "util/timing_util.hpp"
-#include "util/exception.hpp"
 
 #include "extractor/guidance/toolkit.hpp"
 
@@ -122,8 +122,7 @@ void EdgeBasedGraphFactory::InsertEdgeBasedNode(const NodeID node_u, const NodeI
 
     NodeID current_edge_source_coordinate_id = node_u;
 
-    const auto edge_id_to_segment_id = [](const NodeID edge_based_node_id)
-    {
+    const auto edge_id_to_segment_id = [](const NodeID edge_based_node_id) {
         if (edge_based_node_id == SPECIAL_NODEID)
         {
             return SegmentID{SPECIAL_SEGMENTID, false};
@@ -133,7 +132,7 @@ void EdgeBasedGraphFactory::InsertEdgeBasedNode(const NodeID node_u, const NodeI
     };
 
     // traverse arrays from start and end respectively
-    for (const auto i : util::irange(std::size_t{ 0 }, geometry_size))
+    for (const auto i : util::irange(std::size_t{0}, geometry_size))
     {
         BOOST_ASSERT(
             current_edge_source_coordinate_id ==
@@ -468,6 +467,8 @@ int EdgeBasedGraphFactory::GetTurnPenalty(double angle, lua_State *lua_state) co
     {
         // call lua profile to compute turn penalty
         double penalty = luabind::call_function<double>(lua_state, "turn_function", 180. - angle);
+        BOOST_ASSERT(penalty < std::numeric_limits<int>::max());
+        BOOST_ASSERT(penalty > std::numeric_limits<int>::min());
         return boost::numeric_cast<int>(penalty);
     }
     catch (const luabind::error &er)

src/util/coordinate_calculation.cpp

@@ -197,6 +197,13 @@ double computeAngle(const Coordinate first, const Coordinate second, const Coord
     using namespace boost::math::constants;
     using namespace coordinate_calculation;
 
+    if (first == second || second == third)
+        return 180;
+
+    BOOST_ASSERT(first.IsValid());
+    BOOST_ASSERT(second.IsValid());
+    BOOST_ASSERT(third.IsValid());
+
     const double v1x = static_cast<double>(toFloating(first.lon - second.lon));
     const double v1y =
         web_mercator::latToY(toFloating(first.lat)) - web_mercator::latToY(toFloating(second.lat));
@@ -211,6 +218,7 @@ double computeAngle(const Coordinate first, const Coordinate second, const Coord
         angle += 360.;
     }
 
+    BOOST_ASSERT(angle >= 0);
     return angle;
 }
 
@@ -230,9 +238,11 @@ circleCenter(const Coordinate C1, const Coordinate C2, const Coordinate C3)
     const double C3C2_lat = static_cast<double>(toFloating(C3.lat - C2.lat)); // yDelta_b
     const double C3C2_lon = static_cast<double>(toFloating(C3.lon - C2.lon)); // xDelta_b
 
-    // check for collinear points in X-Direction
-    if (std::abs(C2C1_lon) < std::numeric_limits<double>::epsilon() &&
-        std::abs(C3C2_lon) < std::numeric_limits<double>::epsilon())
+    // check for collinear points in X-Direction / Y-Direction
+    if ((std::abs(C2C1_lon) < std::numeric_limits<double>::epsilon() &&
+         std::abs(C3C2_lon) < std::numeric_limits<double>::epsilon()) ||
+        (std::abs(C2C1_lat) < std::numeric_limits<double>::epsilon() &&
+         std::abs(C3C2_lat) < std::numeric_limits<double>::epsilon()))
     {
         return boost::none;
     }
@@ -258,10 +268,18 @@ circleCenter(const Coordinate C1, const Coordinate C2, const Coordinate C3)
         return circleCenter(C2, C1, C3);
     }
     else
-    { // valid slope values for both lines, calculate the center as intersection of the lines
+    {
         const double C2C1_slope = C2C1_lat / C2C1_lon;
         const double C3C2_slope = C3C2_lat / C3C2_lon;
 
+        if (std::abs(C2C1_slope) < std::numeric_limits<double>::epsilon())
+        {
+            // Three non-collinear points with C2,C1 on same latitude.
+            // Due to the x-values correct, we can swap C3 and C1 to obtain the correct slope value
+            return circleCenter(C3, C2, C1);
+        }
+        // valid slope values for both lines, calculate the center as intersection of the lines
+
         // can this ever happen?
         if (std::abs(C2C1_slope - C3C2_slope) < std::numeric_limits<double>::epsilon())
             return boost::none;