From b2172d88fb445155db84aa64720fa61fd1aa3340 Mon Sep 17 00:00:00 2001 From: Hartmut Seichter Date: Mon, 21 Jan 2019 17:11:10 +0100 Subject: [PATCH] fiddling ... --- src/core/include/pw/core/matrix.hpp | 30 +- src/core/include/pw/core/quaternion.hpp | 358 ++++++++++++------------ src/core/include/pw/core/vector.hpp | 9 +- 3 files changed, 203 insertions(+), 194 deletions(-) diff --git a/src/core/include/pw/core/matrix.hpp b/src/core/include/pw/core/matrix.hpp index 85ceced..029397a 100644 --- a/src/core/include/pw/core/matrix.hpp +++ b/src/core/include/pw/core/matrix.hpp @@ -31,19 +31,34 @@ namespace pw { +template +struct matrixtraits_ +{ + //! rows + inline std::size_t rows() const { return R; } + + //! return number of columns + inline std::size_t cols() const { return C; } +}; + template -struct matrix_ : matrixbase_> { +struct matrix_ : matrixbase_>,matrixtraits_ +{ T data[R*C]; - typedef matrixbase_> Base; - using Base::Base; +// typedef matrixbase_> Base; + using matrixbase_>::matrixbase_; + using matrixtraits_::rows; + using matrixtraits_::cols; + matrix_(const matrix_& other) { *this = other; } + explicit matrix_(std::initializer_list args) { typename std::initializer_list::iterator it = args.begin(); @@ -64,14 +79,9 @@ struct matrix_ : matrixbase_> { return *this; } - //! rows - inline std::size_t rows() const { return R; } - //! return number of columns - inline std::size_t cols() const { return C; } - - //! get cell count - inline std::size_t coefficients() const { return this->size(); } +// //! get cell count +// inline std::size_t coefficients() const { return this->size(); } inline size_t offset(size_t r,size_t c) const { return (RowMajor) ? r * C + c : c * R + r; diff --git a/src/core/include/pw/core/quaternion.hpp b/src/core/include/pw/core/quaternion.hpp index 4948b00..2fe840b 100644 --- a/src/core/include/pw/core/quaternion.hpp +++ b/src/core/include/pw/core/quaternion.hpp @@ -33,79 +33,79 @@ namespace pw { template struct quaternion_ : vector4_ { - typedef vector4_ Base; + typedef vector4_ Base; - using Base::Base; - using Base::x; - using Base::y; - using Base::z; - using Base::w; -// using Base::lerp; -// using Base::operator*; -// using Base::operator/; + using Base::Base; + using Base::x; + using Base::y; + using Base::z; + using Base::w; + // using Base::lerp; + // using Base::operator*; + using Base::operator/; - inline const quaternion_ operator * (const quaternion_& rhs) const { - return quaternion_( - rhs.w()*x() + rhs.x()*w() + rhs.y()*z() - rhs.z()*y(), - rhs.w()*y() - rhs.x()*z() + rhs.y()*w() + rhs.z()*x(), - rhs.w()*z() + rhs.x()*y() - rhs.y()*x() + rhs.z()*w(), - rhs.w()*w() - rhs.x()*x() - rhs.y()*y() - rhs.z()*z() - ); - } + inline const quaternion_ operator * (const quaternion_& rhs) const { + return quaternion_( + rhs.w()*x() + rhs.x()*w() + rhs.y()*z() - rhs.z()*y(), + rhs.w()*y() - rhs.x()*z() + rhs.y()*w() + rhs.z()*x(), + rhs.w()*z() + rhs.x()*y() - rhs.y()*x() + rhs.z()*w(), + rhs.w()*w() - rhs.x()*x() - rhs.y()*y() - rhs.z()*z() + ); + } - //! conjugate - inline quaternion_ conjugate() const { return quaternion_( { -x(),-y(),-z(),w() } ); } + //! conjugate + inline quaternion_ conjugate() const { return quaternion_( { -x(),-y(),-z(),w() } ); } - //! compute inverse - inline auto inverse() const { - return conjugate() / this->norm(); - } + //! compute inverse + inline quaternion_ inverse() const { + return conjugate() / this->norm(); + } - const matrix4x4_ to_matrix() const { + const matrix4x4_ to_matrix() const { - matrix4x4_ m; m.set_identity(); + matrix4x4_ m; m.set_identity(); - T xx = x() * x(); - T xy = x() * y(); - T xz = x() * z(); - T xw = x() * w(); + T xx = x() * x(); + T xy = x() * y(); + T xz = x() * z(); + T xw = x() * w(); - T yy = y() * y(); - T yz = y() * z(); - T yw = y() * w(); + T yy = y() * y(); + T yz = y() * z(); + T yw = y() * w(); - T zz = z() * z(); - T zw = z() * w(); + T zz = z() * z(); + T zw = z() * w(); - m(0,0) = 1 - 2 * ( yy + zz ); - m(0,1) = 2 * ( xy - zw ); - m(0,2) = 2 * ( xz + yw ); + m(0,0) = 1 - 2 * ( yy + zz ); + m(0,1) = 2 * ( xy - zw ); + m(0,2) = 2 * ( xz + yw ); - m(1,0) = 2 * ( xy + zw ); - m(1,1) = 1 - 2 * ( xx + zz ); - m(1,2) = 2 * ( yz - xw ); + m(1,0) = 2 * ( xy + zw ); + m(1,1) = 1 - 2 * ( xx + zz ); + m(1,2) = 2 * ( yz - xw ); - m(2,0) = 2 * ( xz - yw ); - m(2,1) = 2 * ( yz + xw ); - m(2,2) = 1 - 2 * ( xx + yy ); + m(2,0) = 2 * ( xz - yw ); + m(2,1) = 2 * ( yz + xw ); + m(2,2) = 1 - 2 * ( xx + yy ); - return m; - } + return m; + } - static quaternion_ from_matrix(const matrix_ &m) { - using std::sqrt; - const T wtemp = sqrt(T(1) + m(0,0) + m(1,1) + m(2,2)) / T(2); - const T w4 = T(4.0) * wtemp; - return quaternion_( - (m(2,1) - m(1,2)) / w4, - (m(0,2) - m(2,0)) / w4, - (m(1,0) - m(0,1)) / w4, - wtemp); - } + static quaternion_ from_matrix(const matrix_ &m) { + using std::sqrt; + const T wtemp = sqrt(T(1) + m(0,0) + m(1,1) + m(2,2)) / T(2); + const T w4 = T(4.0) * wtemp; + return quaternion_( + (m(2,1) - m(1,2)) / w4, + (m(0,2) - m(2,0)) / w4, + (m(1,0) - m(0,1)) / w4, + wtemp); + } - static const quaternion_ normalized_lerp(const quaternion_ &a,const quaternion_ &b,const T &t) { - return quaternion_(lerp(a,b,t).normalized()); - } + static const quaternion_ normalized_lerp(const quaternion_ &a,const quaternion_ &b,const T &t) { + return quaternion_(lerp(a,b,t).normalized()); + } }; // @@ -129,129 +129,129 @@ typedef quaternion_ quaterniond; template class quaternion_ { - static const T _sqrt90; + static const T _sqrt90; public: - typedef vector4_ coefficient_type; - typedef T value_type; + typedef vector4_ coefficient_type; + typedef T value_type; - quaternion_() { *this = identity(); } + quaternion_() { *this = identity(); } - quaternion_(const T& x,const T& y,const T& z,const T& w) - : _q(coefficient_type(x,y,z,w)) {} + quaternion_(const T& x,const T& y,const T& z,const T& w) + : _q(coefficient_type(x,y,z,w)) {} - quaternion_(const coefficient_type& vec) { *this = vec; } + quaternion_(const coefficient_type& vec) { *this = vec; } - inline quaternion_& operator = (const coefficient_type& vec) { _q = vec; return *this; } + inline quaternion_& operator = (const coefficient_type& vec) { _q = vec; return *this; } - inline void set(const T& x,const T& y,const T& z,const T& w) { - _q.set(x,y,z,w); - } + inline void set(const T& x,const T& y,const T& z,const T& w) { + _q.set(x,y,z,w); + } - inline void set_x(const T& v) { x() = v; } - inline void set_y(const T& v) { y() = v; } - inline void set_z(const T& v) { z() = v; } - inline void set_w(const T& v) { w() = v; } + inline void set_x(const T& v) { x() = v; } + inline void set_y(const T& v) { y() = v; } + inline void set_z(const T& v) { z() = v; } + inline void set_w(const T& v) { w() = v; } - inline const coefficient_type& as_vector() const { return _q; } + inline const coefficient_type& as_vector() const { return _q; } - inline T& x() { return _q.x(); } - inline T& y() { return _q.x(); } - inline T& z() { return _q.z(); } - inline T& w() { return _q.w(); } + inline T& x() { return _q.x(); } + inline T& y() { return _q.x(); } + inline T& z() { return _q.z(); } + inline T& w() { return _q.w(); } - inline const T& x() const { return _q.z(); } - inline const T& y() const { return _q.y(); } - inline const T& z() const { return _q.z(); } - inline const T& w() const { return _q.w(); } + inline const T& x() const { return _q.z(); } + inline const T& y() const { return _q.y(); } + inline const T& z() const { return _q.z(); } + inline const T& w() const { return _q.w(); } - //! multiplication - inline const quaternion_ operator * (const quaternion_& rhs) const { - return quaternion_( - rhs.w()*x() + rhs.x()*w() + rhs.y()*z() - rhs.z()*y(), - rhs.w()*y() - rhs.x()*z() + rhs.y()*w() + rhs.z()*x(), - rhs.w()*z() + rhs.x()*y() - rhs.y()*x() + rhs.z()*w(), - rhs.w()*w() - rhs.x()*x() - rhs.y()*y() - rhs.z()*z() - ); - } + //! multiplication + inline const quaternion_ operator * (const quaternion_& rhs) const { + return quaternion_( + rhs.w()*x() + rhs.x()*w() + rhs.y()*z() - rhs.z()*y(), + rhs.w()*y() - rhs.x()*z() + rhs.y()*w() + rhs.z()*x(), + rhs.w()*z() + rhs.x()*y() - rhs.y()*x() + rhs.z()*w(), + rhs.w()*w() - rhs.x()*x() - rhs.y()*y() - rhs.z()*z() + ); + } - //! multiply with scalar - inline const quaternion_ operator * (const T& s) const { - return quaternion_(x()*s,y()*s,z()*s,w()*s); - } + //! multiply with scalar + inline const quaternion_ operator * (const T& s) const { + return quaternion_(x()*s,y()*s,z()*s,w()*s); + } - //! addition - inline const quaternion_ operator + (const quaternion_& rhs) const { - return quaternion_(coefficient_type(this->_q + rhs._q)); - } + //! addition + inline const quaternion_ operator + (const quaternion_& rhs) const { + return quaternion_(coefficient_type(this->_q + rhs._q)); + } - //! addition - inline const quaternion_ operator - (const quaternion_& rhs) const { - return quaternion_(this->_q - rhs._q); - } + //! addition + inline const quaternion_ operator - (const quaternion_& rhs) const { + return quaternion_(this->_q - rhs._q); + } - //! squared norm - inline const T squared_norm() const { return _q.squared_norm(); } + //! squared norm + inline const T squared_norm() const { return _q.squared_norm(); } - //! norm - inline const T norm() const { return _q.norm(); } + //! norm + inline const T norm() const { return _q.norm(); } - //! dot product - inline const T dot(const quaternion_& other) const { return dot(_q,other._q); } + //! dot product + inline const T dot(const quaternion_& other) const { return dot(_q,other._q); } - //! compute normalized - inline quaternion_ normalized() const { - return quaternion_(_q.normalized()); - } + //! compute normalized + inline quaternion_ normalized() const { + return quaternion_(_q.normalized()); + } - inline void normalize() { *this = this->normalized(); } + inline void normalize() { *this = this->normalized(); } - //! conversion from a matrix - inline static const quaternion_ from_matrix(const matrix_ &m); + //! conversion from a matrix + inline static const quaternion_ from_matrix(const matrix_ &m); - //! conversion to a matrix - const matrix_ to_matrix() const; + //! conversion to a matrix + const matrix_ to_matrix() const; - //! return identiy quaternion - static const quaternion_ identity(); + //! return identiy quaternion + static const quaternion_ identity(); - static const quaternion_ rotate_180_degree_around_x(); ///< rotate 180 degree around X axis - static const quaternion_ rotate_180_degree_around_y(); ///< rotate 180 degree around Y axis - static const quaternion_ rotate_180_degree_around_z(); ///< rotate 180 degree around Z axis + static const quaternion_ rotate_180_degree_around_x(); ///< rotate 180 degree around X axis + static const quaternion_ rotate_180_degree_around_y(); ///< rotate 180 degree around Y axis + static const quaternion_ rotate_180_degree_around_z(); ///< rotate 180 degree around Z axis - static const quaternion_ rotate_90_degree_around_x(bool negative = false); - static const quaternion_ rotate_90_degree_around_y(bool negative = false); - static const quaternion_ rotate_90_degree_around_z(bool negative = false); + static const quaternion_ rotate_90_degree_around_x(bool negative = false); + static const quaternion_ rotate_90_degree_around_y(bool negative = false); + static const quaternion_ rotate_90_degree_around_z(bool negative = false); - template - static const quaternion_ from_axisangle(const AxisAngleType &aa) { + template + static const quaternion_ from_axisangle(const AxisAngleType &aa) { - using std::sin; - using std::cos; + using std::sin; + using std::cos; - const T sinHalfAngle(sin(aa.angle() * T(0.5) )); + const T sinHalfAngle(sin(aa.angle() * T(0.5) )); - return quaternion_(aa.axis().x() * sinHalfAngle, // x - aa.axis().y() * sinHalfAngle, // y - aa.axis().z() * sinHalfAngle, // z - cos(aa.angle() * 0.5) // w - ); + return quaternion_(aa.axis().x() * sinHalfAngle, // x + aa.axis().y() * sinHalfAngle, // y + aa.axis().z() * sinHalfAngle, // z + cos(aa.angle() * 0.5) // w + ); - } + } - static const quaternion_ lerp(const quaternion_& qa,const quaternion_& qb,const T& t); - static const quaternion_ normalized_lerp(const quaternion_& qa,const quaternion_& qb,const T& t); - static const quaternion_ slerp(const quaternion_& qa,const quaternion_& qb,const T& t); + static const quaternion_ lerp(const quaternion_& qa,const quaternion_& qb,const T& t); + static const quaternion_ normalized_lerp(const quaternion_& qa,const quaternion_& qb,const T& t); + static const quaternion_ slerp(const quaternion_& qa,const quaternion_& qb,const T& t); protected: - coefficient_type _q; + coefficient_type _q; }; template @@ -265,42 +265,42 @@ const T quaternion_::_sqrt90 = std::sqrt(0.5); template const quaternion_ quaternion_::identity() { - return quaternion_(0,0,0,1); + return quaternion_(0,0,0,1); } template const quaternion_ quaternion_::rotate_180_degree_around_x() { - return quaternion_(1,0,0,0); + return quaternion_(1,0,0,0); } template const quaternion_ quaternion_::rotate_180_degree_around_y() { - return quaternion_(0,1,0,0); + return quaternion_(0,1,0,0); } template const quaternion_ quaternion_::rotate_180_degree_around_z() { - return quaternion_(0,0,1,0); + return quaternion_(0,0,1,0); } template const quaternion_ quaternion_::rotate_90_degree_around_x(bool negative/* = false*/) { - return quaternion_((negative) ? - _sqrt90 : _sqrt90,0,0,_sqrt90); + return quaternion_((negative) ? - _sqrt90 : _sqrt90,0,0,_sqrt90); } template const quaternion_ quaternion_::rotate_90_degree_around_y(bool negative/* = false*/) { - return quaternion_(0, (negative) ? -_sqrt90 : _sqrt90,0,_sqrt90); + return quaternion_(0, (negative) ? -_sqrt90 : _sqrt90,0,_sqrt90); } template const quaternion_ quaternion_::rotate_90_degree_around_z(bool negative/* = false*/) { - return quaternion_(0,0,(negative) ? -_sqrt90 : _sqrt90, _sqrt90); + return quaternion_(0,0,(negative) ? -_sqrt90 : _sqrt90, _sqrt90); } @@ -308,40 +308,40 @@ const quaternion_ quaternion_::rotate_90_degree_around_z(bool negative/* = template const quaternion_ quaternion_::slerp(const quaternion_& qa,const quaternion_& qb,const T& t) { - using std::abs; - using std::sqrt; - using std::acos; + using std::abs; + using std::sqrt; + using std::acos; - // quaternion to return - quaternion_ qm; - // Calculate angle between them. - double cosHalfTheta = qa.w() * qb.w() + qa.x() * qb.x() + qa.y() * qb.y() + qa.z() * qb.z(); - // if qa=qb or qa=-qb then theta = 0 and we can return qa - if (abs(cosHalfTheta) >= T(1.)) { - return qa; - } + // quaternion to return + quaternion_ qm; + // Calculate angle between them. + double cosHalfTheta = qa.w() * qb.w() + qa.x() * qb.x() + qa.y() * qb.y() + qa.z() * qb.z(); + // if qa=qb or qa=-qb then theta = 0 and we can return qa + if (abs(cosHalfTheta) >= T(1.)) { + return qa; + } - // Calculate temporary values. - double halfTheta = acos(cosHalfTheta); - double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta); - // if theta = 180 degrees then result is not fully defined - // we could rotate around any axis normal to qa or qb - if (::std::abs(sinHalfTheta) < 0.001){ // fabs is floating point absolute - qm.w() = (qa.w() * 0.5 + qb.w() * 0.5); - qm.x() = (qa.x() * 0.5 + qb.x() * 0.5); - qm.y() = (qa.y() * 0.5 + qb.y() * 0.5); - qm.z() = (qa.z() * 0.5 + qb.z() * 0.5); - return qm; - } - double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta; - double ratioB = sin(t * halfTheta) / sinHalfTheta; - //calculate Quaternion. - qm.w() = (qa.w() * ratioA + qb.w() * ratioB); - qm.x() = (qa.x() * ratioA + qb.x() * ratioB); - qm.y() = (qa.y() * ratioA + qb.y() * ratioB); - qm.z() = (qa.z() * ratioA + qb.z() * ratioB); + // Calculate temporary values. + double halfTheta = acos(cosHalfTheta); + double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta); + // if theta = 180 degrees then result is not fully defined + // we could rotate around any axis normal to qa or qb + if (::std::abs(sinHalfTheta) < 0.001){ // fabs is floating point absolute + qm.w() = (qa.w() * 0.5 + qb.w() * 0.5); + qm.x() = (qa.x() * 0.5 + qb.x() * 0.5); + qm.y() = (qa.y() * 0.5 + qb.y() * 0.5); + qm.z() = (qa.z() * 0.5 + qb.z() * 0.5); + return qm; + } + double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta; + double ratioB = sin(t * halfTheta) / sinHalfTheta; + //calculate Quaternion. + qm.w() = (qa.w() * ratioA + qb.w() * ratioB); + qm.x() = (qa.x() * ratioA + qb.x() * ratioB); + qm.y() = (qa.y() * ratioA + qb.y() * ratioB); + qm.z() = (qa.z() * ratioA + qb.z() * ratioB); - return qm; + return qm; } diff --git a/src/core/include/pw/core/vector.hpp b/src/core/include/pw/core/vector.hpp index 235bd5b..5b59463 100644 --- a/src/core/include/pw/core/vector.hpp +++ b/src/core/include/pw/core/vector.hpp @@ -27,22 +27,21 @@ namespace pw { - template struct vector_ : matrix_ { typedef matrix_ Base; using typename Base::value_type; - using Base::Base; + using Base::Base; - static T dot(const vector_ &a,const vector_ &b) { + static T dot(const Base &a,const Base &b) { vector_ r; for (size_t i = 0;i < N;i++) r[i] = a[i] * b[i]; return std::accumulate(std::begin(r), std::end(r), T(0)); } - static T angle_between(const vector_ &a,const vector_ &b) { - return std::acos( dot( a.normalized(), b.normalized() ) ); + static T angle_between(const Base &a,const Base &b) { + return std::acos( dot( a.normalized(), b.normalized() ) ); } static const vector_ lerp(const vector_ &a,const vector_ &b,const T& t) {