From 56100b1c4cb02db7608763dddd77f8052a533dae Mon Sep 17 00:00:00 2001 From: Dan Goodliffe Date: Mon, 21 Oct 2024 18:30:16 +0100 Subject: Further template maths functions --- lib/maths.h | 53 +++++++++++++++++++++++++++++++---------------------- 1 file changed, 31 insertions(+), 22 deletions(-) (limited to 'lib') diff --git a/lib/maths.h b/lib/maths.h index 94e357b..90ddb69 100644 --- a/lib/maths.h +++ b/lib/maths.h @@ -87,43 +87,52 @@ glm::mat4 flat_orientation(const Rotation3D & diff); namespace { // Helpers // C++ wrapper for C's sincosf, but with references, not pointers - constexpr auto - sincosf(float angle, float & sinOut, float & cosOut) + template + constexpr void + sincos(T angle, T & sinOut, T & cosOut) { if consteval { - sinOut = ::sinf(angle); - cosOut = ::cosf(angle); + sinOut = std::sin(angle); + cosOut = std::cos(angle); } else { - ::sincosf(angle, &sinOut, &cosOut); + if constexpr (std::is_same_v) { + ::sincosf(angle, &sinOut, &cosOut); + } + else if constexpr (std::is_same_v) { + ::sincos(angle, &sinOut, &cosOut); + } + else if constexpr (std::is_same_v) { + ::sincosl(angle, &sinOut, &cosOut); + } } } - template + template constexpr auto - sincosf(const T angle) + sincos(const T angle) { - Rotation2D sincosOut; - sincosf(angle, sincosOut.x, sincosOut.y); + glm::vec<2, T, Q> sincosOut {}; + sincos(angle, sincosOut.x, sincosOut.y); return sincosOut; } // Helper to lookup into a matrix given an xy vector coordinate - template + template constexpr auto & - operator^(M & matrix, glm::vec<2, I> rowCol) + operator^(glm::mat & matrix, const glm::vec<2, I> rowCol) { return matrix[rowCol.x][rowCol.y]; } // Create a matrix for the angle, given the targets into the matrix - template + template constexpr auto - rotation(typename M::value_type angle, glm::vec<2, I> cos1, glm::vec<2, I> sin1, glm::vec<2, I> cos2, - glm::vec<2, I> negSin1) + rotation(const T angle, const glm::vec<2, I> cos1, const glm::vec<2, I> sin1, const glm::vec<2, I> cos2, + const glm::vec<2, I> negSin1) { - M out(1); - sincosf(angle, out ^ sin1, out ^ cos1); + glm::mat out(1); + sincos(angle, out ^ sin1, out ^ cos1); out ^ cos2 = out ^ cos1; out ^ negSin1 = -(out ^ sin1); return out; @@ -136,7 +145,7 @@ template, glm::length_t>(angle, {0, 0}, {0, 1}, {1, 1}, {1, 0}); + return rotation(angle, {0, 0}, {0, 1}, {1, 1}, {1, 0}); } // Create a yaw transformation matrix @@ -145,7 +154,7 @@ template, glm::length_t>(angle, {0, 0}, {1, 0}, {1, 1}, {0, 1}); + return rotation(angle, {0, 0}, {1, 0}, {1, 1}, {0, 1}); } // Create a roll transformation matrix @@ -154,7 +163,7 @@ template, glm::length_t>(angle, {0, 0}, {2, 0}, {2, 2}, {0, 2}); + return rotation(angle, {0, 0}, {2, 0}, {2, 2}, {0, 2}); } // Create a pitch transformation matrix @@ -163,7 +172,7 @@ template, glm::length_t>(angle, {1, 1}, {1, 2}, {2, 2}, {2, 1}); + return rotation(angle, {1, 1}, {1, 2}, {2, 2}, {2, 1}); } // Create a combined yaw, pitch, roll transformation matrix @@ -337,7 +346,7 @@ find_arc_centre(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, An if (start == end) { return {start, false}; } - return find_arc_centre(start, sincosf(entrys + half_pi), end, sincosf(entrye - half_pi)); + return find_arc_centre(start, sincos(entrys + half_pi), end, sincos(entrye - half_pi)); } template @@ -370,7 +379,7 @@ std::pair find_arcs_radius(glm::vec<2, T, Q> start, Angle entrys, glm::vec<2, T, Q> end, Angle entrye) { const auto getrad = [&](auto leftOrRight) { - return find_arcs_radius(start, sincosf(entrys + leftOrRight), end, sincosf(entrye + leftOrRight)); + return find_arcs_radius(start, sincos(entrys + leftOrRight), end, sincos(entrye + leftOrRight)); }; return {getrad(-half_pi), getrad(half_pi)}; } -- cgit v1.2.3