diff --git a/features/guidance/rotary.feature b/features/guidance/rotary.feature index ef8a1eb14..e1b14d3a3 100644 --- a/features/guidance/rotary.feature +++ b/features/guidance/rotary.feature @@ -165,3 +165,98 @@ Feature: Rotary | j,i | jkl,ghi,ghi | depart,bkheb-exit-2,arrive | | j,f | jkl,def,def | depart,bkheb-exit-3,arrive | | j,c | jkl,abc,abc | depart,bkheb-exit-4,arrive | + + Scenario: Collinear in X,Y + Given the node map + | a | | | + | b | | | + | c | d | f | + | e | | | + + And the ways + | nodes | junction | + | ab | | + | bcdb | roundabout | + | ce | | + | df | | + + When I route I should get + | waypoints | route | turns | + | a,e | ab,ce,ce | depart,bcdb-exit-1,arrive | + | a,f | ab,df,df | depart,bcdb-exit-2,arrive | + + Scenario: Collinear in X,Y + Given the node map + | a | | | + | d | | | + | b | c | f | + | e | | | + + And the ways + | nodes | junction | + | ab | | + | bcdb | roundabout | + | ce | | + | df | | + + When I route I should get + | waypoints | route | turns | + | a,e | ab,ce,ce | depart,bcdb-exit-1,arrive | + | a,f | ab,df,df | depart,bcdb-exit-2,arrive | + + Scenario: Collinear in X,Y + Given the node map + | a | | | + | c | | | + | d | b | f | + | e | | | + + And the ways + | nodes | junction | + | ab | | + | bcdb | roundabout | + | ce | | + | df | | + + When I route I should get + | waypoints | route | turns | + | a,e | ab,ce,ce | depart,bcdb-exit-1,arrive | + | a,f | ab,df,df | depart,bcdb-exit-2,arrive | + + Scenario: Collinear in X,Y + Given the node map + | f | | | + | d | c | e | + | | b | | + | | a | | + + And the ways + | nodes | junction | + | ab | | + | bcdb | roundabout | + | ce | | + | df | | + + When I route I should get + | waypoints | route | turns | + | a,e | ab,ce,ce | depart,bcdb-exit-1,arrive | + | a,f | ab,df,df | depart,bcdb-exit-2,arrive | + + Scenario: Collinear in X,Y + Given the node map + | f | | | + | d | c | e | + | b | | | + | a | | | + + And the ways + | nodes | junction | + | ab | | + | bcdb | roundabout | + | ce | | + | df | | + + When I route I should get + | waypoints | route | turns | + | a,e | ab,ce,ce | depart,bcdb-exit-1,arrive | + | a,f | ab,df,df | depart,bcdb-exit-2,arrive | diff --git a/features/guidance/roundabout.feature b/features/guidance/roundabout.feature index 71e527a1a..d03737c9c 100644 --- a/features/guidance/roundabout.feature +++ b/features/guidance/roundabout.feature @@ -164,3 +164,98 @@ Feature: Basic Roundabout | j,i | jkl,ghi,ghi | depart,roundabout-exit-2,arrive | | j,f | jkl,def,def | depart,roundabout-exit-3,arrive | | j,c | jkl,abc,abc | depart,roundabout-exit-4,arrive | + + Scenario: Collinear in X + Given the node map + | a | b | c | d | f | + | | | e | | | + + And the ways + | nodes | junction | + | ab | | + | bcdb | roundabout | + | ce | | + | df | | + + When I route I should get + | waypoints | route | turns | + | a,e | ab,ce,ce | depart,roundabout-exit-1,arrive | + | a,f | ab,df,df | depart,roundabout-exit-2,arrive | + + Scenario: Collinear in Y + Given the node map + | a | | + | b | | + | c | e | + | d | | + | f | | + + And the ways + | nodes | junction | + | ab | | + | bcdb | roundabout | + | ce | | + | df | | + + When I route I should get + | waypoints | route | turns | + | a,e | ab,ce,ce | depart,roundabout-exit-1,arrive | + | a,f | ab,df,df | depart,roundabout-exit-2,arrive | + + Scenario: Collinear in X,Y + Given the node map + | a | | | + | b | | | + | c | d | f | + | e | | | + + And the ways + | nodes | junction | + | ab | | + | bcdb | roundabout | + | ce | | + | df | | + + When I route I should get + | waypoints | route | turns | + | a,e | ab,ce,ce | depart,roundabout-exit-1,arrive | + | a,f | ab,df,df | depart,roundabout-exit-2,arrive | + + Scenario: Collinear in X,Y + Given the node map + | a | | | + | d | | | + | b | c | f | + | e | | | + + And the ways + | nodes | junction | + | ab | | + | bcdb | roundabout | + | ce | | + | df | | + + When I route I should get + | waypoints | route | turns | + | a,e | ab,ce,ce | depart,roundabout-exit-1,arrive | + | a,f | ab,df,df | depart,roundabout-exit-2,arrive | + + Scenario: Collinear in X,Y + Given the node map + | a | | | + | c | | | + | d | b | f | + | e | | | + + And the ways + | nodes | junction | + | ab | | + | bcdb | roundabout | + | ce | | + | df | | + + When I route I should get + | waypoints | route | turns | + | a,e | ab,ce,ce | depart,roundabout-exit-1,arrive | + | a,f | ab,df,df | depart,roundabout-exit-2,arrive | + diff --git a/include/extractor/guidance/toolkit.hpp b/include/extractor/guidance/toolkit.hpp index ff3ba8e53..35fd5472c 100644 --- a/include/extractor/guidance/toolkit.hpp +++ b/include/extractor/guidance/toolkit.hpp @@ -56,7 +56,7 @@ getCoordinateFromCompressedRange(util::Coordinate current_coordinate, BOOST_ASSERT(segment_length > 0); BOOST_ASSERT(second_distance >= detail::DESIRED_SEGMENT_LENGTH); double missing_distance = detail::DESIRED_SEGMENT_LENGTH - first_distance; - return missing_distance / segment_length; + return std::max(0., std::min(missing_distance / segment_length, 1.0)); }; for (auto compressed_geometry_itr = compressed_geometry_begin; @@ -84,7 +84,8 @@ getCoordinateFromCompressedRange(util::Coordinate current_coordinate, util::coordinate_calculation::haversineDistance(current_coordinate, final_coordinate); // reached point where coordinates switch between - if (distance_to_next_coordinate >= detail::DESIRED_SEGMENT_LENGTH) + if (distance_to_current_coordinate < detail::DESIRED_SEGMENT_LENGTH && + distance_to_next_coordinate >= detail::DESIRED_SEGMENT_LENGTH) return util::coordinate_calculation::interpolateLinear( getFactor(distance_to_current_coordinate, distance_to_next_coordinate), current_coordinate, final_coordinate); diff --git a/src/extractor/edge_based_graph_factory.cpp b/src/extractor/edge_based_graph_factory.cpp index 4cbde57c7..47ab7d10f 100644 --- a/src/extractor/edge_based_graph_factory.cpp +++ b/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(lua_state, "turn_function", 180. - angle); + BOOST_ASSERT(penalty < std::numeric_limits::max()); + BOOST_ASSERT(penalty > std::numeric_limits::min()); return boost::numeric_cast(penalty); } catch (const luabind::error &er) diff --git a/src/util/coordinate_calculation.cpp b/src/util/coordinate_calculation.cpp index 8af5ac2a4..24b280fef 100644 --- a/src/util/coordinate_calculation.cpp +++ b/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(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(toFloating(C3.lat - C2.lat)); // yDelta_b const double C3C2_lon = static_cast(toFloating(C3.lon - C2.lon)); // xDelta_b - // check for collinear points in X-Direction - if (std::abs(C2C1_lon) < std::numeric_limits::epsilon() && - std::abs(C3C2_lon) < std::numeric_limits::epsilon()) + // check for collinear points in X-Direction / Y-Direction + if ((std::abs(C2C1_lon) < std::numeric_limits::epsilon() && + std::abs(C3C2_lon) < std::numeric_limits::epsilon()) || + (std::abs(C2C1_lat) < std::numeric_limits::epsilon() && + std::abs(C3C2_lat) < std::numeric_limits::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::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::epsilon()) return boost::none; diff --git a/unit_tests/util/coordinate_calculation.cpp b/unit_tests/util/coordinate_calculation.cpp index c14280f5f..0515a3665 100644 --- a/unit_tests/util/coordinate_calculation.cpp +++ b/unit_tests/util/coordinate_calculation.cpp @@ -1,4 +1,5 @@ #include +#include #include "util/coordinate_calculation.hpp" @@ -11,6 +12,130 @@ using namespace osrm::util; BOOST_AUTO_TEST_SUITE(coordinate_calculation_tests) +BOOST_AUTO_TEST_CASE(compute_angle) +{ + // Simple cases + // North-South straight line + Coordinate first(FloatLongitude(1), FloatLatitude(-1)); + Coordinate middle(FloatLongitude(1), FloatLatitude(0)); + Coordinate end(FloatLongitude(1), FloatLatitude(1)); + auto angle = coordinate_calculation::computeAngle(first, middle, end); + BOOST_CHECK_EQUAL(angle, 180); + + // North-South-North u-turn + first = Coordinate(FloatLongitude(1), FloatLatitude(0)); + middle = Coordinate(FloatLongitude(1), FloatLatitude(1)); + end = Coordinate(FloatLongitude(1), FloatLatitude(0)); + angle = coordinate_calculation::computeAngle(first, middle, end); + BOOST_CHECK_EQUAL(angle, 0); + + // East-west straight lines are harder, *simple* coordinates only + // work at the equator. For other locations, we need to follow + // a rhumb line. + first = Coordinate(FloatLongitude(1), FloatLatitude(0)); + middle = Coordinate(FloatLongitude(2), FloatLatitude(0)); + end = Coordinate(FloatLongitude(3), FloatLatitude(0)); + angle = coordinate_calculation::computeAngle(first, middle, end); + BOOST_CHECK_EQUAL(angle, 180); + + // East-West-East u-turn + first = Coordinate(FloatLongitude(1), FloatLatitude(0)); + middle = Coordinate(FloatLongitude(2), FloatLatitude(0)); + end = Coordinate(FloatLongitude(1), FloatLatitude(0)); + angle = coordinate_calculation::computeAngle(first, middle, end); + BOOST_CHECK_EQUAL(angle, 0); + + // 90 degree left turn + first = Coordinate(FloatLongitude(1), FloatLatitude(1)); + middle = Coordinate(FloatLongitude(0), FloatLatitude(1)); + end = Coordinate(FloatLongitude(0), FloatLatitude(2)); + angle = coordinate_calculation::computeAngle(first, middle, end); + BOOST_CHECK_EQUAL(angle, 90); + + // 90 degree right turn + first = Coordinate(FloatLongitude(1), FloatLatitude(1)); + middle = Coordinate(FloatLongitude(0), FloatLatitude(1)); + end = Coordinate(FloatLongitude(0), FloatLatitude(0)); + angle = coordinate_calculation::computeAngle(first, middle, end); + BOOST_CHECK_EQUAL(angle, 270); + + // Weird cases + // Crossing both the meridians + first = Coordinate(FloatLongitude(-1), FloatLatitude(-1)); + middle = Coordinate(FloatLongitude(0), FloatLatitude(1)); + end = Coordinate(FloatLongitude(1), FloatLatitude(-1)); + angle = coordinate_calculation::computeAngle(first, middle, end); + BOOST_CHECK_CLOSE(angle, 53.1, 0.2); + + // All coords in the same spot + first = Coordinate(FloatLongitude(-1), FloatLatitude(-1)); + middle = Coordinate(FloatLongitude(-1), FloatLatitude(-1)); + end = Coordinate(FloatLongitude(-1), FloatLatitude(-1)); + angle = coordinate_calculation::computeAngle(first, middle, end); + BOOST_CHECK_EQUAL(angle, 180); + + // First two coords in the same spot, then heading north-east + first = Coordinate(FloatLongitude(-1), FloatLatitude(-1)); + middle = Coordinate(FloatLongitude(-1), FloatLatitude(-1)); + end = Coordinate(FloatLongitude(1), FloatLatitude(1)); + angle = coordinate_calculation::computeAngle(first, middle, end); + BOOST_CHECK_EQUAL(angle, 180); + + // First two coords in the same spot, then heading west + first = Coordinate(FloatLongitude(1), FloatLatitude(1)); + middle = Coordinate(FloatLongitude(1), FloatLatitude(1)); + end = Coordinate(FloatLongitude(2), FloatLatitude(1)); + angle = coordinate_calculation::computeAngle(first, middle, end); + BOOST_CHECK_EQUAL(angle, 180); + + // First two coords in the same spot then heading north + first = Coordinate(FloatLongitude(1), FloatLatitude(1)); + middle = Coordinate(FloatLongitude(1), FloatLatitude(1)); + end = Coordinate(FloatLongitude(1), FloatLatitude(2)); + angle = coordinate_calculation::computeAngle(first, middle, end); + BOOST_CHECK_EQUAL(angle, 180); + + // Second two coords in the same spot + first = Coordinate(FloatLongitude(1), FloatLatitude(1)); + middle = Coordinate(FloatLongitude(-1), FloatLatitude(-1)); + end = Coordinate(FloatLongitude(-1), FloatLatitude(-1)); + angle = coordinate_calculation::computeAngle(first, middle, end); + BOOST_CHECK_EQUAL(angle, 180); + + // First and last coords on the same spot + first = Coordinate(FloatLongitude(1), FloatLatitude(1)); + middle = Coordinate(FloatLongitude(-1), FloatLatitude(-1)); + end = Coordinate(FloatLongitude(1), FloatLatitude(1)); + angle = coordinate_calculation::computeAngle(first, middle, end); + BOOST_CHECK_EQUAL(angle, 0); + + // Check the antimeridian + first = Coordinate(FloatLongitude(180), FloatLatitude(90)); + middle = Coordinate(FloatLongitude(180), FloatLatitude(0)); + end = Coordinate(FloatLongitude(180), FloatLatitude(-90)); + angle = coordinate_calculation::computeAngle(first, middle, end); + BOOST_CHECK_EQUAL(angle, 180); + + // Tiny changes below our calculation resolution + // This should be equivalent to having two points on the same + // spot. + first = Coordinate(FloatLongitude(0), FloatLatitude(0)); + middle = Coordinate(FloatLongitude(1), FloatLatitude(0)); + end = Coordinate(FloatLongitude(1 + std::numeric_limits::epsilon()), FloatLatitude(0)); + angle = coordinate_calculation::computeAngle(first, middle, end); + BOOST_CHECK_EQUAL(angle, 180); + + // Invalid values + /* TODO: Enable this when I figure out how to use BOOST_CHECK_THROW + * and not have the whole test case fail... + first = Coordinate(FloatLongitude(0), FloatLatitude(0)); + middle = Coordinate(FloatLongitude(1), FloatLatitude(0)); + end = Coordinate(FloatLongitude(std::numeric_limits::max()), FloatLatitude(0)); + BOOST_CHECK_THROW( coordinate_calculation::computeAngle(first,middle,end), + boost::numeric::positive_overflow); + */ +} + // Regression test for bug captured in #1347 BOOST_AUTO_TEST_CASE(regression_test_1347) { @@ -54,8 +179,10 @@ BOOST_AUTO_TEST_CASE(regression_point_on_segment) FloatCoordinate diff{target.lon - start.lon, target.lat - start.lat}; - BOOST_CHECK_CLOSE(static_cast(start.lon + FloatLongitude(ratio) * diff.lon), static_cast(nearest.lon), 0.1); - BOOST_CHECK_CLOSE(static_cast(start.lat + FloatLatitude(ratio) * diff.lat), static_cast(nearest.lat), 0.1); + BOOST_CHECK_CLOSE(static_cast(start.lon + FloatLongitude(ratio) * diff.lon), + static_cast(nearest.lon), 0.1); + BOOST_CHECK_CLOSE(static_cast(start.lat + FloatLatitude(ratio) * diff.lat), + static_cast(nearest.lat), 0.1); } BOOST_AUTO_TEST_CASE(point_on_segment) @@ -122,4 +249,53 @@ BOOST_AUTO_TEST_CASE(point_on_segment) BOOST_CHECK_EQUAL(result_4.second.lat, reference_point_4.lat); } +BOOST_AUTO_TEST_CASE(circleCenter) +{ + Coordinate a(FloatLongitude(-100.), FloatLatitude(10.)); + Coordinate b(FloatLongitude(-100.002), FloatLatitude(10.001)); + Coordinate c(FloatLongitude(-100.001), FloatLatitude(10.002)); + + auto result = coordinate_calculation::circleCenter(a, b, c); + BOOST_CHECK(result); + BOOST_CHECK_EQUAL(*result, Coordinate(FloatLongitude(-100.000833), FloatLatitude(10.000833))); + + // Co-linear longitude + a = Coordinate(FloatLongitude(-100.), FloatLatitude(10.)); + b = Coordinate(FloatLongitude(-100.001), FloatLatitude(10.001)); + c = Coordinate(FloatLongitude(-100.001), FloatLatitude(10.002)); + result = coordinate_calculation::circleCenter(a, b, c); + BOOST_CHECK(result); + BOOST_CHECK_EQUAL(*result, Coordinate(FloatLongitude(-99.9995), FloatLatitude(10.0015))); + + // Co-linear longitude, impossible to calculate + a = Coordinate(FloatLongitude(-100.001), FloatLatitude(10.)); + b = Coordinate(FloatLongitude(-100.001), FloatLatitude(10.001)); + c = Coordinate(FloatLongitude(-100.001), FloatLatitude(10.002)); + result = coordinate_calculation::circleCenter(a, b, c); + BOOST_CHECK(!result); + + // Co-linear latitude, this is a real case that failed + a = Coordinate(FloatLongitude(-112.096234), FloatLatitude(41.147101)); + b = Coordinate(FloatLongitude(-112.096606), FloatLatitude(41.147101)); + c = Coordinate(FloatLongitude(-112.096419), FloatLatitude(41.147259)); + result = coordinate_calculation::circleCenter(a, b, c); + BOOST_CHECK(result); + BOOST_CHECK_EQUAL(*result, Coordinate(FloatLongitude(-112.09642), FloatLatitude(41.14707))); + + // Co-linear latitude, variation + a = Coordinate(FloatLongitude(-112.096234), FloatLatitude(41.147101)); + b = Coordinate(FloatLongitude(-112.096606), FloatLatitude(41.147259)); + c = Coordinate(FloatLongitude(-112.096419), FloatLatitude(41.147259)); + result = coordinate_calculation::circleCenter(a, b, c); + BOOST_CHECK(result); + BOOST_CHECK_EQUAL(*result, Coordinate(FloatLongitude(-112.096512), FloatLatitude(41.146962))); + + // Co-linear latitude, impossible to calculate + a = Coordinate(FloatLongitude(-112.096234), FloatLatitude(41.147259)); + b = Coordinate(FloatLongitude(-112.096606), FloatLatitude(41.147259)); + c = Coordinate(FloatLongitude(-112.096419), FloatLatitude(41.147259)); + result = coordinate_calculation::circleCenter(a, b, c); + BOOST_CHECK(!result); +} + BOOST_AUTO_TEST_SUITE_END()