1 files changed, 14 insertions, 50 deletions

diff --git a/lib/maths.h b/lib/maths.h
index 3ef12e7..09af048 100644
--- a/lib/maths.h
+++ b/lib/maths.h
@@ -427,70 +427,34 @@ linesIntersectAt(const glm::vec<2, T, Q> Aabs, const glm::vec<2, T, Q> Babs, con
 template<std::floating_point T> constexpr auto EPSILON = 0.0001F;
 
 template<std::floating_point T>
-auto
+[[nodiscard]] constexpr auto
 isWithinLimit(T lhs, T rhs, T limit = EPSILON<T>)
 {
 	return std::abs(lhs - rhs) <= limit;
 }
 
-template<Arithmetic T, glm::qualifier Q = glm::defaultp>
-std::pair<glm::vec<2, T, Q>, bool>
-find_arc_centre(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, Angle entrye)
-{
-	if (start == end) {
-		return {start, false};
-	}
-	return find_arc_centre(start, sincos(entrys + half_pi), end, sincos(entrye - half_pi));
-}
-
-template<Arithmetic T, glm::qualifier Q = glm::defaultp>
-std::pair<glm::vec<2, T, Q>, bool>
-find_arc_centre(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end)
+template<Arithmetic T, std::floating_point D, glm::qualifier Q = glm::defaultp>
+constexpr std::pair<glm::vec<2, T, Q>, D>
+find_arc_centre(glm::vec<2, T, Q> start, glm::vec<2, D, Q> entrys, glm::vec<2, T, Q> end)
 {
 	if (start == end) {
-		return {start, false};
+		return {start, 0};
 	}
-	const auto startNormal = vector_normal(sincos(entrys) * 10'000.F);
 	const auto diffEnds = difference(end, start);
+	const auto offset = entrys.x * diffEnds.y - entrys.y * diffEnds.x;
+	if (offset == 0.F) {
+		return {start, offset};
+	}
 	const auto midEnds = start + ((end - start) / 2);
-	const auto diffNormal = vector_normal(diffEnds);
-	const auto centre = linesIntersectAt(start, start + startNormal, midEnds, midEnds + diffNormal);
-	return {*centre, normalize(vector_yaw(diffEnds) - entrys) < 0};
-}
-
-template<Arithmetic T, glm::qualifier Q = glm::defaultp>
-Angle
-find_arcs_radius(glm::vec<2, T, Q> start, Rotation2D ad, glm::vec<2, T, Q> end, Rotation2D bd)
-{
-	using std::sqrt;
-
-	// Calculates path across both arcs along the normals... pythagorean theorem... for some known radius r
-	// (2r)^2 = ((m + (X*r)) - (o + (Z*r)))^2 + ((n + (Y*r)) - (p + (W*r)))^2
-	// According to symbolabs.com equation tool, that solves for r to give:
-	// r=(-2 m X+2 X o+2 m Z-2 o Z-2 n Y+2 Y p+2 n W-2 p W-sqrt((2 m X-2 X o-2 m Z+2 o Z+2 n Y-2 Y p-2 n W+2 p W)^(2)-4
-	// (X^(2)-2 X Z+Z^(2)+Y^(2)-2 Y W+W^(2)-4) (m^(2)-2 m o+o^(2)+n^(2)-2 n p+p^(2))))/(2 (X^(2)-2 X Z+Z^(2)+Y^(2)-2 Y
-	// W+W^(2)-4))
-	// Locally simplified to work relative, removing one half of the problem and operating on relative positions.
-
-	// These exist cos limitations of online formula rearrangement, and I'm OK with that.
-	const RelativePosition2D diff {end - start};
-	const auto &o {diff.x}, &p {diff.y};
-	const auto &X {ad.x}, &Y {ad.y}, &Z {bd.x}, &W {bd.y};
-
-	return (-2 * X * o + 2 * o * Z - 2 * Y * p + 2 * p * W
-				   - sqrt(sq(2 * X * o - 2 * o * Z + 2 * Y * p - 2 * p * W)
-						   - (4 * (sq(X) - 2 * X * Z + sq(Z) + sq(Y) - 2 * Y * W + sq(W) - 4) * (sq(o) + sq(p)))))
-			/ (2 * (sq(X) - 2 * X * Z + sq(Z) + sq(Y) - 2 * Y * W + sq(W) - 4));
+	const auto centre = linesIntersectAtDirs(start, vector_normal(entrys), midEnds, vector_normal(diffEnds));
+	return {*centre, offset};
 }
 
 template<Arithmetic T, glm::qualifier Q = glm::defaultp>
-std::pair<Angle, Angle>
-find_arcs_radius(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, Angle entrye)
+constexpr std::pair<glm::vec<2, T, Q>, float>
+find_arc_centre(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end)
 {
-	const auto getrad = [&](auto leftOrRight) {
-		return find_arcs_radius(start, sincos(entrys + leftOrRight), end, sincos(entrye + leftOrRight));
-	};
-	return {getrad(-half_pi), getrad(half_pi)};
+	return find_arc_centre(start, sincos(entrys), end);
 }
 
 template<Arithmetic T>