From 98ff134d3ad3f8825f34592c2ffe26097fe24f01 Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Mon, 28 Sep 2020 11:03:30 +0300
Subject: [PATCH] Add point-box distance

---
 libs/geom/include/psemek/geom/distance.hpp | 24 ++++++++++++++++++++++
 1 file changed, 24 insertions(+)
 create mode 100644 libs/geom/include/psemek/geom/distance.hpp

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 <psemek/geom/point.hpp>
+#include <psemek/geom/box.hpp>
+#include <psemek/geom/math.hpp>
+
+namespace psemek::geom
+{
+
+	template <typename T, std::size_t N>
+	T distance(point<T, N> const & p, box<T, N> 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);
+	}
+
+}