From 8f30029a598ccf99ffee87590f70df7f1f163e02 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Mon, 25 Jul 2016 11:03:27 +0200 Subject: api/simple-mat: move to logic Change causes rebuilds of all modules otherwise. --- opentrack-logic/simple-mat.cpp | 57 +++++++++ opentrack-logic/simple-mat.hpp | 275 +++++++++++++++++++++++++++++++++++++++++ opentrack/simple-mat.cpp | 57 --------- opentrack/simple-mat.hpp | 275 ----------------------------------------- pose-widget/CMakeLists.txt | 1 + 5 files changed, 333 insertions(+), 332 deletions(-) create mode 100644 opentrack-logic/simple-mat.cpp create mode 100644 opentrack-logic/simple-mat.hpp delete mode 100644 opentrack/simple-mat.cpp delete mode 100644 opentrack/simple-mat.hpp diff --git a/opentrack-logic/simple-mat.cpp b/opentrack-logic/simple-mat.cpp new file mode 100644 index 00000000..ad83a508 --- /dev/null +++ b/opentrack-logic/simple-mat.cpp @@ -0,0 +1,57 @@ +#include "simple-mat.hpp" +#include "opentrack-compat/pi-constant.hpp" +#include + +namespace euler { + +euler_t rmat_to_euler(const dmat<3, 3>& 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(2, 1), R(2, 2)), + atan2(R(2, 0), cy), + atan2(-R(1, 0), R(0, 0))); + else + return euler_t(0., + atan2(R(2, 0), cy), + atan2(R(0, 1), R(1, 1))); +} + +// tait-bryan angles, not euler +rmat 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 dmat<3, 3>( + // 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 + ); +} + +} // end ns euler diff --git a/opentrack-logic/simple-mat.hpp b/opentrack-logic/simple-mat.hpp new file mode 100644 index 00000000..f1ad0717 --- /dev/null +++ b/opentrack-logic/simple-mat.hpp @@ -0,0 +1,275 @@ +/* Copyright (c) 2014-2016, Stanislaw Halik + + * Permission to use, copy, modify, and/or distribute this + * software for any purpose with or without fee is hereby granted, + * provided that the above copyright notice and this permission + * notice appear in all copies. + */ + +#pragma once + +#include "export.hpp" + +#include +#include +#include + +namespace { + // last param to fool SFINAE into overloading + template + struct equals + { + enum { value = i == j }; + }; + template + struct maybe_add_swizzle + { + enum { value = (i == 1 || j == 1) && (i >= min || j >= min) }; + }; + template + struct is_vector_pair + { + enum { value = (i1 == i2 && j1 == 1 && j2 == 1) || (j1 == j2 && i1 == 1 && i2 == 1) }; + }; + template + struct vector_len + { + enum { value = i > j ? i : j }; + }; + template + struct is_dim3 + { + enum { value = (a == 1 && c == 1 && b == 3 && d == 3) || (a == 3 && c == 3 && b == 1 && d == 1) }; + enum { P = a == 1 ? 1 : 3 }; + enum { Q = a == 1 ? 3 : 1 }; + }; + + template + struct is_arglist_correct + { + enum { value = h * w == sizeof...(ts) }; + }; +} + +template +class Mat +{ + static_assert(h_ > 0 && w_ > 0, "must have positive mat dimensions"); + num data[h_][w_]; + +public: + template typename std::enable_if::value, num>::type + inline operator()(int i) const { return data[i][0]; } + + template typename std::enable_if::value, num>::type + inline operator()(int i) const { return data[0][i]; } + + template typename std::enable_if::value, num&>::type + inline operator()(int i) { return data[i][0]; } + + template typename std::enable_if::value, num&>::type + inline operator()(int i) { return data[0][i]; } + +#define OPENTRACK_ASSERT_SWIZZLE static_assert(P == h_ && Q == w_, "") + + template typename std::enable_if::value, num>::type + x() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(0); } + + template typename std::enable_if::value, num>::type + y() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(1); } + + template typename std::enable_if::value, num>::type + z() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(2); } + + template typename std::enable_if::value, num>::type + w() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(3); } + + template typename std::enable_if::value, num&>::type + x() { OPENTRACK_ASSERT_SWIZZLE; return operator()(0); } + + template typename std::enable_if::value, num&>::type + y() { OPENTRACK_ASSERT_SWIZZLE; return operator()(1); } + + template typename std::enable_if::value, num&>::type + z() { OPENTRACK_ASSERT_SWIZZLE; return operator()(2); } + + template typename std::enable_if::value, num&>::type + 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 + typename std::enable_if::value, num>::type + dot(const Mat& p2) const + { + static_assert(P == h_ && Q == w_, ""); + + num ret = 0; + constexpr int len = vector_len::value; + for (int i = 0; i < len; i++) + ret += operator()(i) * p2(i); + return ret; + } + + template + typename std::enable_if::value, Mat::P, is_dim3::Q>>::type + cross(const Mat& p2) const + { + static_assert(P == h_ && Q == w_, ""); + decltype(*this)& p1 = *this; + + return Mat(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 operator+(const Mat& other) const + { + Mat ret; + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) + ret(j, i) = data[j][i] + other.data[j][i]; + return ret; + } + + Mat operator-(const Mat& other) const + { + Mat ret; + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) + ret(j, i) = data[j][i] - other.data[j][i]; + return ret; + } + + Mat operator+(const num& other) const + { + Mat ret; + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) + ret(j, i) = data[j][i] + other; + return ret; + } + + Mat operator-(const num& other) const + { + Mat 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 + Mat operator*(const Mat& other) const + { + Mat ret; + for (int k = 0; k < h_; k++) + for (int i = 0; i < p; i++) + { + ret(k, i) = 0; + for (int j = 0; j < w_; j++) + ret(k, i) += data[k][j] * other(j, i); + } + return ret; + } + + inline num operator()(int j, int i) const { return data[j][i]; } + inline num& operator()(int j, int i) { return data[j][i]; } + + template::value>::type> + Mat(const ts... xs) + { + static_assert(h__ == h_ && w__ == w_, ""); + + std::initializer_list init = { static_cast(xs)... }; + + *this = Mat(std::move(init)); + } + + Mat() + { + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) + data[j][i] = num(0); + } + + Mat(const num* mem) + { + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) + data[j][i] = mem[i*h_+j]; + } + + Mat(std::initializer_list&& 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(data); } + operator const num*() const { return reinterpret_cast(data); } + + // XXX add more operators as needed, third-party dependencies mostly + // not needed merely for matrix algebra -sh 20141030 + + template + static typename std::enable_if>::type eye() + { + static_assert(h_ == h__, ""); + + Mat ret; + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) + ret.data[j][i] = 0; + + for (int i = 0; i < h_; i++) + ret.data[i][i] = 1; + + return ret; + } + + Mat t() const + { + Mat ret; + + for (int j = 0; j < h_; j++) + for (int i = 0; i < w_; i++) + ret(i, j) = data[j][i]; + + return ret; + } +}; + +template using dmat = Mat; + +template +Mat operator*(num scalar, const Mat& mat) +{ + return mat * scalar; +} + +template +Mat operator*(const Mat& self, num other) +{ + Mat 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 { + +template using dmat = Mat; +using rmat = dmat<3, 3>; +using euler_t = dmat<3, 1>; + +OPENTRACK_API_EXPORT rmat euler_to_rmat(const euler_t& input); + +// http://stackoverflow.com/a/18436193 +euler_t OPENTRACK_API_EXPORT rmat_to_euler(const dmat<3, 3>& R); + +} // end ns euler diff --git a/opentrack/simple-mat.cpp b/opentrack/simple-mat.cpp deleted file mode 100644 index ad83a508..00000000 --- a/opentrack/simple-mat.cpp +++ /dev/null @@ -1,57 +0,0 @@ -#include "simple-mat.hpp" -#include "opentrack-compat/pi-constant.hpp" -#include - -namespace euler { - -euler_t rmat_to_euler(const dmat<3, 3>& 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(2, 1), R(2, 2)), - atan2(R(2, 0), cy), - atan2(-R(1, 0), R(0, 0))); - else - return euler_t(0., - atan2(R(2, 0), cy), - atan2(R(0, 1), R(1, 1))); -} - -// tait-bryan angles, not euler -rmat 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 dmat<3, 3>( - // 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 - ); -} - -} // end ns euler diff --git a/opentrack/simple-mat.hpp b/opentrack/simple-mat.hpp deleted file mode 100644 index f1ad0717..00000000 --- a/opentrack/simple-mat.hpp +++ /dev/null @@ -1,275 +0,0 @@ -/* Copyright (c) 2014-2016, Stanislaw Halik - - * Permission to use, copy, modify, and/or distribute this - * software for any purpose with or without fee is hereby granted, - * provided that the above copyright notice and this permission - * notice appear in all copies. - */ - -#pragma once - -#include "export.hpp" - -#include -#include -#include - -namespace { - // last param to fool SFINAE into overloading - template - struct equals - { - enum { value = i == j }; - }; - template - struct maybe_add_swizzle - { - enum { value = (i == 1 || j == 1) && (i >= min || j >= min) }; - }; - template - struct is_vector_pair - { - enum { value = (i1 == i2 && j1 == 1 && j2 == 1) || (j1 == j2 && i1 == 1 && i2 == 1) }; - }; - template - struct vector_len - { - enum { value = i > j ? i : j }; - }; - template - struct is_dim3 - { - enum { value = (a == 1 && c == 1 && b == 3 && d == 3) || (a == 3 && c == 3 && b == 1 && d == 1) }; - enum { P = a == 1 ? 1 : 3 }; - enum { Q = a == 1 ? 3 : 1 }; - }; - - template - struct is_arglist_correct - { - enum { value = h * w == sizeof...(ts) }; - }; -} - -template -class Mat -{ - static_assert(h_ > 0 && w_ > 0, "must have positive mat dimensions"); - num data[h_][w_]; - -public: - template typename std::enable_if::value, num>::type - inline operator()(int i) const { return data[i][0]; } - - template typename std::enable_if::value, num>::type - inline operator()(int i) const { return data[0][i]; } - - template typename std::enable_if::value, num&>::type - inline operator()(int i) { return data[i][0]; } - - template typename std::enable_if::value, num&>::type - inline operator()(int i) { return data[0][i]; } - -#define OPENTRACK_ASSERT_SWIZZLE static_assert(P == h_ && Q == w_, "") - - template typename std::enable_if::value, num>::type - x() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(0); } - - template typename std::enable_if::value, num>::type - y() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(1); } - - template typename std::enable_if::value, num>::type - z() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(2); } - - template typename std::enable_if::value, num>::type - w() const { OPENTRACK_ASSERT_SWIZZLE; return operator()(3); } - - template typename std::enable_if::value, num&>::type - x() { OPENTRACK_ASSERT_SWIZZLE; return operator()(0); } - - template typename std::enable_if::value, num&>::type - y() { OPENTRACK_ASSERT_SWIZZLE; return operator()(1); } - - template typename std::enable_if::value, num&>::type - z() { OPENTRACK_ASSERT_SWIZZLE; return operator()(2); } - - template typename std::enable_if::value, num&>::type - 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 - typename std::enable_if::value, num>::type - dot(const Mat& p2) const - { - static_assert(P == h_ && Q == w_, ""); - - num ret = 0; - constexpr int len = vector_len::value; - for (int i = 0; i < len; i++) - ret += operator()(i) * p2(i); - return ret; - } - - template - typename std::enable_if::value, Mat::P, is_dim3::Q>>::type - cross(const Mat& p2) const - { - static_assert(P == h_ && Q == w_, ""); - decltype(*this)& p1 = *this; - - return Mat(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 operator+(const Mat& other) const - { - Mat ret; - for (int j = 0; j < h_; j++) - for (int i = 0; i < w_; i++) - ret(j, i) = data[j][i] + other.data[j][i]; - return ret; - } - - Mat operator-(const Mat& other) const - { - Mat ret; - for (int j = 0; j < h_; j++) - for (int i = 0; i < w_; i++) - ret(j, i) = data[j][i] - other.data[j][i]; - return ret; - } - - Mat operator+(const num& other) const - { - Mat ret; - for (int j = 0; j < h_; j++) - for (int i = 0; i < w_; i++) - ret(j, i) = data[j][i] + other; - return ret; - } - - Mat operator-(const num& other) const - { - Mat 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 - Mat operator*(const Mat& other) const - { - Mat ret; - for (int k = 0; k < h_; k++) - for (int i = 0; i < p; i++) - { - ret(k, i) = 0; - for (int j = 0; j < w_; j++) - ret(k, i) += data[k][j] * other(j, i); - } - return ret; - } - - inline num operator()(int j, int i) const { return data[j][i]; } - inline num& operator()(int j, int i) { return data[j][i]; } - - template::value>::type> - Mat(const ts... xs) - { - static_assert(h__ == h_ && w__ == w_, ""); - - std::initializer_list init = { static_cast(xs)... }; - - *this = Mat(std::move(init)); - } - - Mat() - { - for (int j = 0; j < h_; j++) - for (int i = 0; i < w_; i++) - data[j][i] = num(0); - } - - Mat(const num* mem) - { - for (int j = 0; j < h_; j++) - for (int i = 0; i < w_; i++) - data[j][i] = mem[i*h_+j]; - } - - Mat(std::initializer_list&& 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(data); } - operator const num*() const { return reinterpret_cast(data); } - - // XXX add more operators as needed, third-party dependencies mostly - // not needed merely for matrix algebra -sh 20141030 - - template - static typename std::enable_if>::type eye() - { - static_assert(h_ == h__, ""); - - Mat ret; - for (int j = 0; j < h_; j++) - for (int i = 0; i < w_; i++) - ret.data[j][i] = 0; - - for (int i = 0; i < h_; i++) - ret.data[i][i] = 1; - - return ret; - } - - Mat t() const - { - Mat ret; - - for (int j = 0; j < h_; j++) - for (int i = 0; i < w_; i++) - ret(i, j) = data[j][i]; - - return ret; - } -}; - -template using dmat = Mat; - -template -Mat operator*(num scalar, const Mat& mat) -{ - return mat * scalar; -} - -template -Mat operator*(const Mat& self, num other) -{ - Mat 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 { - -template using dmat = Mat; -using rmat = dmat<3, 3>; -using euler_t = dmat<3, 1>; - -OPENTRACK_API_EXPORT rmat euler_to_rmat(const euler_t& input); - -// http://stackoverflow.com/a/18436193 -euler_t OPENTRACK_API_EXPORT rmat_to_euler(const dmat<3, 3>& R); - -} // end ns euler diff --git a/pose-widget/CMakeLists.txt b/pose-widget/CMakeLists.txt index d70c3487..d298eee0 100644 --- a/pose-widget/CMakeLists.txt +++ b/pose-widget/CMakeLists.txt @@ -1 +1,2 @@ opentrack_boilerplate(opentrack-pose-widget BIN) +target_link_libraries(opentrack-pose-widget opentrack-logic) -- cgit v1.2.3