From 22b7c9dfa80d0f7abca40f061865c2ab3c136a74 Mon Sep 17 00:00:00 2001 From: Oussama Date: Thu, 3 Jan 2019 14:26:51 +0100 Subject: Update Bullet to the latest commit 126b676 --- .../details/IDLinearMathInterface.hpp | 156 ++++++++++++--------- 1 file changed, 93 insertions(+), 63 deletions(-) (limited to 'thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp') diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp index c179daeec6..0c398a3727 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -10,32 +10,37 @@ #include "../../LinearMath/btMatrixX.h" #define BT_ID_HAVE_MAT3X -namespace btInverseDynamics { +namespace btInverseDynamics +{ class vec3; class vecx; class mat33; typedef btMatrixX matxx; -class vec3 : public btVector3 { +class vec3 : public btVector3 +{ public: vec3() : btVector3() {} vec3(const btVector3& btv) { *this = btv; } idScalar& operator()(int i) { return (*this)[i]; } const idScalar& operator()(int i) const { return (*this)[i]; } int size() const { return 3; } - const vec3& operator=(const btVector3& rhs) { + const vec3& operator=(const btVector3& rhs) + { *static_cast(this) = rhs; return *this; } }; -class mat33 : public btMatrix3x3 { +class mat33 : public btMatrix3x3 +{ public: mat33() : btMatrix3x3() {} mat33(const btMatrix3x3& btm) { *this = btm; } idScalar& operator()(int i, int j) { return (*this)[i][j]; } const idScalar& operator()(int i, int j) const { return (*this)[i][j]; } - const mat33& operator=(const btMatrix3x3& rhs) { + const mat33& operator=(const btMatrix3x3& rhs) + { *static_cast(this) = rhs; return *this; } @@ -47,10 +52,12 @@ inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s) inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } -class vecx : public btVectorX { +class vecx : public btVectorX +{ public: vecx(int size) : btVectorX(size) {} - const vecx& operator=(const btVectorX& rhs) { + const vecx& operator=(const btVectorX& rhs) + { *static_cast*>(this) = rhs; return *this; } @@ -66,43 +73,53 @@ public: friend vecx operator/(const vecx& a, const idScalar& s); }; -inline vecx operator*(const vecx& a, const idScalar& s) { +inline vecx operator*(const vecx& a, const idScalar& s) +{ vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { + for (int i = 0; i < result.size(); i++) + { result(i) = a(i) * s; } return result; } inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; } -inline vecx operator+(const vecx& a, const vecx& b) { +inline vecx operator+(const vecx& a, const vecx& b) +{ vecx result(a.size()); // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { + if (a.size() != b.size()) + { bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); abort(); } - for (int i = 0; i < a.size(); i++) { + for (int i = 0; i < a.size(); i++) + { result(i) = a(i) + b(i); } return result; } -inline vecx operator-(const vecx& a, const vecx& b) { +inline vecx operator-(const vecx& a, const vecx& b) +{ vecx result(a.size()); // TODO: error handling for a.size() != b.size()?? - if (a.size() != b.size()) { + if (a.size() != b.size()) + { bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size()); abort(); } - for (int i = 0; i < a.size(); i++) { + for (int i = 0; i < a.size(); i++) + { result(i) = a(i) - b(i); } return result; } -inline vecx operator/(const vecx& a, const idScalar& s) { +inline vecx operator/(const vecx& a, const idScalar& s) +{ vecx result(a.size()); - for (int i = 0; i < result.size(); i++) { + for (int i = 0; i < result.size(); i++) + { result(i) = a(i) / s; } @@ -110,63 +127,76 @@ inline vecx operator/(const vecx& a, const idScalar& s) { } // use btMatrixX to implement 3xX matrix -class mat3x : public matxx { +class mat3x : public matxx +{ public: - mat3x(){} - mat3x(const mat3x&rhs) { - matxx::resize(rhs.rows(), rhs.cols()); - *this = rhs; - } - mat3x(int rows, int cols): matxx(3,cols) { - } - void operator=(const mat3x& rhs) { - if (m_cols != rhs.m_cols) { - bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols()); - abort(); + mat3x() {} + mat3x(const mat3x& rhs) + { + matxx::resize(rhs.rows(), rhs.cols()); + *this = rhs; + } + mat3x(int rows, int cols) : matxx(3, cols) + { + } + void operator=(const mat3x& rhs) + { + if (m_cols != rhs.m_cols) + { + bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols()); + abort(); + } + for (int i = 0; i < rows(); i++) + { + for (int k = 0; k < cols(); k++) + { + setElem(i, k, rhs(i, k)); + } + } + } + void setZero() + { + matxx::setZero(); } - for(int i=0;isetElem(row, col, val); +inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx* m) +{ + m->setElem(row, col, val); } -inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){ - m->setElem(row, col, val); +inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x* m) +{ + m->setElem(row, col, val); } -} +} // namespace btInverseDynamics #endif // IDLINEARMATHINTERFACE_HPP_ -- cgit v1.2.3