diff --git a/include/engine/area_clearance.hpp b/include/engine/area_clearance.hpp new file mode 100644 index 0000000000..a1671c6fdb --- /dev/null +++ b/include/engine/area_clearance.hpp @@ -0,0 +1,69 @@ +#ifndef OSRM_ENGINE_AREA_CLEARANCE_HPP +#define OSRM_ENGINE_AREA_CLEARANCE_HPP + +#include "engine/area_visibility.hpp" + +#include +#include + +namespace osrm::engine::area +{ + +/** + * @brief How much room there is at a point, and which way the nearest obstacle lies. + * + * This is the quantity an elastic band is built on. The clearance is the radius of a + * disc around the point that is guaranteed free of geometry, so a path whose consecutive + * points have overlapping discs is collision-free without any segment-versus-obstacle + * test ever being done. The gradient is what pushes a path away from whatever it is + * closest to. + */ +struct Clearance +{ + //! Distance to the nearest point of the area's geometry, in projected units. + double distance = 0.0; + //! The nearest point itself, which is on the segment named below. + Point nearest{}; + /** + * Unit vector from @c nearest towards the query point, which is the direction the + * clearance grows fastest in. Zero when the point lies on the geometry, where the + * clearance is not differentiable and there is no direction to give. + */ + Point gradient{}; + //! Which ring the nearest point is on, indexing the span that was passed in. + std::size_t ring = 0; + //! Which segment of that ring, by the index of its first vertex. + std::size_t segment = 0; +}; + +/** + * @brief The clearance at a point. + * + * Measures against every ring, the outer boundary included: a path has to stay off the + * walls of a plaza as much as off the fountain in the middle of it. The point is not + * required to be inside the area, and the distance is unsigned, so a point outside gets + * its distance to the boundary rather than a negative number. + * + * A flat scan over the segments. Plazas have hundreds of them and correctness comes + * first; an r-tree here would buy a logarithm and cost the ability to check this against + * a brute-force scan, which is currently how it is tested. Revisit when a profile says + * to. + */ +Clearance clearance(const Point &point, std::span rings); + +/** + * @brief How many metres one projected unit is, at a given latitude. + * + * Web Mercator is conformal, so locally it scales every direction alike and a distance in + * projected units can be turned into metres by one factor. That factor grows with + * latitude, so it is taken at the area being worked on rather than globally. + * + * This exists because the band's parameters are lengths a person would recognise, a + * clearance of a quarter of a metre and a comfort margin of one, while the geometry + * predicates all work in projected units. Converting once, here, keeps the two apart. + */ +double metres_per_projected_unit(double latitude_degrees); + +} // namespace osrm::engine::area + +#endif // OSRM_ENGINE_AREA_CLEARANCE_HPP diff --git a/src/engine/area_clearance.cpp b/src/engine/area_clearance.cpp new file mode 100644 index 0000000000..45a45fb6e6 --- /dev/null +++ b/src/engine/area_clearance.cpp @@ -0,0 +1,102 @@ +#include "engine/area_clearance.hpp" + +#include "util/coordinate_calculation.hpp" + +#include +#include +#include + +namespace osrm::engine::area +{ + +namespace +{ + +struct Nearest +{ + double distance_squared; + Point at; +}; + +/** + * The point of a segment closest to @p p, by projecting onto the line and clamping to the + * segment. A degenerate segment, which OSM rings do produce, collapses to its first + * point rather than dividing by zero. + */ +Nearest nearest_on_segment(const Point &p, const Point &a, const Point &b) +{ + const auto dx = b.x - a.x; + const auto dy = b.y - a.y; + const auto length_squared = dx * dx + dy * dy; + + auto t = 0.0; + if (length_squared > 0.0) + { + t = std::clamp(((p.x - a.x) * dx + (p.y - a.y) * dy) / length_squared, 0.0, 1.0); + } + + const Point at{a.x + t * dx, a.y + t * dy}; + const auto ex = p.x - at.x; + const auto ey = p.y - at.y; + return {ex * ex + ey * ey, at}; +} + +} // namespace + +Clearance clearance(const Point &point, std::span rings) +{ + Clearance best; + auto best_squared = std::numeric_limits::infinity(); + + for (std::size_t r = 0; r < rings.size(); ++r) + { + const auto &ring = rings[r]; + if (ring.size() < 2) + { + continue; + } + + for (std::size_t i = 0; i < ring.size(); ++i) + { + const auto found = nearest_on_segment(point, ring[i], ring[(i + 1) % ring.size()]); + // Strictly less, so the first segment wins a tie. A point equidistant from + // two segments is on the medial axis, where which one is named is arbitrary; + // what matters is that the choice is the same on every platform and in every + // run, because a path is built out of these answers. + if (found.distance_squared < best_squared) + { + best_squared = found.distance_squared; + best.nearest = found.at; + best.ring = r; + best.segment = i; + } + } + } + + if (!std::isfinite(best_squared)) + { + return {}; + } + + best.distance = std::sqrt(best_squared); + if (best.distance > 0.0) + { + best.gradient = {(point.x - best.nearest.x) / best.distance, + (point.y - best.nearest.y) / best.distance}; + } + return best; +} + +double metres_per_projected_unit(const double latitude_degrees) +{ + // One degree of longitude at the equator, which is what a projected unit is there. + // Mercator stretches by 1/cos(latitude), so a projected unit covers correspondingly + // less ground as one moves away from it. + namespace detail = util::coordinate_calculation::detail; + const auto metres_per_degree = + static_cast(detail::EARTH_RADIUS) * detail::DEGREE_TO_RAD; + const auto latitude = std::clamp(latitude_degrees, -89.9, 89.9); + return metres_per_degree * std::cos(latitude * detail::DEGREE_TO_RAD); +} + +} // namespace osrm::engine::area diff --git a/unit_tests/engine/area_clearance.cpp b/unit_tests/engine/area_clearance.cpp new file mode 100644 index 0000000000..057b6c1ffb --- /dev/null +++ b/unit_tests/engine/area_clearance.cpp @@ -0,0 +1,179 @@ +#include "engine/area_clearance.hpp" + +#include + +#include +#include +#include + +BOOST_AUTO_TEST_SUITE(area_clearance_test) + +using namespace osrm; +using namespace osrm::engine::area; + +namespace +{ + +// A 10x10 square with a 2x2 block in the middle, so every answer below can be worked out +// on paper. +struct Fixture +{ + std::vector outer{{0, 0}, {10, 0}, {10, 10}, {0, 10}}; + std::vector block{{4, 4}, {6, 4}, {6, 6}, {4, 6}}; + std::vector rings; + + explicit Fixture(bool with_block = true) + { + rings.emplace_back(outer); + if (with_block) + rings.emplace_back(block); + } +}; + +constexpr double TOLERANCE = 1e-9; + +} // namespace + +BOOST_AUTO_TEST_CASE(distance_to_a_wall) +{ + const Fixture f{false}; + + // Nearest wall is the one 1 away, not the three that are further. + const auto near_left = clearance({1, 5}, f.rings); + BOOST_CHECK_CLOSE(near_left.distance, 1.0, TOLERANCE); + BOOST_CHECK_CLOSE(near_left.nearest.x, 0.0, TOLERANCE); + BOOST_CHECK_CLOSE(near_left.nearest.y, 5.0, TOLERANCE); + // The gradient points away from the wall, into the square. + BOOST_CHECK_CLOSE(near_left.gradient.x, 1.0, TOLERANCE); + BOOST_CHECK_SMALL(near_left.gradient.y, 1e-12); + + // Dead centre of an empty square: 5 from all four walls. + BOOST_CHECK_CLOSE(clearance({5, 5}, f.rings).distance, 5.0, TOLERANCE); +} + +BOOST_AUTO_TEST_CASE(distance_to_a_corner) +{ + const Fixture f{false}; + + // Diagonally off the corner, so the point projects beyond the end of both edges and + // the nearest point is the corner itself. This is the case an unclamped projection + // onto the infinite line gets wrong, and it answers with a distance that is too small. + const auto at = clearance({-1, -1}, f.rings); + BOOST_CHECK_CLOSE(at.distance, std::sqrt(2.0), TOLERANCE); + BOOST_CHECK_SMALL(at.nearest.x, 1e-12); + BOOST_CHECK_SMALL(at.nearest.y, 1e-12); + + // Inside, near the same corner, both edges are in span and the answer is the wall at + // 1 rather than the corner at sqrt(2). Worth pinning: it is the mistake this test + // was written with. + BOOST_CHECK_CLOSE(clearance({1, 1}, f.rings).distance, 1.0, TOLERANCE); +} + +BOOST_AUTO_TEST_CASE(the_block_counts_as_much_as_the_walls) +{ + const Fixture f; + + // 5,2 is 2 from the wall below and 2 from the block above: a tie, and the first + // segment scanned wins it. What matters is that it is one of the two and that the + // distance is right. + const auto tie = clearance({5, 2}, f.rings); + BOOST_CHECK_CLOSE(tie.distance, 2.0, TOLERANCE); + + // Just inside the block's shadow the block wins outright. + const auto blocked = clearance({5, 3}, f.rings); + BOOST_CHECK_CLOSE(blocked.distance, 1.0, TOLERANCE); + BOOST_CHECK_EQUAL(blocked.ring, 1u); + BOOST_CHECK_CLOSE(blocked.gradient.y, -1.0, TOLERANCE); +} + +BOOST_AUTO_TEST_CASE(a_point_on_the_geometry_has_no_gradient) +{ + const Fixture f; + + const auto on_wall = clearance({0, 5}, f.rings); + BOOST_CHECK_SMALL(on_wall.distance, 1e-12); + BOOST_CHECK_SMALL(on_wall.gradient.x, 1e-12); + BOOST_CHECK_SMALL(on_wall.gradient.y, 1e-12); + + const auto on_corner = clearance({4, 4}, f.rings); + BOOST_CHECK_SMALL(on_corner.distance, 1e-12); +} + +BOOST_AUTO_TEST_CASE(a_point_outside_gets_its_distance_to_the_boundary) +{ + const Fixture f{false}; + + // Unsigned on purpose. A coordinate a person asked about can be just outside the + // plaza, and answering with a negative number would make every comparison against a + // margin do the wrong thing silently. + const auto outside = clearance({-3, 5}, f.rings); + BOOST_CHECK_CLOSE(outside.distance, 3.0, TOLERANCE); + BOOST_CHECK_CLOSE(outside.gradient.x, -1.0, TOLERANCE); +} + +BOOST_AUTO_TEST_CASE(inside_the_block_measures_to_the_block) +{ + const Fixture f; + + // The block's own centre is 1 from its walls. Nothing here knows about inside and + // outside, which is what makes the answer usable for a point that has strayed. + BOOST_CHECK_CLOSE(clearance({5, 5}, f.rings).distance, 1.0, TOLERANCE); +} + +BOOST_AUTO_TEST_CASE(degenerate_rings_do_not_divide_by_zero) +{ + std::vector repeated{{0, 0}, {0, 0}, {10, 0}, {10, 10}, {0, 10}}; + std::vector rings{repeated}; + + const auto at = clearance({5, 5}, rings); + BOOST_CHECK_CLOSE(at.distance, 5.0, TOLERANCE); + BOOST_CHECK(std::isfinite(at.gradient.x)); + BOOST_CHECK(std::isfinite(at.gradient.y)); + + // A ring with fewer than two points contributes nothing rather than crashing. + std::vector single{{1, 1}}; + std::vector with_single{repeated, single}; + BOOST_CHECK_CLOSE(clearance({5, 5}, with_single).distance, 5.0, TOLERANCE); +} + +BOOST_AUTO_TEST_CASE(no_rings_at_all) +{ + const auto nothing = clearance({5, 5}, {}); + BOOST_CHECK_SMALL(nothing.distance, 1e-12); +} + +BOOST_AUTO_TEST_CASE(the_gradient_is_the_direction_clearance_grows_in) +{ + const Fixture f; + + // Checked by finite difference rather than by construction: step along the reported + // gradient and the clearance must increase by the length of the step, because the + // gradient of a distance function is a unit vector. + const Point at{2.5, 5.0}; + const auto here = clearance(at, f.rings); + constexpr double STEP = 1e-6; + + const Point forward{at.x + here.gradient.x * STEP, at.y + here.gradient.y * STEP}; + const auto there = clearance(forward, f.rings); + BOOST_CHECK_CLOSE(there.distance - here.distance, STEP, 1e-4); + + // And the other way, which is the direction the repulsion force will push against. + const Point back{at.x - here.gradient.x * STEP, at.y - here.gradient.y * STEP}; + BOOST_CHECK_CLOSE(here.distance - clearance(back, f.rings).distance, STEP, 1e-4); +} + +BOOST_AUTO_TEST_CASE(projected_units_convert_to_metres) +{ + // At the equator a projected unit is a degree of longitude. + BOOST_CHECK_CLOSE(metres_per_projected_unit(0.0), 111194.9, 0.1); + + // And it shrinks with the cosine of the latitude, which is what makes the conversion + // a property of the area rather than a constant. + BOOST_CHECK_CLOSE(metres_per_projected_unit(60.0), metres_per_projected_unit(0.0) / 2, 0.1); + + // Symmetric, and defined at the poles rather than zero or NaN. + BOOST_CHECK_CLOSE(metres_per_projected_unit(-45.0), metres_per_projected_unit(45.0), 1e-9); + BOOST_CHECK_GT(metres_per_projected_unit(90.0), 0.0); +} + +BOOST_AUTO_TEST_SUITE_END()