summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--lib/maths.cpp47
-rw-r--r--lib/maths.h21
-rw-r--r--test/test-maths.cpp29
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);
+}