Add linesIntersectAt function

2 dimensional line intersection point

2 files changed, 37 insertions, 0 deletions

diff --git a/lib/maths.h b/lib/maths.h
index 14a29d1..f6130e7 100644
--- a/lib/maths.h
+++ b/lib/maths.h
@@ -5,6 +5,7 @@
 #include <glm/glm.hpp>
 #include <glm/gtc/constants.hpp>
 #include <numeric>
+#include <optional>
 #include <stdexcept>
 #include <utility>
 
@@ -326,6 +327,34 @@ normalize(T ang)
 	return ang;
 }
 
+template<typename T> using CalcType = std::conditional_t<std::is_floating_point_v<T>, T, int64_t>;
+
+template<typename T, glm::qualifier Q = glm::defaultp>
+[[nodiscard]] constexpr std::optional<glm::vec<2, T, Q>>
+linesIntersectAt(const glm::vec<2, T, Q> Aabs, const glm::vec<2, T, Q> Babs, const glm::vec<2, T, Q> Cabs,
+		const glm::vec<2, T, Q> Dabs)
+{
+	using CT = CalcType<T>;
+	using CVec = glm::vec<2, CT, Q>;
+	// Line AB represented as a1x + b1y = c1
+	const CVec Brel = Babs - Aabs;
+	const CT a1 = Brel.y;
+	const CT b1 = -Brel.x;
+
+	// Line CD represented as a2x + b2y = c2
+	const CVec Crel = Cabs - Aabs, Del = Dabs - Aabs;
+	const CT a2 = Del.y - Crel.y;
+	const CT b2 = Crel.x - Del.x;
+	const CT c2 = (a2 * Crel.x) + (b2 * Crel.y);
+
+	const auto determinant = (a1 * b2) - (a2 * b1);
+
+	if (determinant == 0) {
+		return std::nullopt;
+	}
+	return Aabs + CVec {(b1 * c2) / -determinant, (a1 * c2) / determinant};
+}
+
 template<typename T, glm::qualifier Q>
 std::pair<glm::vec<2, T, Q>, bool>
 find_arc_centre(glm::vec<2, T, Q> start, Rotation2D startDir, glm::vec<2, T, Q> end, Rotation2D endDir)
diff --git a/test/test-maths.cpp b/test/test-maths.cpp
index f7f34b3..b9d08bb 100644
--- a/test/test-maths.cpp
+++ b/test/test-maths.cpp
@@ -333,3 +333,11 @@ BOOST_DATA_TEST_CASE(rayLineDistance,
 		BOOST_CHECK_LE(Ray<RelativePosition3D>(c, direction).distanceToLine(n1, n2), 0.01F);
 	}
 }
+
+static_assert(linesIntersectAt(glm::ivec2 {10, 10}, {40, 40}, {10, 80}, {20, 40}).value().x == 24);
+static_assert(linesIntersectAt(glm::vec2 {10, 10}, {40, 40}, {10, 80}, {20, 40}).value().y == 24);
+static_assert(linesIntersectAt(GlobalPosition2D {311000100, 491100100}, {311050000, 491150000}, {312000100, 491200100},
+					  {311000100, 491100100})
+					  .value()
+		== GlobalPosition2D {311000100, 491100100});
+static_assert(!linesIntersectAt(glm::dvec2 {0, 1}, {0, 4}, {1, 8}, {1, 4}).has_value());