diff options
| author | Stanislaw Halik <sthalik@misaki.pl> | 2018-01-05 01:27:58 +0100 |
|---|---|---|
| committer | Stanislaw Halik <sthalik@misaki.pl> | 2018-01-05 01:27:58 +0100 |
| commit | dbacc00b52e294f65a8d292dbcce0f4fc93b5bf0 (patch) | |
| tree | 5bd5eb8b0ac9750d7cd3d77a9b55c7f2122db343 | |
| parent | 4a2d23c14089cb98901e3df535037f7d8095e9a7 (diff) | |
compat/euler: add quat -> rmat code
It was meant as part of AHRS using acc+gyro only, but it was too
unreliable. The conversion code is correct and useful though.
| -rw-r--r-- | compat/euler.cpp | 22 | ||||
| -rw-r--r-- | compat/euler.hpp | 3 |
2 files changed, 22 insertions, 3 deletions
diff --git a/compat/euler.cpp b/compat/euler.cpp index a868478e..7c753214 100644 --- a/compat/euler.cpp +++ b/compat/euler.cpp @@ -54,9 +54,9 @@ rmat OTR_COMPAT_EXPORT euler_to_rmat(const euler_t& input) // https://en.wikipedia.org/wiki/Davenport_chained_rotations#Tait.E2.80.93Bryan_chained_rotations void OTR_COMPAT_EXPORT tait_bryan_to_matrices(const euler_t& input, - rmat& r_roll, - rmat& r_pitch, - rmat& r_yaw) + rmat& r_roll, + rmat& r_pitch, + rmat& r_yaw) { { const double phi = -input(2); @@ -95,4 +95,20 @@ void OTR_COMPAT_EXPORT tait_bryan_to_matrices(const euler_t& input, } } +template<int H, int W, typename f = double> +rmat quaternion_to_mat_(const Mat<f, H, W>& q) +{ + const double w = q.w(), x = q.x(), y = q.y(), z = q.z(); + const double ww = w*w, xx = x*x, yy = y*y, zz = z*z; + + return rmat( + ww + xx - yy - zz, 2 * (x*y - w*z), 2 * (x*z + w*y), + 2 * (x*z - w*y), 2 * (y*z + w*x), ww - xx - yy + zz, + 2 * (x*y + w*z), ww - xx + yy - zz, 2 * (y*z - w*x) + ); +} + +rmat OTR_COMPAT_EXPORT quaternion_to_mat(const dmat<1, 4>& q) { return quaternion_to_mat_(q); } +//rmat OTR_COMPAT_EXPORT quaternion_to_mat(const dmat<4, 1>& q) { return quaternion_to_mat_(q); } + } // end ns euler diff --git a/compat/euler.hpp b/compat/euler.hpp index c50cf052..147f4c50 100644 --- a/compat/euler.hpp +++ b/compat/euler.hpp @@ -22,4 +22,7 @@ void OTR_COMPAT_EXPORT tait_bryan_to_matrices(const euler_t& input, rmat& r_pitch, rmat& r_yaw); +rmat OTR_COMPAT_EXPORT quaternion_to_mat(const dmat<1, 4>& q); +//rmat OTR_COMPAT_EXPORT quaternion_to_mat(const dmat<4, 1>& q); + } // end ns euler |