summaryrefslogtreecommitdiffhomepage
path: root/compat/euler.cpp
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2018-08-03 04:37:49 +0000
committerStanislaw Halik <sthalik@misaki.pl>2018-08-04 13:40:26 +0000
commite841b3324655b5862dc6d79d482b2af474239791 (patch)
treeeace21c2eb0683b664d9b9594b8ef6aa85007451 /compat/euler.cpp
parent9021529ff171425d61a3ffec7a41a652799bc6e5 (diff)
Revert "compat/euler: move gimbal lock from yaw to roll"
This reverts commit d5cd7d31c92593c593c8c809588284d61316c2ec.
Diffstat (limited to 'compat/euler.cpp')
-rw-r--r--compat/euler.cpp59
1 files changed, 37 insertions, 22 deletions
diff --git a/compat/euler.cpp b/compat/euler.cpp
index e9789bee..44b34f90 100644
--- a/compat/euler.cpp
+++ b/compat/euler.cpp
@@ -2,38 +2,53 @@
#include "math-imports.hpp"
#include <cmath>
-// rotation order is XYZ
-
namespace euler {
euler_t OTR_COMPAT_EXPORT rmat_to_euler(const rmat& R)
{
- double alpha, beta, gamma;
-
- beta = atan2( -R(2,0), sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) );
- alpha = atan2( R(1,0), R(0,0));
- gamma = atan2( R(2,1), R(2,2));
-
- return { alpha, beta, gamma };
+ 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 {
+ atan2(-R(1, 0), R(0, 0)),
+ atan2(R(2, 0), cy),
+ atan2(-R(2, 1), R(2, 2))
+ };
+ else
+ return {
+ atan2(R(0, 1), R(1, 1)),
+ atan2(R(2, 0), cy),
+ 0
+ };
}
-rmat OTR_COMPAT_EXPORT euler_to_rmat(const euler_t& e)
+// tait-bryan angles, not euler
+rmat OTR_COMPAT_EXPORT euler_to_rmat(const euler_t& input)
{
- const double X = -e(2);
- const double Y = -e(1);
- const double Z = -e(0);
+ const double H = -input(0);
+ const double P = -input(1);
+ const double B = -input(2);
- const double cx = cos(X);
- const double sx = sin(X);
- const double cy = cos(Y);
- const double sy = sin(Y);
- const double cz = cos(Z);
- const double sz = sin(Z);
+ 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 {
- cy*cz, -cy*sz, sy,
- cz*sx*sy + cx*sz, cx*cz - sx*sy*sz, -cy*sx,
- -cx*cz*sy + sx*sz, cz*sx + cx*sy*sz, cx*cy,
+ // 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
};
}