diff --git a/libs/geom/include/psemek/geom/simplex.hpp b/libs/geom/include/psemek/geom/simplex.hpp index 39483864..3ff6b1a9 100644 --- a/libs/geom/include/psemek/geom/simplex.hpp +++ b/libs/geom/include/psemek/geom/simplex.hpp @@ -43,4 +43,15 @@ namespace psemek::geom return os << ')'; } + template + T distance(point const & p, segment> const & s) + { + T const t = dot(p - s[0], s[1] - s[0]) / length_sqr(s[1] - s[0]); + + if (t >= 0 && t <= 1) + return distance(p, lerp(s[0], s[1], t)); + + return std::min(distance(p, s[0]), distance(p, s[1])); + } + }