From dfcec0a1ee439ec555773166923d49aac3ee8cae Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Thu, 5 Nov 2020 16:52:05 +0300
Subject: [PATCH] Add point-triangle distance

---
 libs/geom/include/psemek/geom/distance.hpp | 31 ++++++++++++++++++++++
 1 file changed, 31 insertions(+)

diff --git a/libs/geom/include/psemek/geom/distance.hpp b/libs/geom/include/psemek/geom/distance.hpp
index e4c154cf..d283aa21 100644
--- a/libs/geom/include/psemek/geom/distance.hpp
+++ b/libs/geom/include/psemek/geom/distance.hpp
@@ -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]})));
+	}
+
 }