feat(engine): measure how much room a point has inside an area

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 <cstddef>
+#include <span>
+
+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<const Ring> 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

src/engine/area_clearance.cpp

@@ -0,0 +1,102 @@
+#include "engine/area_clearance.hpp"
+
+#include "util/coordinate_calculation.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <limits>
+
+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<const Ring> rings)
+{
+    Clearance best;
+    auto best_squared = std::numeric_limits<double>::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<double>(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

unit_tests/engine/area_clearance.cpp

@@ -0,0 +1,179 @@
+#include "engine/area_clearance.hpp"
+
+#include <boost/test/unit_test.hpp>
+
+#include <cmath>
+#include <span>
+#include <vector>
+
+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<Point> outer{{0, 0}, {10, 0}, {10, 10}, {0, 10}};
+    std::vector<Point> block{{4, 4}, {6, 4}, {6, 6}, {4, 6}};
+    std::vector<Ring> 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<Point> repeated{{0, 0}, {0, 0}, {10, 0}, {10, 10}, {0, 10}};
+    std::vector<Ring> 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<Point> single{{1, 1}};
+    std::vector<Ring> 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()