From 539ee59689eb1516fada20ca0bd043388117b3bc Mon Sep 17 00:00:00 2001
From: Pablo Hoch <pablo@melaxis.com>
Date: Thu, 7 Mar 2024 16:28:48 +0100
Subject: [PATCH] use a buffered outer area polygon for extended visibility
 graphs

---
 include/ppr/common/area.h             | 18 ++++++++++++++++--
 include/ppr/common/area_routing.h     | 25 +++++++++++++------------
 include/ppr/common/geometry/polygon.h |  2 ++
 3 files changed, 31 insertions(+), 14 deletions(-)

diff --git a/include/ppr/common/area.h b/include/ppr/common/area.h
index 6839436..9184142 100644
--- a/include/ppr/common/area.h
+++ b/include/ppr/common/area.h
@@ -16,6 +16,19 @@
 
 namespace ppr {
 
+inline area_polygon_t get_buffered_polygon(area_polygon_t const& input) {
+  auto const distance_strategy =
+      boost::geometry::strategy::buffer::distance_symmetric<double>{0.5};
+  auto const join_strategy = boost::geometry::strategy::buffer::join_miter{};
+  auto const end_strategy = boost::geometry::strategy::buffer::end_flat{};
+  auto const point_strategy = boost::geometry::strategy::buffer::point_square{};
+  auto const side_strategy = boost::geometry::strategy::buffer::side_straight{};
+  auto output = area_multipolygon_t{};
+  boost::geometry::buffer(input, output, distance_strategy, side_strategy,
+                          join_strategy, end_strategy, point_strategy);
+  return output[0];
+}
+
 struct area {
   struct point {
     merc get_merc() const { return to_merc(location_); }
@@ -61,9 +74,10 @@ struct area {
     return polygon_.inners();
   }
 
-  area_polygon_t get_outer_polygon() const {
+  area_polygon_t get_outer_polygon(bool const buffered = false) const {
     auto const points = get_ring_points(polygon_.outer());
-    return {{begin(points), end(points)}};
+    auto poly = area_polygon_t{{begin(points), end(points)}};
+    return buffered ? get_buffered_polygon(poly) : poly;
   }
 
   std::vector<inner_area_polygon_t> get_inner_polygons() const {
diff --git a/include/ppr/common/area_routing.h b/include/ppr/common/area_routing.h
index 6b9d124..5da1cd1 100644
--- a/include/ppr/common/area_routing.h
+++ b/include/ppr/common/area_routing.h
@@ -18,7 +18,7 @@
 
 namespace ppr {
 
-using merc_segment_t = boost::geometry::model::segment<merc>;
+using merc_linestring_t = boost::geometry::model::linestring<merc>;
 
 template <typename Area>
 struct visibility_graph {
@@ -69,16 +69,18 @@ struct visibility_graph {
   data::vector<uint16_t> exit_nodes_;
 };
 
-inline void shorten_segment(merc_segment_t& seg, double len) {
-  if (distance(seg.first, seg.second) <= len * 2) {
+inline void shorten_segment(merc_linestring_t& seg, double len) {
+  auto& first = seg.front();
+  auto& second = seg.back();
+  if (distance(first, second) <= len * 2) {
     return;
   }
-  auto dir = seg.second - seg.first;
+  auto dir = second - first;
   dir.normalize();
-  auto offset = len * scale_factor(seg.first);
+  auto offset = len * scale_factor(first);
   dir *= offset;
-  seg.first += dir;
-  seg.second -= dir;
+  first += dir;
+  second -= dir;
 }
 
 template <typename Area>
@@ -121,11 +123,10 @@ void calc_visiblity(visibility_graph<Area>& vg,
     if (a_loc == b_loc) {
       continue;
     }
-    auto seg = merc_segment_t{a_loc, b_loc};
+    auto seg = merc_linestring_t{{a_loc, b_loc}};
     shorten_segment(seg, 0.5);
-    area_polygon_t seg_poly{{seg.first, seg.second, seg.first}};
-    if (!boost::geometry::within(seg_poly,
-                                 outer_polygon)) {  // does not support segments
+    if (!boost::geometry::within(seg,
+                                 outer_polygon)) {
       continue;
     }
     auto visible = true;
@@ -163,7 +164,7 @@ visibility_graph<Area> extend_visibility_graph(
     Area const* area,
     std::vector<typename Area::point_type>& additional_points) {
   visibility_graph<Area> vg(area, additional_points);
-  auto const outer_polygon = area->get_outer_polygon();
+  auto const outer_polygon = area->get_outer_polygon(true);
   auto const obstacles = area->get_inner_polygons();
 
   for (auto i = vg.base_size_; i < vg.n_; i++) {
diff --git a/include/ppr/common/geometry/polygon.h b/include/ppr/common/geometry/polygon.h
index f6bb93b..3391c68 100644
--- a/include/ppr/common/geometry/polygon.h
+++ b/include/ppr/common/geometry/polygon.h
@@ -6,5 +6,7 @@ namespace ppr {
 
 using area_polygon_t = boost::geometry::model::polygon<merc, false>;
 using inner_area_polygon_t = boost::geometry::model::polygon<merc, true>;
+using area_multipolygon_t =
+    boost::geometry::model::multi_polygon<area_polygon_t>;
 
 }  // namespace ppr