diff options
| -rw-r--r-- | lib/maths.h | 29 | ||||
| -rw-r--r-- | test/test-maths.cpp | 8 |
2 files changed, 37 insertions, 0 deletions
diff --git a/lib/maths.h b/lib/maths.h index 14a29d1..f6130e7 100644 --- a/lib/maths.h +++ b/lib/maths.h @@ -5,6 +5,7 @@ #include <glm/glm.hpp> #include <glm/gtc/constants.hpp> #include <numeric> +#include <optional> #include <stdexcept> #include <utility> @@ -326,6 +327,34 @@ normalize(T ang) return ang; } +template<typename T> using CalcType = std::conditional_t<std::is_floating_point_v<T>, T, int64_t>; + +template<typename 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) +{ + 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); + + if (determinant == 0) { + return std::nullopt; + } + return Aabs + CVec {(b1 * c2) / -determinant, (a1 * c2) / determinant}; +} + template<typename T, glm::qualifier Q> std::pair<glm::vec<2, T, Q>, bool> find_arc_centre(glm::vec<2, T, Q> start, Rotation2D startDir, glm::vec<2, T, Q> end, Rotation2D endDir) diff --git a/test/test-maths.cpp b/test/test-maths.cpp index f7f34b3..b9d08bb 100644 --- a/test/test-maths.cpp +++ b/test/test-maths.cpp @@ -333,3 +333,11 @@ BOOST_DATA_TEST_CASE(rayLineDistance, BOOST_CHECK_LE(Ray<RelativePosition3D>(c, direction).distanceToLine(n1, n2), 0.01F); } } + +static_assert(linesIntersectAt(glm::ivec2 {10, 10}, {40, 40}, {10, 80}, {20, 40}).value().x == 24); +static_assert(linesIntersectAt(glm::vec2 {10, 10}, {40, 40}, {10, 80}, {20, 40}).value().y == 24); +static_assert(linesIntersectAt(GlobalPosition2D {311000100, 491100100}, {311050000, 491150000}, {312000100, 491200100}, + {311000100, 491100100}) + .value() + == GlobalPosition2D {311000100, 491100100}); +static_assert(!linesIntersectAt(glm::dvec2 {0, 1}, {0, 4}, {1, 8}, {1, 4}).has_value()); |