summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2018-01-05 01:27:58 +0100
committerStanislaw Halik <sthalik@misaki.pl>2018-01-05 01:27:58 +0100
commitdbacc00b52e294f65a8d292dbcce0f4fc93b5bf0 (patch)
tree5bd5eb8b0ac9750d7cd3d77a9b55c7f2122db343
parent4a2d23c14089cb98901e3df535037f7d8095e9a7 (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.cpp22
-rw-r--r--compat/euler.hpp3
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