diff options
-rw-r--r-- | lib/maths.cpp | 47 | ||||
-rw-r--r-- | lib/maths.h | 21 | ||||
-rw-r--r-- | test/test-maths.cpp | 29 |
3 files changed, 97 insertions, 0 deletions
diff --git a/lib/maths.cpp b/lib/maths.cpp index 543c887..60413f4 100644 --- a/lib/maths.cpp +++ b/lib/maths.cpp @@ -15,6 +15,53 @@ flat_orientation(const glm::vec3 & diff) return (std::isnan(e[0][0])) ? oneeighty : e; } +// Helper to lookup into a matrix given an xy vector coordinate +template<typename M> +inline auto & +operator^(M & m, glm::ivec2 xy) +{ + return m[xy.x][xy.y]; +} + +// Create a matrix for the angle, given the targets into the matrix +inline auto +rotation(float a, glm::ivec2 c1, glm::ivec2 s1, glm::ivec2 c2, glm::ivec2 ms2) +{ + glm::mat4 m(1); + sincosf(a, m ^ s1, m ^ c1); + m ^ c2 = m ^ c1; + m ^ ms2 = -(m ^ s1); + return m; +} + +// Create a roll transformation matrix +glm::mat4 +rotate_roll(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}, {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 bomcined yaw, pitch, roll transformation matrix +glm::mat4 +rotate_ypr(glm::vec3 a) +{ + return rotate_yaw(a.y) * rotate_pitch(a.x) * rotate_roll(a.z); +} + float vector_yaw(const glm::vec3 & diff) { diff --git a/lib/maths.h b/lib/maths.h index c072283..d304b65 100644 --- a/lib/maths.h +++ b/lib/maths.h @@ -1,6 +1,7 @@ #ifndef MATH_H #define MATH_H +#include <cmath> #include <glm/glm.hpp> #include <glm/gtc/constants.hpp> #include <utility> @@ -29,6 +30,26 @@ constexpr auto two_pi {glm::two_pi<float>()}; glm::mat4 flat_orientation(const glm::vec3 & diff); +// C++ wrapper for C's sincosf, but with references, not pointers +inline auto +sincosf(float a, float & s, float & c) +{ + return sincosf(a, &s, &c); +} + +inline std::pair<float, float> +sincosf(float a) +{ + std::pair<float, float> sc; + sincosf(a, &sc.first, &sc.second); + return sc; +} + +glm::mat4 rotate_roll(float); +glm::mat4 rotate_yaw(float); +glm::mat4 rotate_pitch(float); +glm::mat4 rotate_ypr(glm::vec3); + float vector_yaw(const glm::vec3 & diff); float vector_pitch(const glm::vec3 & diff); diff --git a/test/test-maths.cpp b/test/test-maths.cpp index a2e1953..b66b863 100644 --- a/test/test-maths.cpp +++ b/test/test-maths.cpp @@ -2,7 +2,9 @@ #include <boost/test/data/test_case.hpp> #include <boost/test/unit_test.hpp> +#include <glm/gtx/transform.hpp> #include <stream_support.hpp> +#include <string_view> #include <glm/glm.hpp> #include <maths.h> @@ -95,3 +97,30 @@ BOOST_AUTO_TEST_CASE(test_find_arcs_radius) { BOOST_CHECK_CLOSE(find_arcs_radius({10.32, 26.71}, {0.4, .92}, {2.92, 22.41}, {-0.89, -0.45}), 2.29, 1); } + +static void +compare_rotations(float a, const glm::vec3 & axis, glm::mat4 (*rotate_func)(float), std::string_view n) +{ + BOOST_TEST_CONTEXT(n) { + const auto g {glm::rotate(a, axis)}, ilt {rotate_func(a)}; + for (int c = 0; c < 4; c++) { + BOOST_TEST_CONTEXT(c) { + for (int r = 0; r < 4; r++) { + BOOST_TEST_CONTEXT(r) { + BOOST_CHECK_CLOSE(g[c][r], ilt[c][r], 0.0001); + } + } + } + } + } +} +const auto angs = boost::unit_test::data::make({pi, half_pi, two_pi, quarter_pi, -pi, -half_pi, -quarter_pi, 0.F}); +const auto rots = boost::unit_test::data::make<std::tuple<glm::vec3, glm::mat4 (*)(float), std::string_view>>({ + {up, rotate_yaw, "yaw"}, + {west, rotate_pitch, "pitch"}, + {north, rotate_roll, "roll"}, +}); +BOOST_DATA_TEST_CASE(test_rotations, angs * rots, a, ax, func, n) +{ + compare_rotations(a, ax, func, n); +} |