UTM projection

5 files changed, 635 insertions, 0 deletions

diff --git a/server/src/geo/geo.cppm b/server/src/geo/geo.cppm
new file mode 100644
index 0000000..446f1eb
--- /dev/null
+++ b/server/src/geo/geo.cppm
@@ -0,0 +1,7 @@
+module;
+
+#include <boost/geometry.hpp>
+
+export module routemon:geo;
+
+namespace bgeo = boost::geometry;
diff --git a/server/src/geo/multizonal.cppm b/server/src/geo/multizonal.cppm
new file mode 100644
index 0000000..f732994
--- /dev/null
+++ b/server/src/geo/multizonal.cppm
@@ -0,0 +1,240 @@
+module;
+
+#include <boost/geometry.hpp>
+#include <boost/geometry/srs/epsg.hpp>
+#include <boost/geometry/srs/transformation.hpp>
+
+export module routemon:geo.multizonal;
+
+import std;
+import :geo;
+import :geo.utm;
+import :geo.utm.zone_local;
+import :geo.wgs84;
+
+namespace routemon::geo::multizonal {
+
+template<std::default_initializable T>
+class multi_zone
+{
+  std::array<T, utm::zone::max().as_index() + 1> zones_;
+
+public:
+  auto operator[](utm::zone z) -> T&
+  {
+    return zones_[z.as_index()];
+  }
+
+  auto operator[](utm::zone z) const -> T const&
+  {
+    return zones_[z.as_index()];
+  }
+};
+
+namespace wgs84
+{
+
+// Transformations from WGS 84 (EPSG:4326)
+using transform_from_t = bgeo::srs::transformation<bgeo::srs::static_epsg<4326>>;
+
+class utm_transform : public transform_from_t
+{
+  utm::zone to_zone_;
+
+public:
+  explicit utm_transform(utm::zone to_zone)
+    : transform_from_t{{}, bgeo::srs::epsg{to_zone.wgs84_proj_epsg()}},
+      to_zone_{to_zone}
+  {
+  }
+
+  utm_transform()
+    : utm_transform{utm::zone::min()}
+  {}
+
+  auto apply(utm::zonable_wgs84_point p) -> utm::zone_local::point
+  {
+    auto local_p = utm::zone_local::point{to_zone_, 0.0, 0.0};
+    forward(p, local_p);
+    return local_p;
+  }
+};
+
+class utm_transforms : public multi_zone<utm_transform>
+{
+  utm_transforms()
+  {
+    for (auto z = utm::zone::min(); z != utm::zone::max(); z = z.next())
+    {
+      (*this)[z] = utm_transform{z};
+    }
+  }
+
+public:
+  static utm_transforms const& instance()
+  {
+    static utm_transforms inst;
+    return inst;
+  }
+};
+
+auto neighbor_utm_zone(utm::zonable_wgs84_point p) -> std::pair<utm::zone, double>
+{
+  auto separating_meridian_lon = std::round(p.lon() / 6.0) * 6.0;
+  auto closest_zone_middle = separating_meridian_lon < p.lon()
+    ? separating_meridian_lon - 3.0
+    : separating_meridian_lon + 3.0;
+  auto closest_zone = utm::zone::for_wgs84_point(utm::zonable_wgs84_point::from(geo::wgs84::normalized_point{geo::wgs84::from_lat_lon(p.lat(), closest_zone_middle)}).value());
+
+  auto separating_meridian_ls = geo::wgs84::linestring{
+    geo::wgs84::from_lat_lon(90.0, 0.0),
+    geo::wgs84::from_lat_lon(0.0, separating_meridian_lon),
+    geo::wgs84::from_lat_lon(-90.0, 0.0),
+  };
+  auto closest_zone_dist = bgeo::distance(p, separating_meridian_ls, geo::wgs84::vincenty_strategy{});
+
+  return std::make_pair(closest_zone, closest_zone_dist);
+}
+
+} // namespace wgs84
+
+struct multizone_linestring
+{
+  multi_zone<std::vector<utm::zone_local::prim::linestring>> segments;
+
+  // TODO: consider taking an input range instead
+  explicit multizone_linestring(utm::zonable_wgs84_linestring const& ls)
+  {
+    auto to_utm = wgs84::utm_transforms::instance();
+    auto working = multi_zone<utm::zone_local::prim::linestring>{};
+    auto mprev_p_zone = std::optional<utm::zone>{};
+    auto mprev_p_alt_zone = std::optional<utm::zone>{};
+
+    auto push = [&](utm::zonable_wgs84_point p, utm::zone z)
+    { working[z].push_back(to_utm[z].apply(p)); };
+    auto flush = [&](utm::zone z)
+    {
+      segments[z].push_back(std::move(working[z]));
+      working[z] = {};
+    };
+
+    for (auto const& p : ls)
+    {
+      auto p_zone = utm::zone::for_wgs84_point(p);
+      auto mp_alt_zone = std::optional<utm::zone>{};
+      if (auto [neighbor_zone, neighbor_zone_dist] = wgs84::neighbor_utm_zone(p);
+          neighbor_zone_dist < 30.0 /* m */)
+        mp_alt_zone = neighbor_zone;
+      assert(!mp_alt_zone || *mp_alt_zone != p_zone);
+
+      push(p, p_zone);
+      if (mp_alt_zone)
+        push(p, *mp_alt_zone);
+      if (mprev_p_zone && *mprev_p_zone != p_zone && mprev_p_zone != mp_alt_zone)
+      {
+        push(p, *mprev_p_zone);
+        flush(*mprev_p_zone);
+      }
+      if (mprev_p_alt_zone && *mprev_p_alt_zone != p_zone && mprev_p_alt_zone != mp_alt_zone)
+      {
+        push(p, *mprev_p_alt_zone);
+        flush(*mprev_p_alt_zone);
+      }
+
+      mprev_p_zone = p_zone;
+      mprev_p_alt_zone = mp_alt_zone;
+    }
+
+    if (mprev_p_zone && !working[*mprev_p_zone].empty())
+    {
+      flush(*mprev_p_zone);
+    }
+    if (mprev_p_alt_zone && !working[*mprev_p_alt_zone].empty())
+    {
+      flush(*mprev_p_alt_zone);
+    }
+  }
+};
+
+struct zoned_linestring_seg_seq
+{
+  std::vector<utm::zone_local::linestring> segments;
+
+  explicit zoned_linestring_seg_seq(utm::zonable_wgs84_linestring ls)
+  {
+    auto to_utm = wgs84::utm_transforms::instance();
+    auto mworking_seg = std::optional<utm::zone_local::linestring>{};
+
+    for (auto const& p : ls)
+    {
+      auto p_zone = utm::zone::for_wgs84_point(p);
+      if (mworking_seg && mworking_seg->zone != p_zone)
+      {
+        mworking_seg->push_back(to_utm[mworking_seg->zone].apply(p));
+        segments.push_back(std::move(*mworking_seg));
+        mworking_seg = std::nullopt;
+      }
+      if (!mworking_seg)
+        mworking_seg = utm::zone_local::linestring{p_zone};
+      mworking_seg->push_back(to_utm[p_zone].apply(p));
+    }
+
+    if (mworking_seg && !mworking_seg->empty())
+    {
+      segments.push_back(std::move(*mworking_seg));
+    }
+  }
+};
+
+template<class T>
+class linestring_rtree
+{
+public:
+  using index_value = std::tuple<utm::zone_local::prim::box, utm::zone_local::prim::linestring, T>;
+
+private:
+  multi_zone<bgeo::index::rtree<index_value, bgeo::index::quadratic<16>>> local_rtrees_;
+
+public:
+  template<std::convertible_to<T> U>
+  auto insert(utm::zonable_wgs84_linestring const& ls, U&& arg) -> void
+  {
+    auto zone_segments = multizone_linestring{ls}.segments;
+    for (auto z = utm::zone::min(); z != utm::zone::max(); z = z.next())
+    {
+      for (auto ls : zone_segments[z])
+      {
+        utm::zone_local::prim::box blse =
+          bgeo::return_buffer<utm::zone_local::prim::box>(bgeo::return_envelope<utm::zone_local::prim::box>(ls), 30.0 /* m */);
+        local_rtrees_[z].insert(index_value{blse, std::move(ls), std::forward<U>(arg)});
+      }
+    }
+  }
+
+  auto size() const -> std::size_t
+  {
+    auto total_size = 0uz;
+    for (auto z = utm::zone::min(); z != utm::zone::max(); z = z.next())
+      total_size += local_rtrees_[z].size();
+    return total_size;
+  }
+
+  // Note: the same T may be generated more than once!
+  auto intersection(zoned_linestring_seg_seq const& lss) const -> std::generator<T const&>
+  {
+    for (auto const& ls : lss.segments)
+    {
+      // auto ls_box = bgeo::return_envelope<utm::zone_local::prim::box>(ls);
+      for (auto it = local_rtrees_[ls.zone].qbegin(bgeo::index::intersects(ls));
+           it != local_rtrees_[ls.zone].qend(); it++)
+      {
+        if (bgeo::intersects(ls, std::get<1>(*it)))
+        {
+          co_yield std::get<2>(*it);
+        }
+      }
+    }
+  }
+};
+
+} // namespace routemon::geo::multizonal
diff --git a/server/src/geo/utm.cppm b/server/src/geo/utm.cppm
new file mode 100644
index 0000000..83da222
--- /dev/null
+++ b/server/src/geo/utm.cppm
@@ -0,0 +1,199 @@
+module;
+
+#include <cassert>
+#include <boost/geometry/geometries/geometries.hpp>
+
+export module routemon:geo.utm;
+
+import std;
+import :geo;
+import :geo.wgs84;
+
+namespace routemon::geo::utm
+{
+
+class zonable_wgs84_point
+{
+  wgs84::normalized_point p_;
+
+  explicit zonable_wgs84_point(wgs84::normalized_point p)
+    : p_{p}
+  {}
+
+public:
+  zonable_wgs84_point()
+    : p_{}
+  {
+  }
+
+  static auto from(wgs84::normalized_point p) -> std::optional<zonable_wgs84_point>
+  {
+    if (p.lat() < -80 || p.lat() > 84)
+      return std::nullopt;
+    return zonable_wgs84_point{p};
+  }
+
+  auto lat() const -> double
+  {
+    return p_.lat();
+  }
+
+  auto lon() const -> double
+  {
+    return p_.lon();
+  }
+
+  auto lon(double lon) -> void
+  {
+    p_.lon(lon);
+  }
+
+  auto lat(double lat) -> void
+  {
+    if (lat < -80 || lat > 84)
+      throw std::range_error{"latitude out of range for UTM"};
+    p_.lat(lat);
+  }
+};
+
+}
+
+namespace boost::geometry::traits {
+
+  namespace {
+
+    using zonable_wgs84_point = routemon::geo::utm::zonable_wgs84_point;
+
+  } // namespace <anonymous>
+
+  template<> struct tag<zonable_wgs84_point> { using type = point_tag; };
+  template<> struct dimension<zonable_wgs84_point> : boost::mpl::int_<2> {};
+  template<> struct coordinate_type<zonable_wgs84_point> { using type = double; };
+  template<> struct coordinate_system<zonable_wgs84_point> { using type = routemon::geo::wgs84::cs; };
+
+  template<std::size_t index>
+  struct access<zonable_wgs84_point, index> {
+    static inline auto get(zonable_wgs84_point const& p) -> double
+    {
+      if constexpr (index == 0)
+        return p.lon();
+      else if constexpr (index == 1)
+        return p.lat();
+      else static_assert(false, "Out of range");
+    }
+
+    static inline auto set(zonable_wgs84_point& p, double v) -> void
+    {
+      if constexpr (index == 0)
+        p.lon(v);
+      else if constexpr (index == 1)
+        p.lat(v);
+      else static_assert(false, "Out of range");
+    }
+  };
+
+} // namespace boost::geometry::traits
+
+namespace routemon::geo::utm
+{
+
+using zonable_wgs84_linestring = bgeo::model::linestring<zonable_wgs84_point, std::vector>;
+
+// In the sense of the common WGS84 subdivisions by simple northing
+// and easting (so no Norway/Svalbard exceptions).
+class zone
+{
+  // Note: calculations heavily depend on the values of the variants.
+  enum class hemisphere : std::uint8_t
+  {
+    northern = 0,
+    southern = 1,
+  };
+
+  // Negative if in the southern hemisphere
+  // Equal to (zone_no - 1) * 2 + hemisphere
+  // In range [0, 119]
+  std::uint8_t zone_;
+
+  static constexpr auto zone_no_valid(std::uint8_t zone_no) -> bool
+  {
+    return 0 < zone_no && zone_no <= 60;
+  }
+
+  static constexpr auto from(std::uint8_t zone_no, hemisphere h) -> std::uint8_t
+  {
+    if (!zone_no_valid(zone_no))
+      throw std::invalid_argument{"invalid UTM zone number"};
+    return (zone_no - 1) * 2 + static_cast<std::uint8_t>(h);
+  }
+
+public:
+  explicit constexpr zone(std::uint8_t zone_no, hemisphere h)
+    : zone_{from(zone_no, h)}
+  {}
+
+  auto hemisphere() const -> enum hemisphere
+  {
+    return static_cast<enum hemisphere>(zone_ % 2);
+  }
+
+  // Return value in range [1, 60]
+  auto zone_no() const -> std::uint8_t
+  {
+    return 1 + zone_ / 2;
+  }
+
+  // Return value in range [min(), max()]
+  constexpr auto as_index() const -> std::uint8_t
+  {
+    return zone_;
+  }
+
+  static auto for_wgs84_point(zonable_wgs84_point p) noexcept -> zone
+  {
+    auto zone_no = static_cast<std::uint8_t>(1 + (p.lon() + 180.0) / 6.0);
+    auto northern = p.lat() >= 0.0;
+    return zone{zone_no, northern ? hemisphere::northern : hemisphere::southern};
+  }
+
+  auto wgs84_proj_epsg() const -> int
+  {
+    switch (hemisphere())
+    {
+    case hemisphere::northern:
+      return 32600 + zone_no();
+    case hemisphere::southern:
+      return 32700 + zone_no();
+    }
+  }
+
+  // Guarantee: min().as_index() == 0.
+  static constexpr auto min() noexcept -> zone
+  {
+    return zone{1, hemisphere::northern};
+  }
+
+  static constexpr auto max() noexcept -> zone
+  {
+    return zone{60, hemisphere::southern};
+  }
+
+  auto next() const -> zone
+  {
+    assert(zone_ <= max().as_index());
+    if (zone_ == max().as_index())
+      throw std::range_error{"cannot take next of greatest UTM zone"};
+    auto copy = zone{*this};
+    copy.zone_++;
+    return copy;
+  }
+
+  auto operator==(zone rhs) const -> bool
+  {
+    return zone_ == rhs.zone_;
+  }
+};
+static_assert(zone::min().as_index() == 0);
+static_assert(zone::max().as_index() == 119);
+
+} // namespace routemon::geo::utm
diff --git a/server/src/geo/utm_zone_local.cppm b/server/src/geo/utm_zone_local.cppm
new file mode 100644
index 0000000..98a1344
--- /dev/null
+++ b/server/src/geo/utm_zone_local.cppm
@@ -0,0 +1,74 @@
+module;
+
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/register/linestring.hpp>
+
+export module routemon:geo.utm.zone_local;
+
+import :geo;
+import :geo.utm;
+
+// Coordinates that are local to some known UTM zone.
+namespace routemon::geo::utm::zone_local {
+
+  using cs = bgeo::cs::cartesian;
+
+  namespace prim {
+
+    using point = bgeo::model::d2::point_xy<double>;
+    using linestring = bgeo::model::linestring<point>;
+    using box = bgeo::model::box<point>;
+
+  } // namespace prim
+
+  struct point : public prim::point
+  {
+    zone zone;
+
+    explicit point(class zone zone, double x, double y)
+      : prim::point{x, y}, zone{zone}
+    {
+    }
+  };
+
+  struct linestring : public prim::linestring
+  {
+    zone zone;
+
+    explicit linestring(class zone zone)
+      : zone{zone}
+    {
+    }
+  };
+
+} // namespace routemon::geo::utm::zone_local
+
+BOOST_GEOMETRY_REGISTER_LINESTRING(routemon::geo::utm::zone_local::linestring)
+
+namespace boost::geometry::traits {
+
+  namespace {
+
+    using zone_local_point = routemon::geo::utm::zone_local::point;
+
+  } // namespace <anonymous>
+
+  template<> struct tag<zone_local_point> { using type = point_tag; };
+  template<> struct dimension<zone_local_point> : boost::mpl::int_<2> {};
+  template<> struct coordinate_type<zone_local_point> { using type = double; };
+  template<> struct coordinate_system<zone_local_point> { using type = routemon::geo::utm::zone_local::cs; };
+
+  template<std::size_t index>
+  struct access<zone_local_point, index> {
+    static inline auto get(zone_local_point const& p) -> double
+    {
+      return bgeo::get<index>(static_cast<routemon::geo::utm::zone_local::prim::point const&>(p));
+    }
+
+    static inline auto set(zone_local_point& p, double v) -> void
+    {
+      return bgeo::set<index>(static_cast<routemon::geo::utm::zone_local::prim::point&>(p), v);
+    }
+  };
+
+} // namespace boost::geometry::traits
diff --git a/server/src/geo/wgs84.cppm b/server/src/geo/wgs84.cppm
new file mode 100644
index 0000000..d574d39
--- /dev/null
+++ b/server/src/geo/wgs84.cppm
@@ -0,0 +1,115 @@
+module;
+
+#include <boost/geometry.hpp>
+
+export module routemon:geo.wgs84;
+
+import :geo;
+
+namespace routemon::geo::wgs84 {
+
+using cs = bgeo::cs::geographic<bgeo::degree>;
+using point = bgeo::model::point<double, 2, cs>;
+// TODO: guarantee that linestring provides non-static member
+//   auto reserve(std::size_t) -> void
+// Perhaps better yet: guarantee that the backing container is a std::vector.
+using linestring = bgeo::model::linestring<point, std::vector /* the default */>;
+using box = bgeo::model::box<point>;
+using stype = bgeo::srs::spheroid<double>;
+using vincenty_strategy = bgeo::strategy::distance::vincenty<stype>;
+
+auto from_lat_lon(double lat, double lon) -> point
+{
+  return point{lon, lat};
+}
+
+auto lat(point p) -> double
+{
+  return bgeo::get<1>(p);
+}
+
+auto lon(point p) -> double
+{
+  return bgeo::get<0>(p);
+}
+
+// Point p with latitude in [-90, 90] and longitude in [-180, 180)
+auto normalize(point p) -> point
+{
+  // Equivalent degrees in the range [0, 360)
+  auto nonneg_mod360 = [](double t) -> double
+  { return std::remainder(std::remainder(t, 360.0) + 360.0, 360.0); };
+
+  auto p_lon = lon(p);
+  auto p_lat = nonneg_mod360(lat(p)) - 90.0;
+  if (90.0 < p_lat)
+  {
+    assert(p_lat < 270.0);  // by nonneg_mod360
+    p_lon += 180.0;
+    p_lat -= 180.0;
+  }
+  p_lon = nonneg_mod360(p_lon + 180.0) - 180.0;
+
+  return from_lat_lon(p_lat, p_lon);
+}
+
+auto is_normalized(point p) -> bool
+{
+  return -90 <= lat(p) && lat(p) <= 90 && -180 <= lon(p) && lon(p) < 180;
+}
+
+class normalized_point
+{
+  point p_;
+
+public:
+  normalized_point(point p)
+    : p_{is_normalized(p) ? p : normalize(p)}
+  {}
+
+  normalized_point()
+    : p_{}
+  {}
+
+  auto lat() const -> double
+  {
+    return bgeo::get<1>(p_);
+  }
+
+  auto lon() const -> double
+  {
+    return bgeo::get<0>(p_);
+  }
+
+  auto lat(double lat) -> void
+  {
+    if (lat < -90 || lat > 90)
+      throw std::range_error{"latitude out of range"};
+    bgeo::set<1>(p_, lat);
+  }
+
+  auto lon(double lon) -> void
+  {
+    if (lon < -180 || lon > 180)
+      throw std::range_error{"longitude out of range"};
+    bgeo::set<0>(p_, lon);
+  }
+};
+
+class normalized_linestring
+{
+  std::vector<normalized_point> ls_;
+
+public:
+  auto empty() const -> bool
+  {
+    return ls_.empty();
+  }
+
+  auto push_back(normalized_point p) -> void
+  {
+    ls_.push_back(p);
+  }
+};
+
+} // namespace routemon::geo::wgs84