From dbd181a9e49b3b32400eb812ff224074413d2ef4 Mon Sep 17 00:00:00 2001 From: Dan Goodliffe Date: Tue, 12 Nov 2024 20:19:03 +0000 Subject: Add linesIntersectAt function 2 dimensional line intersection point --- lib/maths.h | 29 +++++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) (limited to 'lib/maths.h') 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 #include #include +#include #include #include @@ -326,6 +327,34 @@ normalize(T ang) return ang; } +template using CalcType = std::conditional_t, T, int64_t>; + +template +[[nodiscard]] constexpr std::optional> +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; + 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 std::pair, bool> find_arc_centre(glm::vec<2, T, Q> start, Rotation2D startDir, glm::vec<2, T, Q> end, Rotation2D endDir) -- cgit v1.2.3 From 2d1efc4ff2e8b4bd517fbe0cf5fb1637007dd488 Mon Sep 17 00:00:00 2001 From: Dan Goodliffe Date: Fri, 29 Nov 2024 19:58:30 +0000 Subject: Constrained Arithmatic type concept --- lib/maths.h | 46 +++++++++++++++++++++++++--------------------- 1 file changed, 25 insertions(+), 21 deletions(-) (limited to 'lib/maths.h') diff --git a/lib/maths.h b/lib/maths.h index f6130e7..3959896 100644 --- a/lib/maths.h +++ b/lib/maths.h @@ -9,8 +9,11 @@ #include #include +template +concept Arithmetic = std::is_arithmetic_v; + struct Arc : public std::pair { - template + template requires(Lc >= 2, Le >= 2) Arc(const glm::vec & centre, const glm::vec & e0p, const glm::vec & e1p) : Arc {RelativePosition2D {e0p.xy() - centre.xy()}, RelativePosition2D {e1p.xy() - centre.xy()}} @@ -119,7 +122,7 @@ namespace { } // Helper to lookup into a matrix given an xy vector coordinate - template + template constexpr auto & operator^(glm::mat & matrix, const glm::vec<2, I> rowCol) { @@ -201,7 +204,7 @@ rotate_yp(const glm::vec<2, T, Q> & angles) return rotate_yp(angles.y, angles.x); } -template +template requires(D >= 2) constexpr auto vector_yaw(const glm::vec & diff) @@ -209,7 +212,7 @@ vector_yaw(const glm::vec & diff) return std::atan2(diff.x, diff.y); } -template +template requires(D >= 3) constexpr auto vector_pitch(const glm::vec & diff) @@ -217,7 +220,7 @@ vector_pitch(const glm::vec & diff) return std::atan(diff.z); } -template +template constexpr glm::vec<2, T, Q> vector_normal(const glm::vec<2, T, Q> & vector) { @@ -231,7 +234,7 @@ round_frac(const T value, const T frac) return std::round(value / frac) * frac; } -template +template requires requires(T value) { value * value; } constexpr auto sq(T value) @@ -264,14 +267,15 @@ crossProduct(const glm::vec<3, T, Q> & valueA, const glm::vec<3, T, Q> & valueB) return glm::cross(valueA, valueB); } -template +template constexpr auto ratio(const Ta valueA, const Tb valueB) { - return (static_cast(valueA) / static_cast(valueB)); + using Common = std::common_type_t; + return static_cast((static_cast(valueA) / static_cast(valueB))); } -template +template constexpr auto ratio(const glm::vec<2, T, Q> & value) { @@ -285,14 +289,14 @@ perspective_divide(const glm::vec<4, T, Q> & value) return value / value.w; } -template +template constexpr glm::vec operator||(const glm::vec valueA, const glm::vec valueB) { return {valueA, valueB}; } -template +template constexpr glm::vec operator||(const glm::vec valueA, const T valueB) { @@ -327,9 +331,9 @@ normalize(T ang) return ang; } -template using CalcType = std::conditional_t, T, int64_t>; +template using CalcType = std::conditional_t, T, int64_t>; -template +template [[nodiscard]] constexpr std::optional> 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) @@ -355,7 +359,7 @@ linesIntersectAt(const glm::vec<2, T, Q> Aabs, const glm::vec<2, T, Q> Babs, con return Aabs + CVec {(b1 * c2) / -determinant, (a1 * c2) / determinant}; } -template +template std::pair, bool> find_arc_centre(glm::vec<2, T, Q> start, Rotation2D startDir, glm::vec<2, T, Q> end, Rotation2D endDir) { @@ -368,7 +372,7 @@ find_arc_centre(glm::vec<2, T, Q> start, Rotation2D startDir, glm::vec<2, T, Q> throw std::runtime_error("no intersection"); } -template +template std::pair, bool> find_arc_centre(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, Angle entrye) { @@ -378,7 +382,7 @@ find_arc_centre(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, An return find_arc_centre(start, sincos(entrys + half_pi), end, sincos(entrye - half_pi)); } -template +template Angle find_arcs_radius(glm::vec<2, T, Q> start, Rotation2D ad, glm::vec<2, T, Q> end, Rotation2D bd) { @@ -403,7 +407,7 @@ find_arcs_radius(glm::vec<2, T, Q> start, Rotation2D ad, glm::vec<2, T, Q> end, / (2 * (sq(X) - 2 * X * Z + sq(Z) + sq(Y) - 2 * Y * W + sq(W) - 4)); } -template +template std::pair find_arcs_radius(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, Angle entrye) { @@ -413,7 +417,7 @@ find_arcs_radius(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, A return {getrad(-half_pi), getrad(half_pi)}; } -template +template auto midpoint(const std::pair & v) { @@ -421,7 +425,7 @@ midpoint(const std::pair & v) } // std::pow is not constexpr -template +template requires requires(T n) { n *= n; } constexpr T pow(const T base, std::integral auto exp) @@ -434,14 +438,14 @@ pow(const T base, std::integral auto exp) } // Conversions -template +template constexpr auto mph_to_ms(T v) { return v / 2.237L; } -template +template constexpr auto kph_to_ms(T v) { -- cgit v1.2.3