Add ArcSegment

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

2 files changed, 79 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>