summaryrefslogtreecommitdiffhomepage
path: root/compat/euler.cpp
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2017-04-11 11:41:31 +0200
committerStanislaw Halik <sthalik@misaki.pl>2017-04-12 01:47:41 +0200
commit1d828d640e6d33a473e0a3e08e976bb9cb1b9346 (patch)
tree6ea166aa3c2367a0114e4da3212f51e3a5160a33 /compat/euler.cpp
parentd3c55b0cf8b55d5d4b5c2108d3865de20e3137ea (diff)
compat/euler: add incomplete & untested quat code
Diffstat (limited to 'compat/euler.cpp')
-rw-r--r--compat/euler.cpp39
1 files changed, 39 insertions, 0 deletions
diff --git a/compat/euler.cpp b/compat/euler.cpp
index ab119d3e..e48d977b 100644
--- a/compat/euler.cpp
+++ b/compat/euler.cpp
@@ -93,4 +93,43 @@ void OTR_COMPAT_EXPORT tait_bryan_to_matrices(const euler_t& input,
}
}
+// from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+Quat matrix_to_quat(const rmat& M)
+{
+ Quat q(1, 0, 0, 0);
+
+ using std::sqrt;
+
+ double trace = M(0, 0) + M(1, 1) + M(2, 2); // I removed + 1.0; see discussion with Ethan
+ if( trace > 0 ) {// I changed M_EPSILON to 0
+ double s = .5 / std::sqrt(trace + 1);
+ q.w() = .25 / s;
+ q.x() = ( M(2, 1) - M(1, 2) ) * s;
+ q.y() = ( M(0, 2) - M(2, 0) ) * s;
+ q.z() = ( M(1, 0) - M(0, 1) ) * s;
+ } else {
+ if ( M(0, 0) > M(1, 1) && M(0, 0) > M(2, 2) ) {
+ double s = 2.0 * sqrt( 1.0 + M(0, 0) - M(1, 1) - M(2, 2));
+ q.w() = (M(2, 1) - M(1, 2) ) / s;
+ q.x() = .25 * s;
+ q.y() = (M(0, 1) + M(1, 0) ) / s;
+ q.z() = (M(0, 2) + M(2, 0) ) / s;
+ } else if (M(1, 1) > M(2, 2)) {
+ double s = 2.0 * sqrt( 1.0 + M(1, 1) - M(0, 0) - M(2, 2));
+ q.w() = (M(0, 2) - M(2, 0) ) / s;
+ q.x() = (M(0, 1) + M(1, 0) ) / s;
+ q.y() = .25 * s;
+ q.z() = (M(1, 2) + M(2, 1) ) / s;
+ } else {
+ double s = 2.0 * sqrt( 1.0 + M(2, 2) - M(0, 0) - M(1, 1) );
+ q.w() = (M(1, 0) - M(0, 1) ) / s;
+ q.x() = (M(0, 2) + M(2, 0) ) / s;
+ q.y() = (M(1, 2) + M(2, 1) ) / s;
+ q.z() = .25 * s;
+ }
+ }
+
+ return q;
+}
+
} // end ns euler