add closest_on_segment (based on ppr)

include/geo/constants.h

@@ -4,5 +4,6 @@ namespace geo {
 
 constexpr auto kPI = 3.14159265358979323846;
 constexpr auto kEarthRadiusMeters = 6371000.0;
+constexpr auto kEpsilon = 0.000000001;
 
 }  // namespace geo

include/geo/latlng.h

@@ -26,6 +26,9 @@ double bearing(latlng const&, latlng const&);
 
 latlng midpoint(latlng const&, latlng const&);
 
+latlng closest_on_segment(latlng const& x, latlng const& segment_from,
+                          latlng const& segment_to);
+
 uint32_t tile_hash_32(latlng const&);
 
 }  // namespace geo

include/geo/rad_deg.h

@@ -0,0 +1,13 @@
+#pragma once
+
+#include "geo/constants.h"
+
+namespace geo {
+
+constexpr double to_rad(double const deg) { return deg * kPI / 180.0; }
+constexpr double to_deg(double const rad) { return rad * 180.0 / kPI; }
+
+inline double operator""_rad(long double const deg) { return to_rad(deg); }
+inline double operator""_deg(long double const rad) { return to_deg(rad); }
+
+}  // namespace geo

include/geo/webmercator.h

@@ -7,6 +7,7 @@
 
 #include "geo/constants.h"
 #include "geo/latlng.h"
+#include "geo/rad_deg.h"
 
 //
 // These utilities translate between three coordinate systems:
@@ -59,10 +60,61 @@ struct xy {
   T& x() { return x_; }
   T& y() { return y_; }
 
-  friend bool operator==(xy const& lhs, xy const& rhs) {
-    return std::tie(lhs.x_, lhs.y_) == std::tie(rhs.x_, rhs.y_);
+  double length() const { return std::sqrt(x_ * x_ + y_ * y_); }
+
+  xy normalize() const {
+    auto const len = length();
+    auto copy = *this;
+    copy.x_ /= len;
+    copy.y_ /= len;
+    return copy;
+  }
+  xy normal(bool left = true) const { return left ? xy{-y_, x_} : xy{y_, -x_}; }
+  double dot(xy const& o) const { return x_ * o.x_ + y_ * o.y_; }
+  double cross(xy const& o) const { return x_ * o.y_ - o.x_ * y_; }
+
+  friend xy operator+(xy const& lhs, xy const& rhs) {
+    return {lhs.x() + rhs.x(), lhs.y() + rhs.y()};
+  }
+
+  friend xy operator-(xy const& lhs, xy const& rhs) {
+    return {lhs.x() - rhs.x(), lhs.y() - rhs.y()};
   }
 
+  friend xy operator*(xy const& v, double s) { return {v.x() * s, v.y() * s}; }
+  friend xy operator*(double s, xy const& v) { return v * s; }
+
+  friend void operator*=(xy& v, double s) {
+    v.x_ *= s;
+    v.y_ *= s;
+  }
+
+  friend xy operator/(xy const& v, double s) { return {v.x() / s, v.y() / s}; }
+
+  friend void operator/=(xy& v, double s) {
+    v.x_ /= s;
+    v.y_ /= s;
+  }
+
+  void operator+=(xy const& o) {
+    x_ += o.x_;
+    y_ += o.y_;
+  }
+
+  void operator-=(xy const& o) {
+    x_ -= o.x_;
+    y_ -= o.y_;
+  }
+
+  bool isnan() const { return std::isnan(x_) || std::isnan(y_); }
+
+  friend bool operator==(xy const& a, xy const& b) {
+    return std::fabs(a.x_ - b.x_) < kEpsilon &&
+           std::fabs(a.y_ - b.y_) < kEpsilon;
+  }
+
+  friend bool operator!=(xy const& a, xy const& b) { return !(a == b); }
+
   friend std::ostream& operator<<(std::ostream& out, xy const& pos) {
     return out << "(" << pos.x_ << ", " << pos.y_ << ")";
   }
@@ -79,9 +131,9 @@ struct bounds {
   bounds(T const minx, T const miny, T const maxx, T const maxy)
       : minx_(minx), miny_(miny), maxx_(maxx), maxy_(maxy) {}
 
-  friend bool operator==(bounds<T> const& lhs, bounds<T> const& rhs) {
-    return std::tie(lhs.minx_, lhs.miny_, lhs.maxx_, lhs.maxy_) ==
-           std::tie(rhs.minx_, rhs.miny_, rhs.maxx_, rhs.maxy_);
+  friend bool operator==(bounds<T> const& a, bounds<T> const& b) {
+    return std::tie(a.minx_, a.miny_, a.maxx_, a.maxy_) ==
+           std::tie(b.minx_, b.miny_, b.maxx_, b.maxy_);
   }
 
   friend std::ostream& operator<<(std::ostream& out, bounds<T> const& b) {
@@ -104,12 +156,11 @@ constexpr auto kMercOriginShift = kPI * kMercEarthRadius;
 constexpr auto kMercMaxLatitude = 85.0511287798;
 
 inline merc_xy latlng_to_merc(latlng const& pos) {
-  constexpr auto d = kPI / 180.;
   auto const lat =
       std::max(std::min(kMercMaxLatitude, pos.lat_), -kMercMaxLatitude);
-  auto const sin = std::sin(lat * d);
+  auto const sin = std::sin(to_rad(lat));
 
-  return {kMercEarthRadius * pos.lng_ * d,
+  return {kMercEarthRadius * to_rad(pos.lng_),
           kMercEarthRadius * std::log((1. + sin) / (1. - sin)) / 2.};
 }
 

src/latlng.cc

@@ -77,4 +77,42 @@ uint32_t tile_hash_32(latlng const& pos) {
   return hash;
 }
 
+latlng closest_on_segment(latlng const& x, latlng const& segment_from,
+                          latlng const& segment_to) {
+  auto const merc_x = latlng_to_merc(x);
+  auto const merc_from = latlng_to_merc(segment_from);
+  auto const merc_to = latlng_to_merc(segment_to);
+
+  if (merc_x == merc_from || merc_x == merc_to) {
+    return x;
+  }
+
+  auto const seg_dir = merc_to - merc_from;
+  auto const seg_len = seg_dir.length();
+
+  if (seg_len < kEpsilon) {
+    return segment_from;
+  }
+
+  auto const start_vec = merc_x - merc_from;
+  auto const end_vec = merc_x - merc_to;
+  auto const start_angle =
+      std::acos(seg_dir.dot(start_vec) / (seg_len * start_vec.length()));
+  auto const end_angle =
+      std::acos(seg_dir.dot(end_vec) / (seg_len * end_vec.length()));
+
+  assert(!std::isnan(start_angle) && !std::isnan(end_angle));
+
+  if (start_angle >= 90.0_rad) {
+    return segment_from;
+  } else if (end_angle <= 90.0_rad) {
+    return segment_to;
+  } else {
+    // law of sines
+    auto const beta = 90.0_rad - start_angle;
+    auto const seg_offset = start_vec.length() * std::sin(beta);
+    return merc_to_latlng(merc_from + seg_offset * seg_dir.normalize());
+  }
+}
+
 }  // namespace geo