lisyarus/psemek
1
0
Fork 1
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add point-triangle distance

Browse source
This commit is contained in:
Nikita Lisitsa 2020-11-05 16:52:05 +03:00
parent e4be2e9322
commit dfcec0a1ee
1 changed files with 31 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

31
libs/geom/include/psemek/geom/distance.hpp
View file

@ -4,6 +4,7 @@
#include <psemek/geom/box.hpp>
#include <psemek/geom/math.hpp>
#include <psemek/geom/simplex.hpp>
#include <psemek/geom/gauss.hpp>
namespace psemek::geom
{
@ -35,4 +36,34 @@ namespace psemek::geom
return distance(p, lerp(s[0], s[1], t));
}
template <typename T, std::size_t N>
T distance(point<T, N> const & p, triangle<point<T, N>> const & s)
{
auto n = normal(s[0], s[1], s[2]);
auto v = p - s[0];
auto l = dot(n, v);
v -= n * l;
matrix<T, 3, 2> m;
for (std::size_t i = 0; i < 3; ++i)
{
m[i][0] = (s[1] - s[0])[i];
m[i][1] = (s[2] - s[0])[i];
}
auto res = least_squares(m, v);
if (res)
{
auto u = *res;
if (u[0] >= 0 && u[1] >= 0 && u[0] + u[1] <= 1)
{
return std::abs(l);
}
}
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]})));
}
}