diff --git a/libs/geom/include/psemek/geom/distance.hpp b/libs/geom/include/psemek/geom/distance.hpp index c5d9c5a4..c29c93c1 100644 --- a/libs/geom/include/psemek/geom/distance.hpp +++ b/libs/geom/include/psemek/geom/distance.hpp @@ -6,6 +6,7 @@ #include #include #include +#include namespace psemek::geom { @@ -93,4 +94,71 @@ namespace psemek::geom return std::min(distance(p, simplex{s[0], s[1]}), std::min(distance(p, simplex{s[1], s[2]}), distance(p, simplex{s[2], s[0]}))); } + template + std::pair> signed_polygon_distance_direction(point const & p, Iterator begin, Iterator end) + { + std::pair> result{limits::max(), vector::zero()}; + + for (auto it = begin; it < end; ++it) + { + auto v = p - *it; + auto l = length(v); + if (make_min(result.first, l)) + result.second = v / l; + } + + for (auto it = begin, prev = std::prev(end); it != end; prev = it++) + { + auto v = p - *prev; + auto r = *it - *prev; + auto t = geom::dot(v, r) / geom::dot(r, r); + + if (0 <= t && t <= 1) + { + v -= r * t; + auto l = length(v); + if (make_min(result.first, l)) + result.second = v / l; + } + } + + if (polygon_contains(begin, end, p)) + { + result.first = -result.first; + result.second = -result.second; + } + + return result; + } + + template + std::pair> signed_polygon_distance_direction(point const & p, Polygon const & polygon) + { + return signed_polygon_distance_direction(p, util::begin(polygon), util::end(polygon)); + } + + template + T signed_polygon_distance(point const & p, Iterator begin, Iterator end) + { + return signed_polygon_distance_direction(p, begin, end).first; + } + + template + T signed_polygon_distance(point const & p, Polygon const & polygon) + { + return signed_polygon_distance(p, util::begin(polygon), util::end(polygon)); + } + + template + T polygon_distance(point const & p, Iterator begin, Iterator end) + { + return std::max(T{0}, signed_polygon_distance(p, begin, end)); + } + + template + T polygon_distance(point const & p, Polygon const & polygon) + { + return polygon_distance(p, util::begin(polygon), util::end(polygon)); + } + }