diff options
| -rw-r--r-- | lib/maths.h | 68 | ||||
| -rw-r--r-- | lib/stream_support.h | 11 | ||||
| -rw-r--r-- | test/test-maths.cpp | 31 |
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); +} |