From 47d3156c57bca8c4c06d1cd925835b8c05362c67 Mon Sep 17 00:00:00 2001 From: lisyarus Date: Thu, 14 Mar 2024 08:42:58 +0300 Subject: [PATCH] Add crude point-convex body distance to cg --- libs/cg/include/psemek/cg/convex/distance.hpp | 70 +++++++++++++++++++ 1 file changed, 70 insertions(+) create mode 100644 libs/cg/include/psemek/cg/convex/distance.hpp diff --git a/libs/cg/include/psemek/cg/convex/distance.hpp b/libs/cg/include/psemek/cg/convex/distance.hpp new file mode 100644 index 00000000..2f9b0ddb --- /dev/null +++ b/libs/cg/include/psemek/cg/convex/distance.hpp @@ -0,0 +1,70 @@ +#pragma once + +#include +#include +#include + +namespace psemek::cg +{ + + namespace detail + { + + template + T distance_fast_2d(Body const & b, geom::point const & p) + { + T result = geom::limits::min(); + + auto const & vs = vertices(b); + + for (auto const & e : edges(b)) + { + auto n = geom::ort(geom::normalized(vs[e[0]] - vs[e[1]])); + geom::make_max(result, geom::dot(n, p - vs[e[0]])); + } + + return result; + } + + template + T distance_fast_3d(Body const & b, geom::point const & p) + { + T result = geom::limits::min(); + + auto const & vs = vertices(b); + + for (auto const & f : faces(b)) + { + auto n = geom::normal(vs[f[0]], vs[f[1]], vs[f[2]]); + geom::make_max(result, geom::dot(n, p - vs[f[0]])); + } + + return result; + } + + } + + // Returns an upper bound of the distance between b and p + // using only edge/face normals + template + auto distance_fast(Body const & b, Point const & p) + { + constexpr auto dim = dimension; + + static_assert(dim == 2 || dim == 3); + + if constexpr (dim == 2) + { + return detail::distance_fast_2d(b, p); + } + else if constexpr (dim == 3) + { + return detail::distance_fast_3d(b, p); + } + else + { + return; + } + } + +}