Refactored linesIntersectAt

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

1 files changed, 17 insertions, 18 deletions

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;