summaryrefslogtreecommitdiffhomepage
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
parentd3c55b0cf8b55d5d4b5c2108d3865de20e3137ea (diff)
compat/euler: add incomplete & untested quat code
-rw-r--r--compat/euler.cpp39
-rw-r--r--compat/euler.hpp3
-rw-r--r--compat/simple-mat.hpp63
3 files changed, 105 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
diff --git a/compat/euler.hpp b/compat/euler.hpp
index c50cf052..4eb6e00c 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);
+Quat OTR_COMPAT_EXPORT matrix_to_quat(const rmat& M);
+//XXX TODO rmat OTR_COMPAT_EXPORT quat_to_matrix(const Quat<double>& Q);
+
} // end ns euler
diff --git a/compat/simple-mat.hpp b/compat/simple-mat.hpp
index fbd0a75e..906c0969 100644
--- a/compat/simple-mat.hpp
+++ b/compat/simple-mat.hpp
@@ -273,3 +273,66 @@ Mat<num, h_, w_> operator*(const Mat<num, h_, w_>& self, num other)
ret(j, i) = self(j, i) * other;
return ret;
}
+
+template<typename num>
+class Quat : Mat<num, 4, 1>
+{
+ using quat = Quat<num>;
+
+ enum idx { qw, qx, qy, qz };
+
+ static quat _from_array(const num* data)
+ {
+ return quat(data[qw], data[qx], data[qy], data[qz]);
+ }
+
+ inline num elt(idx k) const { return operator()(k); }
+ inline num& elt(idx k) { return operator()(k); }
+public:
+ Quat(num w, num x, num y, num z) : Mat<num, 4, 1>(w, x, y, z)
+ {
+ }
+
+ Quat() : quat(1, 0, 0, 0) {}
+
+ static quat from_vector(const Mat<num, 4, 1>& data)
+ {
+ return _from_array(data);
+ }
+
+ static quat from_vector(const Mat<num, 1, 4>& data)
+ {
+ return _from_array(data);
+ }
+
+ quat normalized() const
+ {
+ const num x = elt(qx), y = elt(qy), z = elt(qz), w = elt(qw);
+ const num inv_n = 1./std::sqrt(x*x + y*y + z*z + w*w);
+
+ return Quat<num>(elt(qw) * inv_n,
+ elt(qx) * inv_n,
+ elt(qy) * inv_n,
+ elt(qz) * inv_n);
+ }
+
+ quat operator*(const quat& q2)
+ {
+ const quat& OTR_RESTRICT q1 = *this;
+ return quat(-q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z() + q1.w() * q2.w(),
+ q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y() + q1.w() * q2.x(),
+ -q1.x() * q2.z() + q1.y() * q2.w() + q1.z() * q2.x() + q1.w() * q2.y(),
+ q1.x() * q2.y() - q1.y() * q2.x() + q1.z() * q2.w() + q1.w() * q2.z());
+ }
+
+
+ inline num w() const { return elt(qw); }
+ inline num x() const { return elt(qx); }
+ inline num y() const { return elt(qy); }
+ inline num z() const { return elt(qz); }
+
+ inline num& w() { return elt(qw); }
+ inline num& x() { return elt(qx); }
+ inline num& y() { return elt(qy); }
+ inline num& z() { return elt(qz); }
+};