From 81cafa6f054665b0aad7ca782db5b1f1380cb4af Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Sat, 26 Sep 2020 23:23:40 +0300
Subject: [PATCH] Add ray-simplex intersection

---
 .../geom/include/psemek/geom/intersection.hpp | 40 +++++++++++++++++++
 1 file changed, 40 insertions(+)

diff --git a/libs/geom/include/psemek/geom/intersection.hpp b/libs/geom/include/psemek/geom/intersection.hpp
index dbb72cf2..22fbe183 100644
--- a/libs/geom/include/psemek/geom/intersection.hpp
+++ b/libs/geom/include/psemek/geom/intersection.hpp
@@ -7,7 +7,9 @@
 #include <psemek/geom/box.hpp>
 #include <psemek/geom/orientation.hpp>
 #include <psemek/geom/contains.hpp>
+#include <psemek/geom/gauss.hpp>
 
+#include <optional>
 #include <variant>
 #include <type_traits>
 
@@ -128,4 +130,42 @@ namespace psemek::geom
 		return !intersection(r, b).empty();
 	}
 
+	template <typename T, std::size_t N>
+	std::optional<T> intersection(ray<T, N> const & r, simplex<point<T, N>, N - 1> const & s)
+	{
+		geom::matrix<T, N, N> m;
+		geom::vector<T, N> b;
+
+		for (std::size_t i = 0; i < N; ++i)
+		{
+			m[i][0] = r.direction[i];
+			b[i] = s[0][i] - r.origin[i];
+
+			for (std::size_t j = 1; j < N; ++j)
+			{
+				m[i][j] = s[j][i] - s[0][i];
+			}
+		}
+
+		if (!geom::gauss(m, b))
+			return std::nullopt;
+
+		T sum{};
+		for (std::size_t j = 1; j < N; ++j)
+		{
+			if (b[j] >= T{}) return std::nullopt;
+			sum += (-b[j]);
+		}
+
+		if (sum > T{1}) return std::nullopt;
+
+		return b[0];
+	}
+
+	template <typename T, std::size_t N>
+	bool intersect(ray<T, N> const & r, simplex<T, N> const & s)
+	{
+		return static_cast<bool>(intersection(r, s));
+	}
+
 }