From 38a839533e76d3e9570591a6aeec7c5c500270cf Mon Sep 17 00:00:00 2001 From: Dan Goodliffe Date: Sat, 5 Oct 2024 12:15:28 +0100 Subject: Add rotate_yp taking two separate parameters --- lib/maths.cpp | 8 +++++++- lib/maths.h | 1 + 2 files changed, 8 insertions(+), 1 deletion(-) (limited to 'lib') diff --git a/lib/maths.cpp b/lib/maths.cpp index 51e27fe..ea245f0 100644 --- a/lib/maths.cpp +++ b/lib/maths.cpp @@ -77,7 +77,13 @@ rotate_ypr(Rotation3D a) glm::mat4 rotate_yp(Rotation2D a) { - return rotate_yaw(a.y) * rotate_pitch(a.x); + return rotate_yp(a.y, a.x); +} + +glm::mat4 +rotate_yp(Angle yaw, Angle pitch) +{ + return rotate_yaw(yaw) * rotate_pitch(pitch); } float diff --git a/lib/maths.h b/lib/maths.h index 3127d3c..32f2884 100644 --- a/lib/maths.h +++ b/lib/maths.h @@ -97,6 +97,7 @@ glm::mat4 rotate_roll(float); glm::mat4 rotate_yaw(float); glm::mat4 rotate_pitch(float); glm::mat4 rotate_yp(Rotation2D); +glm::mat4 rotate_yp(Angle yaw, Angle pitch); glm::mat4 rotate_ypr(Rotation3D); float vector_yaw(const Direction2D & diff); -- cgit v1.2.3 From cfec8308a39ae4a2a1a1b1fe0df0a099470d9c78 Mon Sep 17 00:00:00 2001 From: Dan Goodliffe Date: Thu, 17 Oct 2024 02:58:16 +0100 Subject: Simple constexpr pow function --- lib/maths.cpp | 12 ++++++++++++ lib/maths.h | 13 +++++++++++++ 2 files changed, 25 insertions(+) (limited to 'lib') diff --git a/lib/maths.cpp b/lib/maths.cpp index ea245f0..f527881 100644 --- a/lib/maths.cpp +++ b/lib/maths.cpp @@ -116,6 +116,18 @@ normalize(float ang) return ang; } +static_assert(pow(1, 0) == 1); +static_assert(pow(1, 1) == 1); +static_assert(pow(1, 2) == 1); +static_assert(pow(2, 0) == 1); +static_assert(pow(2, 1) == 2); +static_assert(pow(2, 2) == 4); +static_assert(pow(2, 3) == 8); +static_assert(pow(3, 0) == 1); +static_assert(pow(3, 1) == 3); +static_assert(pow(3, 2) == 9); +static_assert(pow(pi, 3) == 31.006278991699219F); + float operator"" _mph(const long double v) { diff --git a/lib/maths.h b/lib/maths.h index 32f2884..97b7ef5 100644 --- a/lib/maths.h +++ b/lib/maths.h @@ -261,6 +261,19 @@ midpoint(const std::pair & v) return std::midpoint(v.first, v.second); } +// std::pow is not constexpr +template + requires requires(T n) { n *= n; } +constexpr inline T +pow(const T base, std::integral auto exp) +{ + T res {1}; + while (exp--) { + res *= base; + } + return res; +} + // Conversions template inline constexpr auto -- cgit v1.2.3 From 5b6a6f3b241fea6d19521ddbb705e27d5e4c0268 Mon Sep 17 00:00:00 2001 From: Dan Goodliffe Date: Sun, 20 Oct 2024 16:24:14 +0100 Subject: Move lots of maths helpers to inline, constexpr, templates Always for working with different dimensions/types --- lib/maths.cpp | 97 -------------------- lib/maths.h | 287 +++++++++++++++++++++++++++++++++++++++++----------------- 2 files changed, 204 insertions(+), 180 deletions(-) (limited to 'lib') diff --git a/lib/maths.cpp b/lib/maths.cpp index f527881..3a9bf9b 100644 --- a/lib/maths.cpp +++ b/lib/maths.cpp @@ -19,103 +19,6 @@ flat_orientation(const Direction3D & diff) return (std::isnan(e[0][0])) ? oneeighty : e; } -// Helper to lookup into a matrix given an xy vector coordinate -template -inline auto & -operator^(M & m, glm::vec<2, I> xy) -{ - return m[xy.x][xy.y]; -} - -// Create a matrix for the angle, given the targets into the matrix -template -inline auto -rotation(typename M::value_type a, glm::vec<2, I> c1, glm::vec<2, I> s1, glm::vec<2, I> c2, glm::vec<2, I> ms2) -{ - M m(1); - sincosf(a, m ^ s1, m ^ c1); - m ^ c2 = m ^ c1; - m ^ ms2 = -(m ^ s1); - return m; -} - -// Create a flat (2D) transformation matrix -glm::mat2 -rotate_flat(float a) -{ - return rotation(a, {0, 0}, {0, 1}, {1, 1}, {1, 0}); -} - -// Create a yaw transformation matrix -glm::mat4 -rotate_yaw(float a) -{ - return rotation(a, {0, 0}, {1, 0}, {1, 1}, {0, 1}); -} - -// Create a roll transformation matrix -glm::mat4 -rotate_roll(float a) -{ - return rotation(a, {0, 0}, {2, 0}, {2, 2}, {0, 2}); -} - -// Create a pitch transformation matrix -glm::mat4 -rotate_pitch(float a) -{ - return rotation(a, {1, 1}, {1, 2}, {2, 2}, {2, 1}); -} - -// Create a combined yaw, pitch, roll transformation matrix -glm::mat4 -rotate_ypr(Rotation3D a) -{ - return rotate_yaw(a.y) * rotate_pitch(a.x) * rotate_roll(a.z); -} - -glm::mat4 -rotate_yp(Rotation2D a) -{ - return rotate_yp(a.y, a.x); -} - -glm::mat4 -rotate_yp(Angle yaw, Angle pitch) -{ - return rotate_yaw(yaw) * rotate_pitch(pitch); -} - -float -vector_yaw(const Direction2D & diff) -{ - return std::atan2(diff.x, diff.y); -} - -float -vector_pitch(const Direction3D & diff) -{ - return std::atan(diff.z); -} - -float -round_frac(const float & v, const float & frac) -{ - return std::round(v / frac) * frac; -} - -float -normalize(float ang) -{ - while (ang > pi) { - ang -= two_pi; - } - while (ang <= -pi) { - ang += two_pi; - } - return ang; -} - static_assert(pow(1, 0) == 1); static_assert(pow(1, 1) == 1); static_assert(pow(1, 2) == 1); diff --git a/lib/maths.h b/lib/maths.h index 97b7ef5..94e357b 100644 --- a/lib/maths.h +++ b/lib/maths.h @@ -17,22 +17,22 @@ struct Arc : public std::pair { } Arc(const RelativePosition2D & dir0, const RelativePosition2D & dir1); - Arc(const Angle angb, const Angle anga); + Arc(Angle anga, Angle angb); auto - operator[](bool i) const + operator[](bool getSecond) const { - return i ? second : first; + return getSecond ? second : first; } - [[nodiscard]] constexpr inline float + [[nodiscard]] constexpr float length() const { return second - first; } }; -constexpr const RelativePosition3D up {0, 0, 1}; +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}; constexpr const RelativePosition3D south {0, -1, 0}; @@ -40,7 +40,7 @@ constexpr const RelativePosition3D east {1, 0, 0}; constexpr const RelativePosition3D west {-1, 0, 0}; constexpr auto half_pi {glm::half_pi()}; constexpr auto quarter_pi {half_pi / 2}; -constexpr auto pi {glm::pi()}; +constexpr auto pi {glm::pi()}; // NOLINT(readability-identifier-length) constexpr auto two_pi {glm::two_pi()}; constexpr auto degreesToRads = pi / 180.F; @@ -48,153 +48,274 @@ constexpr auto earthMeanRadius = 6371.01F; // In km constexpr auto astronomicalUnit = 149597890.F; // In km template -constexpr inline GlobalPosition -operator+(const GlobalPosition & g, const RelativePosition & r) +constexpr GlobalPosition +operator+(const GlobalPosition & global, const RelativePosition & relative) { - return g + GlobalPosition(glm::round(r)); + return global + GlobalPosition(glm::round(relative)); } template -constexpr inline GlobalPosition -operator+(const GlobalPosition & g, const CalcPosition & r) +constexpr GlobalPosition +operator+(const GlobalPosition & global, const CalcPosition & relative) { - return g + GlobalPosition(r); + return global + GlobalPosition(relative); } template -constexpr inline GlobalPosition -operator-(const GlobalPosition & g, const RelativePosition & r) +constexpr GlobalPosition +operator-(const GlobalPosition & global, const RelativePosition & relative) { - return g - GlobalPosition(glm::round(r)); + return global - GlobalPosition(glm::round(relative)); } template -constexpr inline GlobalPosition -operator-(const GlobalPosition & g, const CalcPosition & r) +constexpr GlobalPosition +operator-(const GlobalPosition & global, const CalcPosition & relative) { - return g - GlobalPosition(r); + return global - GlobalPosition(relative); +} + +template +constexpr RelativePosition +difference(const glm::vec & globalA, const glm::vec & globalB) +{ + return globalA - globalB; } glm::mat4 flat_orientation(const Rotation3D & diff); -// C++ wrapper for C's sincosf, but with references, not pointers -inline auto -sincosf(float a, float & s, float & c) +namespace { + // Helpers + // C++ wrapper for C's sincosf, but with references, not pointers + constexpr auto + sincosf(float angle, float & sinOut, float & cosOut) + { + if consteval { + sinOut = ::sinf(angle); + cosOut = ::cosf(angle); + } + else { + ::sincosf(angle, &sinOut, &cosOut); + } + } + + template + constexpr auto + sincosf(const T angle) + { + Rotation2D sincosOut; + sincosf(angle, sincosOut.x, sincosOut.y); + return sincosOut; + } + + // Helper to lookup into a matrix given an xy vector coordinate + template + constexpr auto & + operator^(M & matrix, glm::vec<2, I> rowCol) + { + return matrix[rowCol.x][rowCol.y]; + } + + // Create a matrix for the angle, given the targets into the matrix + 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) + { + M out(1); + sincosf(angle, out ^ sin1, out ^ cos1); + out ^ cos2 = out ^ cos1; + out ^ negSin1 = -(out ^ sin1); + return out; + } +} + +// Create a flat transformation matrix +template + requires(D >= 2) +constexpr auto +rotate_flat(const T angle) { - return sincosf(a, &s, &c); + return rotation, glm::length_t>(angle, {0, 0}, {0, 1}, {1, 1}, {1, 0}); } -inline Rotation2D -sincosf(float a) +// Create a yaw transformation matrix +template + requires(D >= 2) +constexpr auto +rotate_yaw(const T angle) { - Rotation2D sc; - sincosf(a, sc.x, sc.y); - return sc; + return rotation, glm::length_t>(angle, {0, 0}, {1, 0}, {1, 1}, {0, 1}); } -glm::mat2 rotate_flat(float); -glm::mat4 rotate_roll(float); -glm::mat4 rotate_yaw(float); -glm::mat4 rotate_pitch(float); -glm::mat4 rotate_yp(Rotation2D); -glm::mat4 rotate_yp(Angle yaw, Angle pitch); -glm::mat4 rotate_ypr(Rotation3D); +// Create a roll transformation matrix +template + requires(D >= 3) +constexpr auto +rotate_roll(const T angle) +{ + return rotation, glm::length_t>(angle, {0, 0}, {2, 0}, {2, 2}, {0, 2}); +} -float vector_yaw(const Direction2D & diff); -float vector_pitch(const Direction3D & diff); +// Create a pitch transformation matrix +template + requires(D >= 3) +constexpr auto +rotate_pitch(const T angle) +{ + return rotation, glm::length_t>(angle, {1, 1}, {1, 2}, {2, 2}, {2, 1}); +} + +// Create a combined yaw, pitch, roll transformation matrix +template + requires(D >= 3) +constexpr auto +rotate_ypr(const glm::vec<3, T, Q> & angles) +{ + return rotate_yaw(angles.y) * rotate_pitch(angles.x) * rotate_roll(angles.z); +} + +template + requires(D >= 3) +constexpr auto +rotate_yp(const T yaw, const T pitch) +{ + return rotate_yaw(yaw) * rotate_pitch(pitch); +} + +template + requires(D >= 3) +constexpr auto +rotate_yp(const glm::vec<2, T, Q> & angles) +{ + return rotate_yp(angles.y, angles.x); +} + +template + requires(D >= 2) +constexpr auto +vector_yaw(const glm::vec & diff) +{ + return std::atan2(diff.x, diff.y); +} + +template + requires(D >= 3) +constexpr auto +vector_pitch(const glm::vec & diff) +{ + return std::atan(diff.z); +} template -glm::vec<2, T, Q> -vector_normal(const glm::vec<2, T, Q> & v) +constexpr glm::vec<2, T, Q> +vector_normal(const glm::vec<2, T, Q> & vector) { - return {-v.y, v.x}; + return {-vector.y, vector.x}; }; -float round_frac(const float & v, const float & frac); +template +constexpr auto +round_frac(const T value, const T frac) +{ + return std::round(value / frac) * frac; +} template -inline constexpr auto -sq(T v) + requires requires(T value) { value * value; } +constexpr auto +sq(T value) { - return v * v; + return value * value; } template -inline constexpr glm::vec<3, int64_t, Q> -crossProduct(const glm::vec<3, int64_t, Q> a, const glm::vec<3, int64_t, Q> b) +constexpr glm::vec<3, int64_t, Q> +crossProduct(const glm::vec<3, int64_t, Q> & valueA, const glm::vec<3, int64_t, Q> & valueB) { return { - (a.y * b.z) - (a.z * b.y), - (a.z * b.x) - (a.x * b.z), - (a.x * b.y) - (a.y * b.x), + (valueA.y * valueB.z) - (valueA.z * valueB.y), + (valueA.z * valueB.x) - (valueA.x * valueB.z), + (valueA.x * valueB.y) - (valueA.y * valueB.x), }; } template -inline constexpr glm::vec<3, T, Q> -crossProduct(const glm::vec<3, T, Q> a, const glm::vec<3, T, Q> b) +constexpr glm::vec<3, T, Q> +crossProduct(const glm::vec<3, T, Q> & valueA, const glm::vec<3, T, Q> & valueB) { - return crossProduct(a, b); + return crossProduct(valueA, valueB); } template -inline constexpr glm::vec<3, T, Q> -crossProduct(const glm::vec<3, T, Q> a, const glm::vec<3, T, Q> b) +constexpr glm::vec<3, T, Q> +crossProduct(const glm::vec<3, T, Q> & valueA, const glm::vec<3, T, Q> & valueB) { - return glm::cross(a, b); + return glm::cross(valueA, valueB); } template -inline constexpr auto -ratio(Ta a, Tb b) +constexpr auto +ratio(const Ta valueA, const Tb valueB) { - return (static_cast(a) / static_cast(b)); + return (static_cast(valueA) / static_cast(valueB)); } template -inline constexpr auto -ratio(glm::vec<2, T, Q> v) +constexpr auto +ratio(const glm::vec<2, T, Q> & value) { - return ratio(v.x, v.y); + return ratio(value.x, value.y); } -template -inline constexpr glm::vec -perspective_divide(glm::vec<4, T, Q> v) +template +constexpr auto +perspective_divide(const glm::vec<4, T, Q> & value) { - return v / v.w; + return value / value.w; } template -inline constexpr glm::vec -operator||(const glm::vec v1, const glm::vec v2) +constexpr glm::vec +operator||(const glm::vec valueA, const glm::vec valueB) { - return {v1, v2}; + return {valueA, valueB}; } template -inline constexpr glm::vec -operator||(const glm::vec v1, const T v2) +constexpr glm::vec +operator||(const glm::vec valueA, const T valueB) { - return {v1, v2}; + return {valueA, valueB}; } -template -inline constexpr glm::vec -perspectiveMultiply(const glm::vec & p, const glm::mat & mutation) +template +constexpr glm::vec +perspectiveMultiply(const glm::vec & base, const glm::mat & mutation) { - const auto p2 = mutation * (p || T(1)); - return p2 / p2.w; + const auto mutated = mutation * (base || T(1)); + return mutated / mutated.w; } -template -inline constexpr glm::vec -perspectiveApply(glm::vec & p, const glm::mat & mutation) +template +constexpr glm::vec +perspectiveApply(glm::vec & base, const glm::mat & mutation) { - return p = perspectiveMultiply(p, mutation); + return base = perspectiveMultiply(base, mutation); } -float normalize(float ang); +template +constexpr T +normalize(T ang) +{ + while (ang > glm::pi()) { + ang -= glm::two_pi(); + } + while (ang <= -glm::pi()) { + ang += glm::two_pi(); + } + return ang; +} template std::pair, bool> @@ -264,7 +385,7 @@ midpoint(const std::pair & v) // std::pow is not constexpr template requires requires(T n) { n *= n; } -constexpr inline T +constexpr T pow(const T base, std::integral auto exp) { T res {1}; @@ -276,14 +397,14 @@ pow(const T base, std::integral auto exp) // Conversions template -inline constexpr auto +constexpr auto mph_to_ms(T v) { return v / 2.237L; } template -inline constexpr auto +constexpr auto kph_to_ms(T v) { return v / 3.6L; -- cgit v1.2.3 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