Refactored linesIntersectAt

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

1 files changed, 14 insertions, 0 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)
 {