Add ArcSegment

Extends Arc, with method for determining intersection point with line segment

3 files changed, 110 insertions, 0 deletions

diff --git a/lib/maths.h b/lib/maths.h
index 7f2d7b4..f43321a 100644
--- a/lib/maths.h
+++ b/lib/maths.h
@@ -1,6 +1,8 @@
 #pragma once
 
 #include "config/types.h"
+#include <algorithm>
+#include <array>
 #include <cmath>
 #include <glm/glm.hpp>
 #include <glm/gtc/constants.hpp>
@@ -36,6 +38,26 @@ struct Arc : public std::pair<Angle, Angle> {
 	}
 };
 
+template<typename T, glm::qualifier Q = glm::defaultp> struct ArcSegment : public Arc {
+	using PointType = glm::vec<2, T, Q>;
+
+	constexpr ArcSegment(PointType centre, PointType ep0, PointType ep1);
+
+	PointType centre;
+	PointType ep0;
+	PointType ep1;
+	RelativeDistance radius;
+
+	[[nodiscard]] constexpr std::optional<glm::vec<2, T, Q>> crossesLineAt(
+			const glm::vec<2, T, Q> & lineStart, const glm::vec<2, T, Q> & lineEnd) const;
+
+	[[nodiscard]] constexpr bool
+	angleWithinArc(Angle angle) const
+	{
+		return first <= angle && angle <= second;
+	}
+};
+
 constexpr const RelativePosition3D up {0, 0, 1}; // NOLINT(readability-identifier-length)
 constexpr const RelativePosition3D down {0, 0, -1};
 constexpr const RelativePosition3D north {0, 1, 0};
@@ -471,3 +493,49 @@ operator"" _degrees(long double degrees)
 {
 	return static_cast<float>(degrees) * degreesToRads;
 }
+
+// Late implementations due to dependencies
+template<typename T, glm::qualifier Q>
+constexpr ArcSegment<T, Q>::ArcSegment(PointType centre, PointType ep0, PointType ep1) :
+	Arc {centre, ep0, ep1}, centre {centre}, ep0 {ep0}, ep1 {ep1}, radius {glm::length(difference(centre, ep0))}
+{
+}
+
+template<typename T, glm::qualifier Q>
+[[nodiscard]] constexpr std::optional<glm::vec<2, T, Q>>
+ArcSegment<T, Q>::crossesLineAt(const glm::vec<2, T, Q> & lineStart, const glm::vec<2, T, Q> & lineEnd) const
+{
+	// Based on formulas from https://mathworld.wolfram.com/Circle-LineIntersection.html
+	const auto lineDiff = difference(lineEnd, lineStart);
+	const auto lineLen = glm::length(lineDiff);
+	const auto lineRelStart = difference(lineStart, centre);
+	const auto lineRelEnd = difference(lineEnd, centre);
+	const auto determinant = (lineRelStart.x * lineRelEnd.y) - (lineRelEnd.x * lineRelStart.y);
+	const auto discriminant = (radius * radius * lineLen * lineLen) - (determinant * determinant);
+	if (discriminant < 0) {
+		return std::nullopt;
+	}
+
+	const auto rootDiscriminant = std::sqrt(discriminant);
+	const auto drdr = lineLen * lineLen;
+	const RelativeDistance sgn = (lineDiff.y < 0 ? -1 : 1);
+	std::array points {
+			RelativePosition2D {((determinant * lineDiff.y) + sgn * lineDiff.x * rootDiscriminant),
+					((-determinant * lineDiff.x) + std::abs(lineDiff.y) * rootDiscriminant)}
+					/ drdr,
+			RelativePosition2D {((determinant * lineDiff.y) - sgn * lineDiff.x * rootDiscriminant),
+					((-determinant * lineDiff.x) - std::abs(lineDiff.y) * rootDiscriminant)}
+					/ drdr,
+	};
+	const auto end
+			= std::remove_if(points.begin(), points.end(), [this, lineRelStart, lineDiff, drdr](const auto point) {
+				  const auto dot = glm::dot(lineDiff, point - lineRelStart);
+				  return !angleWithinArc(vector_yaw(point)) || dot < 0 || dot > drdr;
+	});
+	if (points.begin() == end) {
+		return std::nullopt;
+	}
+	return centre + *std::ranges::min_element(points.begin(), end, {}, [lineRelStart](const auto point) {
+		return glm::distance(lineRelStart, point);
+	});
+}
diff --git a/lib/stream_support.h b/lib/stream_support.h
index f21622a..f5c5e37 100644
--- a/lib/stream_support.h
+++ b/lib/stream_support.h
@@ -4,6 +4,7 @@
 #include <glm/glm.hpp>
 #include <iostream>
 #include <maths.h>
