Ray function a calculate how close it passes to a line defined by 2 points

3 files changed, 49 insertions, 0 deletions

diff --git a/lib/ray.cpp b/lib/ray.cpp
new file mode 100644
index 0000000..1e30ae4
--- /dev/null
+++ b/lib/ray.cpp
@@ -0,0 +1,18 @@
+#include "ray.hpp"
+
+float
+Ray::distanceToLine(const glm::vec3 & p1, const glm::vec3 & e1) const
+{
+	// https://en.wikipedia.org/wiki/Skew_lines
+	const auto diff = p1 - e1;
+	const auto d1 = glm::normalize(diff);
+	const auto &p2 = start, &d2 = direction;
+	const auto n = glm::cross(d1, d2);
+	const auto n2 = glm::cross(d2, n);
+	const auto c1 = p1 + (glm::dot((p2 - p1), n2) / glm::dot(d1, n2)) * d1;
+	const auto difflength = glm::length(diff);
+	if (glm::length(c1 - p1) > difflength || glm::length(c1 - e1) > difflength) {
+		return std::numeric_limits<float>::infinity();
+	}
+	return glm::abs(glm::dot(n, p1 - p2));
+}
diff --git a/lib/ray.hpp b/lib/ray.hpp
index 4c0710a..bce4a30 100644
--- a/lib/ray.hpp
+++ b/lib/ray.hpp
@@ -8,4 +8,6 @@ public:
 
 	glm::vec3 start;
 	glm::vec3 direction;
+
+	float distanceToLine(const glm::vec3 & a, const glm::vec3 & b) const;
 };
diff --git a/test/test-maths.cpp b/test/test-maths.cpp
index 1f2a096..70cf206 100644
--- a/test/test-maths.cpp
+++ b/test/test-maths.cpp
@@ -287,3 +287,32 @@ BOOST_AUTO_TEST_CASE(camera_clicks)
 	BOOST_CHECK_CLOSE_VEC(camera.unProject({right, centre}).direction, glm::normalize(::north));
 	BOOST_CHECK_CLOSE_VEC(camera.unProject({left, centre}).direction, glm::normalize(::west));
 }
+
+template<typename T = float>
+auto
+n_test_points_between(std::size_t n = 2, T min = -100.F, T max = 100.F)
+{
+	return boost::unit_test::data::xrange(n) ^ boost::unit_test::data::random(min, max);
+}
+
+BOOST_DATA_TEST_CASE(rayLineDistance,
+		n_test_points_between() * // n1x
+				n_test_points_between() * // n1y
+				n_test_points_between() * // n1z
+				n_test_points_between() * // n2x
+				n_test_points_between() * // n2y
+				n_test_points_between() * // n2z
+				n_test_points_between() * // cx
+				n_test_points_between() * // cy
+				n_test_points_between(), // cz
+		i1, n1x, i2, n1y, i3, n1z, i4, n2x, i5, n2y, i6, n2z, i7, cx, i8, cy, i9, cz)
+{
+	const glm::vec3 n1 {n1x, n1y, n1z}, n2 {n2x, n2y, n2z}, c {cx, cy, cz};
+
+	const auto nstep = n2 - n1;
+	for (float along = 0.2F; along <= 0.8F; along += 0.1F) {
+		const auto target = n1 + (along * nstep);
+		const auto direction = glm::normalize(target - c);
+		BOOST_CHECK_LE(Ray(c, direction).distanceToLine(n1, n2), 0.01F);
+	}
+}