diff options
| -rw-r--r-- | opentrack/simple-mat.cpp | 96 | ||||
| -rw-r--r-- | opentrack/simple-mat.hpp | 216 |
2 files changed, 195 insertions, 117 deletions
diff --git a/opentrack/simple-mat.cpp b/opentrack/simple-mat.cpp new file mode 100644 index 00000000..6a53e6ad --- /dev/null +++ b/opentrack/simple-mat.cpp @@ -0,0 +1,96 @@ +#include "simple-mat.hpp" +#include <cmath> + +namespace euler { + +euler_t OPENTRACK_API_EXPORT rmat_to_euler(const rmat& R) +{ + using std::atan2; + using std::sqrt; + + const double cy = sqrt(R(2,2)*R(2, 2) + R(2, 1)*R(2, 1)); + const bool large_enough = cy > 1e-10; + if (large_enough) + return euler_t(atan2(-R(1, 0), R(0, 0)), + atan2(R(2, 0), cy), + atan2(-R(2, 1), R(2, 2))); + else + return euler_t(atan2(R(0, 1), R(1, 1)), + atan2(R(2, 0), cy), + 0); +} + +// tait-bryan angles, not euler +rmat OPENTRACK_API_EXPORT euler_to_rmat(const euler_t& input) +{ + const double H = -input(0); + const double P = -input(1); + const double B = -input(2); + + using std::cos; + using std::sin; + + const auto c1 = cos(H); + const auto s1 = sin(H); + const auto c2 = cos(P); + const auto s2 = sin(P); + const auto c3 = cos(B); + const auto s3 = sin(B); + + return rmat( + // z + c1 * c2, + c1 * s2 * s3 - c3 * s1, + s1 * s3 + c1 * c3 * s2, + // y + c2 * s1, + c1 * c3 + s1 * s2 * s3, + c3 * s1 * s2 - c1 * s3, + // x + -s2, + c2 * s3, + c2 * c3 + ); +} + +// https://en.wikipedia.org/wiki/Davenport_chained_rotations#Tait.E2.80.93Bryan_chained_rotations +void OPENTRACK_API_EXPORT tait_bryan_to_matrices(const euler_t& input, + rmat& r_roll, + rmat& r_pitch, + rmat& r_yaw) +{ + using std::cos; + using std::sin; + + { + const double phi = -input(2); + const double sin_phi = sin(phi); + const double cos_phi = cos(phi); + + r_roll = rmat(1, 0, 0, + 0, cos_phi, -sin_phi, + 0, sin_phi, cos_phi); + } + + { + const double theta = input(1); + const double sin_theta = sin(theta); + const double cos_theta = cos(theta); + + r_pitch = rmat(cos_theta, 0, -sin_theta, + 0, 1, 0, + sin_theta, 0, cos_theta); + } + + { + const double psi = -input(0); + const double sin_psi = sin(psi); + const double cos_psi = cos(psi); + + r_yaw = rmat(cos_psi, -sin_psi, 0, + sin_psi, cos_psi, 0, + 0, 0, 1); + } +} + +} // end ns euler diff --git a/opentrack/simple-mat.hpp b/opentrack/simple-mat.hpp index 4f885f4f..e70c24a8 100644 --- a/opentrack/simple-mat.hpp +++ b/opentrack/simple-mat.hpp @@ -1,4 +1,4 @@ -/* Copyright (c) 2014-2015, Stanislaw Halik <sthalik@misaki.pl> +/* Copyright (c) 2014-2016, Stanislaw Halik <sthalik@misaki.pl> * Permission to use, copy, modify, and/or distribute this * software for any purpose with or without fee is hereby granted, @@ -7,14 +7,16 @@ */ #pragma once -#include <algorithm> + +#include "export.hpp" + #include <initializer_list> #include <type_traits> -#include <cmath> +#include <utility> namespace { // last param to fool SFINAE into overloading - template<int i, int j, int ignored> + template<int i, int j, int> struct equals { enum { value = i == j }; @@ -52,75 +54,75 @@ namespace { template<typename num, int h_, int w_> class Mat { -#ifdef __GNUC__ - __restrict -#endif - num data[h_][w_]; - static_assert(h_ > 0 && w_ > 0, "must have positive mat dimensions"); - - Mat(std::initializer_list<num>&& xs) = delete; + num data[h_][w_]; public: - - // parameters w_ and h_ are rebound so that SFINAE occurs - // removing them causes a compile-time error -sh 20150811 - template<int Q = w_> typename std::enable_if<equals<Q, 1, 0>::value, num>::type inline operator()(int i) const { return data[i][0]; } - + template<int P = h_> typename std::enable_if<equals<P, 1, 1>::value, num>::type inline operator()(int i) const { return data[0][i]; } - + template<int Q = w_> typename std::enable_if<equals<Q, 1, 2>::value, num&>::type inline operator()(int i) { return data[i][0]; } - + template<int P = h_> typename std::enable_if<equals<P, 1, 3>::value, num&>::type inline operator()(int i) { return data[0][i]; } - + +#define OPENTRACK_ASSERT_SWIZZLE static_assert(P == h_ && Q == w_, "") + template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 1>::value, num>::type - inline x() const { return operator()(0); } - + x() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(0); } + template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 2>::value, num>::type - inline y() const { return operator()(1); } - + y() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(1); } + template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 3>::value, num>::type - inline z() const { return operator()(2); } - + z() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(2); } + template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 4>::value, num>::type - inline w() const { return operator()(3); } - + w() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(3); } + template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 1>::value, num&>::type - inline x() { return operator()(0); } - + x() { OPENTRACK_ASSERT_SWIZZLE; return operator()(0); } + template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 2>::value, num&>::type - inline y() { return operator()(1); } - + y() { OPENTRACK_ASSERT_SWIZZLE; return operator()(1); } + template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 3>::value, num&>::type - inline z() { return operator()(2); } - + z() { OPENTRACK_ASSERT_SWIZZLE; return operator()(2); } + template<int P = h_, int Q = w_> typename std::enable_if<maybe_add_swizzle<P, Q, 4>::value, num&>::type - inline w() { return operator()(3); } - + w() { OPENTRACK_ASSERT_SWIZZLE; return operator()(3); } + // parameters w_ and h_ are rebound so that SFINAE occurs + // removing them causes a compile-time error -sh 20150811 + template<int R, int S, int P = h_, int Q = w_> typename std::enable_if<is_vector_pair<R, S, P, Q>::value, num>::type - dot(const Mat<num, R, S>& p2) const { + dot(const Mat<num, R, S>& p2) const + { + static_assert(P == h_ && Q == w_, ""); + num ret = 0; constexpr int len = vector_len<R, S>::value; for (int i = 0; i < len; i++) ret += operator()(i) * p2(i); return ret; } - + template<int R, int S, int P = h_, int Q = w_> typename std::enable_if<is_dim3<P, Q, R, S>::value, Mat<num, is_dim3<P, Q, R, S>::P, is_dim3<P, Q, R, S>::Q>>::type cross(const Mat<num, R, S>& p2) const { - return Mat<num, R, S>(y() * p2.z() - p2.y() * z(), - p2.x() * z() - x() * p2.z(), - x() * p2.y() - y() * p2.x()); + static_assert(P == h_ && Q == w_, ""); + decltype(*this)& p1 = *this; + + return Mat<num, R, S>(p1.y() * p2.z() - p2.y() * p1.z(), + p2.x() * p1.z() - p1.x() * p2.z(), + p1.x() * p2.y() - p1.y() * p2.x()); } - + Mat<num, h_, w_> operator+(const Mat<num, h_, w_>& other) const { Mat<num, h_, w_> ret; @@ -129,7 +131,7 @@ public: ret(j, i) = data[j][i] + other.data[j][i]; return ret; } - + Mat<num, h_, w_> operator-(const Mat<num, h_, w_>& other) const { Mat<num, h_, w_> ret; @@ -138,7 +140,7 @@ public: ret(j, i) = data[j][i] - other.data[j][i]; return ret; } - + Mat<num, h_, w_> operator+(const num& other) const { Mat<num, h_, w_> ret; @@ -147,7 +149,7 @@ public: ret(j, i) = data[j][i] + other; return ret; } - + Mat<num, h_, w_> operator-(const num& other) const { Mat<num, h_, w_> ret; @@ -156,16 +158,7 @@ public: ret(j, i) = data[j][i] - other; return ret; } - - Mat<num, h_, w_> operator*(const num other) const - { - Mat<num, h_, w_> ret; - for (int j = 0; j < h_; j++) - for (int i = 0; i < w_; i++) - ret(j, i) = data[j][i] * other; - return ret; - } - + template<int p> Mat<num, h_, p> operator*(const Mat<num, w_, p>& other) const { @@ -187,11 +180,11 @@ public: typename = typename std::enable_if<is_arglist_correct<num, h__, w__, ts...>::value>::type> Mat(const ts... xs) { - const std::initializer_list<num> init = { static_cast<num>(xs)... }; - auto iter = init.begin(); - for (int j = 0; j < h_; j++) - for (int i = 0; i < w_; i++) - data[j][i] = *iter++; + static_assert(h__ == h_ && w__ == w_, ""); + + std::initializer_list<num> init = { static_cast<num>(xs)... }; + + *this = Mat(std::move(init)); } Mat() @@ -208,13 +201,25 @@ public: data[j][i] = mem[i*h_+j]; } - Mat(num* mem) : Mat(const_cast<const num*>(mem)) {} + Mat(std::initializer_list<num>&& init) + { + auto iter = init.begin(); + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) + data[j][i] = *iter++; + } + + operator num*() { return reinterpret_cast<num*>(data); } + operator const num*() const { return reinterpret_cast<const num*>(data); } // XXX add more operators as needed, third-party dependencies mostly // not needed merely for matrix algebra -sh 20141030 - static Mat<num, h_, h_> eye() + template<int h__ = h_> + static typename std::enable_if<h_ == w_, Mat<num, h__, h__>>::type eye() { + static_assert(h_ == h__, ""); + Mat<num, h_, h_> ret; for (int j = 0; j < h_; j++) for (int i = 0; i < w_; i++) @@ -236,61 +241,38 @@ public: return ret; } - - template<int h__, int w__> using dmat = Mat<double, h__, w__>; - - // http://stackoverflow.com/a/18436193 - static dmat<3, 1> rmat_to_euler(const dmat<3, 3>& R) - { - static constexpr double pi = 3.141592653; - const double pitch_1 = asin(-R(0, 2)); - const double pitch_2 = pi - pitch_1; - const double cos_p1 = cos(pitch_1), cos_p2 = cos(pitch_2); - const double roll_1 = atan2(R(1, 2) / cos_p1, R(2, 2) / cos_p1); - const double roll_2 = atan2(R(1, 2) / cos_p2, R(2, 2) / cos_p2); - const double yaw_1 = atan2(R(0, 1) / cos_p1, R(0, 0) / cos_p1); - const double yaw_2 = atan2(R(0, 1) / cos_p2, R(0, 0) / cos_p2); - if (std::abs(pitch_1) + std::abs(roll_1) + std::abs(yaw_1) > std::abs(pitch_2) + std::abs(roll_2) + std::abs(yaw_2)) - { - bool fix_neg_pitch = pitch_1 < 0; - return dmat<3, 1>(yaw_2, std::fmod(fix_neg_pitch ? -pi - pitch_1 : pitch_2, pi), roll_2); - } - else - return dmat<3, 1>(yaw_1, pitch_1, roll_1); - } - - // tait-bryan angles, not euler - static dmat<3, 3> euler_to_rmat(const double* input) - { - static constexpr double pi = 3.141592653; - auto H = input[0] * pi / 180; - auto P = input[1] * pi / 180; - auto B = input[2] * pi / 180; - - const auto c1 = cos(H); - const auto s1 = sin(H); - const auto c2 = cos(P); - const auto s2 = sin(P); - const auto c3 = cos(B); - const auto s3 = sin(B); - - double foo[] = { - // z - c1 * c2, - c1 * s2 * s3 - c3 * s1, - s1 * s3 + c1 * c3 * s2, - // y - c2 * s1, - c1 * c3 + s1 * s2 * s3, - c3 * s1 * s2 - c1 * s3, - // x - -s2, - c2 * s3, - c2 * c3 - }; - - return dmat<3, 3>(foo); - } }; - + template<int h_, int w_> using dmat = Mat<double, h_, w_>; + +template<typename num, int h, int w> +Mat<num, h, w> operator*(num scalar, const Mat<num, h, w>& mat) +{ + return mat * scalar; +} + +template<typename num, int h_, int w_> +Mat<num, h_, w_> operator*(const Mat<num, h_, w_>& self, num other) +{ + Mat<num, h_, w_> ret; + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) + ret(j, i) = self(j, i) * other; + return ret; +} + +namespace euler { + +using rmat = dmat<3, 3>; +using euler_t = dmat<3, 1>; + +rmat OPENTRACK_API_EXPORT euler_to_rmat(const euler_t& input); + +euler_t OPENTRACK_API_EXPORT rmat_to_euler(const rmat& R); + +void OPENTRACK_API_EXPORT tait_bryan_to_matrices(const euler_t& input, + rmat& r_roll, + rmat& r_pitch, + rmat& r_yaw); + +} // end ns euler |