diff --git a/libs/geom/include/psemek/geom/distance.hpp b/libs/geom/include/psemek/geom/distance.hpp new file mode 100644 index 00000000..a1935210 --- /dev/null +++ b/libs/geom/include/psemek/geom/distance.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include +#include +#include + +namespace psemek::geom +{ + + template + T distance(point const & p, box const & b) + { + T s = T{}; + for (std::size_t i = 0; i < N; ++i) + { + if (p[i] < b[i].min) + s += sqr(b[i].min - p[i]); + else if (p[i] > b[i].max) + s += sqr(p[i] - b[i].max); + } + return std::sqrt(s); + } + +}