+#include <optional>
 #include <source_location>
 #include <span>
 #include <sstream>
@@ -82,6 +83,16 @@ namespace std {
 	{
 		return s << EnumTypeDetails<E>::typeName << "::" << EnumDetails<E>::to_string(e).value();
 	}
+
+	template<typename T>
+	inline std::ostream &
+	operator<<(std::ostream & s, const std::optional<T> & v)
+	{
+		if (v) {
+			return s << *v;
+		}
+		return s << "nullopt";
+	}
 }
 
 template<typename T>
diff --git a/test/test-maths.cpp b/test/test-maths.cpp
index 1278c44..aa2b9c8 100644
--- a/test/test-maths.cpp
+++ b/test/test-maths.cpp
@@ -375,3 +375,34 @@ BOOST_AUTO_TEST_CASE(triangle3d_helpers)
 
 	BOOST_CHECK_CLOSE(t.area(), 12.5F, 0.01F);
 }
+
+using ArcLineIntersectData = std::tuple<GlobalPosition2D, GlobalPosition2D, GlobalPosition2D, GlobalPosition2D,
+		GlobalPosition2D, std::optional<GlobalPosition2D>>;
+
+BOOST_DATA_TEST_CASE(arcline_intersection,
+		boost::unit_test::data::make<ArcLineIntersectData>({
+				{{0, 0}, {0, 100}, {100, 0}, {200, 0}, {0, 200}, std::nullopt},
+				{{0, 0}, {0, 100}, {100, 0}, {0, 0}, {10, 10}, std::nullopt},
+				{{0, 0}, {0, 100}, {100, 0}, {0, 0}, {100, 100}, GlobalPosition2D {71, 71}},
+				{{15, 27}, {15, 127}, {115, 27}, {15, 27}, {115, 127}, GlobalPosition2D {86, 98}},
+				{{0, 0}, {0, 100}, {100, 0}, {0, 0}, {-100, -100}, std::nullopt},
+				{{0, 0}, {0, 100}, {100, 0}, {-10, 125}, {125, -10}, GlobalPosition2D {16, 99}},
+				{{0, 0}, {0, 100}, {100, 0}, {125, -10}, {-10, 125}, GlobalPosition2D {99, 16}},
+				{{0, 0}, {0, 100}, {100, 0}, {10, 125}, {125, -10}, GlobalPosition2D {38, 93}},
+				{{0, 0}, {0, 100}, {100, 0}, {12, 80}, {125, -10}, GlobalPosition2D {99, 10}},
+				{{0, 0}, {0, 100}, {100, 0}, {40, 80}, {125, -10}, GlobalPosition2D {98, 18}},
+				{{0, 0}, {0, 100}, {100, 0}, {40, 80}, {80, 20}, std::nullopt},
+				{{0, 0}, {0, 100}, {100, 0}, {40, 80}, {80, 80}, GlobalPosition2D {60, 80}},
+				{{0, 0}, {0, 100}, {100, 0}, {80, 40}, {80, 80}, GlobalPosition2D {80, 60}},
+				{{310002000, 490203000}, {310202000, 490203000}, {310002000, 490003000}, {310200000, 490150000},
+						{310150000, 490150000}, GlobalPosition2D {310194850, 490150000}},
+		}),
+		centre, arcStart, arcEnd, lineStart, lineEnd, intersection)
+{
+	const ArcSegment arc {centre, arcStart, arcEnd};
+	BOOST_TEST_INFO(arc.first);
+	BOOST_TEST_INFO(arc.second);
+	BOOST_TEST_INFO(arc.length());
+
+	BOOST_CHECK_EQUAL(arc.crossesLineAt(lineStart, lineEnd), intersection);
+}