diff options
| -rw-r--r-- | compat/euler.cpp | 39 | ||||
| -rw-r--r-- | compat/euler.hpp | 3 | ||||
| -rw-r--r-- | compat/simple-mat.hpp | 63 |
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); } +}; |