Refactored linesIntersectAt

Splits out the underlying logic for two points and their direction.
Refactors to use standard vector maths/functions.

2 files changed, 31 insertions, 18 deletions

diff --git a/lib/maths.cpp b/lib/maths.cpp
index 12e0681..000cea7 100644
--- a/lib/maths.cpp
+++ b/lib/maths.cpp
@@ -19,6 +19,7 @@ flat_orientation(const Direction3D & diff)
 	return (std::isnan(e[0][0])) ? oneeighty : e;
 }
 
+// NOLINTBEGIN(readability-magic-numbers)
 static_assert(pow(1, 0) == 1);
 static_assert(pow(1, 1) == 1);
 static_assert(pow(1, 2) == 1);
@@ -31,6 +32,19 @@ static_assert(pow(3, 1) == 3);
 static_assert(pow(3, 2) == 9);
 static_assert(pow(pi, 3) == 31.006278991699219F);
 
+static_assert(!linesIntersectAt<int>({0, 10}, {10, 10}, {10, 0}, {0, 0}).has_value());
+static_assert(*linesIntersectAt<int>({0, 0}, {10, 10}, {10, 0}, {0, 10}) == GlobalPosition2D {5, 5});
+static_assert(*linesIntersectAt<int>({300'000'000, 400'000'00}, {300'010'000, 400'010'00}, {310'010'000, 410'000'00},
+					  {310'000'000, 410'010'00})
+		== GlobalPosition2D {310'005'000, 410'005'00});
+
+constexpr auto NORTH2D = RelativePosition2D(north);
+constexpr auto EAST2D = RelativePosition2D(east);
+static_assert(!linesIntersectAtDirs<int>({0, 0}, NORTH2D, {10, 10}, NORTH2D).has_value());
+static_assert(linesIntersectAtDirs<int>({0, 0}, NORTH2D, {10, 10}, EAST2D) == GlobalPosition2D {0, 10});
+static_assert(linesIntersectAtDirs<int>({0, 0}, EAST2D, {10, 10}, NORTH2D) == GlobalPosition2D {10, 0});
+// NOLINTEND(readability-magic-numbers)
+
 float
 operator""_mph(const long double v)
 {
diff --git a/lib/maths.h b/lib/maths.h
index 86f2e05..8bc728f 100644
--- a/lib/maths.h
+++ b/lib/maths.h
@@ -399,30 +399,29 @@ normalize(T ang)
 	return ang;
 }
 
-template<Arithmetic T, glm::qualifier Q = glm::defaultp>
+template<Arithmetic T, std::floating_point D, 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)
+linesIntersectAtDirs(const glm::vec<2, T, Q> Aabs, const glm::vec<2, D, Q> Adir, const glm::vec<2, T, Q> Cabs,
+		const glm::vec<2, D, Q> Cdir)
 {
-	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);
+	const auto abNormal = vector_normal(Adir);
+	const auto cdNormal = vector_normal(Cdir);
+	const auto determinant = (abNormal.x * cdNormal.y) - (cdNormal.x * abNormal.y);
 
 	if (determinant == 0) {
 		return std::nullopt;
 	}
-	return Aabs + CVec {(b1 * c2) / -determinant, (a1 * c2) / determinant};
+	const auto Crel = difference(Cabs, Aabs);
+	const auto c2 = (cdNormal.x * Crel.x) + (cdNormal.y * Crel.y);
+	return Aabs + vector_normal((abNormal * c2) / determinant);
+}
+
+template<Arithmetic 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)
+{
+	return linesIntersectAtDirs(Aabs, difference(Babs, Aabs), Cabs, difference(Dabs, Cabs));
 }
 
 template<std::floating_point T> constexpr auto EPSILON = 0.0001F;