1 files changed, 71 insertions, 7 deletions

diff --git a/lib/ray.h b/lib/ray.h
index bc70c74..e1f43c3 100644
--- a/lib/ray.h
+++ b/lib/ray.h
@@ -1,20 +1,84 @@
 #pragma once
 
 #include "config/types.h"
+#include "maths.h"
+
+#include <algorithm>
 #include <glm/glm.hpp>
+#include <glm/gtx/intersect.hpp>
 #include <span>
 
-class Ray {
+template<typename PositionType> class Ray {
 public:
 #ifndef __cpp_aggregate_paren_init
-	Ray(Position3D start, Direction3D direction) : start {start}, direction {direction} { }
+	Ray(PositionType start, Direction3D direction) : start {start}, direction {direction} { }
 #endif
 
-	static Ray fromPoints(Position3D, Position3D);
-
-	Position3D start;
+	PositionType start;
 	Direction3D direction;
 
-	[[nodiscard]] float distanceToLine(const Position3D & a, const Position3D & b) const;
-	[[nodiscard]] bool passesCloseToEdges(const std::span<const Position3D> positions, float distance) const;
+	[[nodiscard]] PositionType::value_type
+	distanceToLine(const PositionType & p1, const PositionType & e1) const
+	{
+		// https://en.wikipedia.org/wiki/Skew_lines
+		const RelativePosition3D diff = p1 - e1;
+		const auto d1 = glm::normalize(diff);
+		const auto n = crossProduct(d1, direction);
+		const auto n2 = crossProduct(direction, n);
+		const auto c1 = p1 + PositionType((glm::dot(RelativePosition3D(start - p1), n2) / glm::dot(d1, n2)) * d1);
+		const auto difflength = glm::length(diff);
+		if (glm::length(RelativePosition3D(c1 - p1)) > difflength
+				|| glm::length(RelativePosition3D(c1 - e1)) > difflength) {
+			return std::numeric_limits<typename PositionType::value_type>::infinity();
+		}
+		return static_cast<PositionType::value_type>(glm::abs(glm::dot(n, RelativePosition3D(p1 - start))));
+	}
+
+	[[nodiscard]] bool
+	passesCloseToEdges(const std::span<const PositionType> positions, const PositionType::value_type distance) const
+	{
+		return std::adjacent_find(positions.begin(), positions.end(), [this, distance](const auto & a, const auto & b) {
+			return distanceToLine(a, b) <= distance;
+		}) != positions.end();
+	}
+
+	bool
+	intersectTriangle(const PositionType t0, const PositionType t1, const PositionType t2, BaryPosition & bary,
+			RelativeDistance & distance) const
+	{
+		if constexpr (std::is_floating_point_v<typename PositionType::value_type>) {
+			return glm::intersectRayTriangle(start, direction, t0, t1, t2, bary, distance);
+		}
+		else {
+			const RelativePosition3D t0r = t0 - start, t1r = t1 - start, t2r = t2 - start;
+			return glm::intersectRayTriangle({}, direction, t0r, t1r, t2r, bary, distance);
+		}
+	}
+
+	bool
+	intersectSphere(const PositionType centre, const PositionType::value_type size, PositionType & position,
+			Normal3D & normal) const
+	{
+		if constexpr (std::is_floating_point_v<typename PositionType::value_type>) {
+			return glm::intersectRaySphere(start, direction, centre, size, position, normal);
+		}
+		else {
+			const RelativePosition3D cr = centre - start;
+			RelativePosition3D positionF {};
+			const auto r = glm::intersectRaySphere(
+					{}, direction, cr, static_cast<RelativeDistance>(size), positionF, normal);
+			position = GlobalPosition3D(positionF) + start;
+			return r;
+		}
+	}
+};
+
+class RayFactory {
+public:
+	template<typename PositionType>
+	static Ray<PositionType>
+	fromPoints(PositionType start, PositionType p)
+	{
+		return {start, glm::normalize(p - start)};
+	}
 };