diff options
Diffstat (limited to 'thirdparty/bullet/BulletInverseDynamics/details')
4 files changed, 32 insertions, 32 deletions
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp index 5bb4a33bdd..c179daeec6 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -49,9 +49,9 @@ inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; } class vecx : public btVectorX<idScalar> { public: - vecx(int size) : btVectorX(size) {} + vecx(int size) : btVectorX<idScalar>(size) {} const vecx& operator=(const btVectorX<idScalar>& rhs) { - *static_cast<btVectorX*>(this) = rhs; + *static_cast<btVectorX<idScalar>*>(this) = rhs; return *this; } @@ -78,7 +78,7 @@ 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()) { - error_message("size missmatch. a.size()= %d, b.size()= %d\n", 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++) { @@ -92,7 +92,7 @@ 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()) { - error_message("size missmatch. a.size()= %d, b.size()= %d\n", 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++) { @@ -121,7 +121,7 @@ public: } void operator=(const mat3x& rhs) { if (m_cols != rhs.m_cols) { - error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.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++) { @@ -139,7 +139,7 @@ public: inline vec3 operator*(const mat3x& a, const vecx& b) { vec3 result; if (a.cols() != b.size()) { - error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size()); + bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size()); abort(); } result(0)=0.0; diff --git a/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp b/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp index 4d3f6c87e9..c89db5e123 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/IDMatVec.hpp @@ -123,7 +123,7 @@ public: }; void operator=(const mat3x& rhs) { if (m_cols != rhs.m_cols) { - error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols()); + bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols()); abort(); } for(int i=0;i<3*m_cols;i++) { @@ -336,7 +336,7 @@ inline vec3 operator/(const vec3& a, const idScalar& s) { inline const vecx& vecx::operator=(const vecx& rhs) { if (size() != rhs.size()) { - error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size()); + bt_id_error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size()); abort(); } if (&rhs != this) { @@ -356,7 +356,7 @@ 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()) { - error_message("size missmatch. a.size()= %d, b.size()= %d\n", 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++) { @@ -369,7 +369,7 @@ 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()) { - error_message("size missmatch. a.size()= %d, b.size()= %d\n", 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++) { @@ -389,7 +389,7 @@ inline vecx operator/(const vecx& a, const idScalar& s) { inline vec3 operator*(const mat3x& a, const vecx& b) { vec3 result; if (a.cols() != b.size()) { - error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size()); + bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size()); abort(); } result(0)=0.0; diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp index b35c55df61..e8563238c3 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -80,7 +80,7 @@ int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const { case FLOATING: return 6; } - error_message("unknown joint type %d\n", type); + bt_id_error_message("unknown joint type %d\n", type); return 0; } @@ -136,13 +136,13 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() { q_index += 6; break; default: - error_message("unsupported joint type %d\n", body.m_joint_type); + bt_id_error_message("unsupported joint type %d\n", body.m_joint_type); return -1; } } // sanity check if (q_index != m_num_dofs) { - error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs); + bt_id_error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs); return -1; } @@ -155,10 +155,10 @@ int MultiBodyTree::MultiBodyImpl::generateIndexSets() { } else { if (-1 == parent) { // multiple bodies are directly linked to the environment, ie, not a single root - error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child); + bt_id_error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child); } else { // should never happen - error_message( + bt_id_error_message( "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n", child, parent, static_cast<int>(m_parent_index.size())); } @@ -234,7 +234,7 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const const vecx &dot_u, vecx *joint_forces) { if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs || joint_forces->size() != m_num_dofs) { - error_message("wrong vector dimension. system has %d DOFs,\n" + bt_id_error_message("wrong vector dimension. system has %d DOFs,\n" "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n", m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()), static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size())); @@ -242,7 +242,7 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const } // 1. relative kinematics if(-1 == calculateKinematics(q,u,dot_u, POSITION_VELOCITY_ACCELERATION)) { - error_message("error in calculateKinematics\n"); + bt_id_error_message("error in calculateKinematics\n"); return -1; } // 2. update contributions to equations of motion for every body. @@ -322,14 +322,14 @@ int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx& dot_u, const KinUpdateType type) { if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ) { - error_message("wrong vector dimension. system has %d DOFs,\n" + bt_id_error_message("wrong vector dimension. system has %d DOFs,\n" "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n", m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()), static_cast<int>(dot_u.size())); return -1; } if(type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION) { - error_message("invalid type %d\n", type); + bt_id_error_message("invalid type %d\n", type); return -1; } @@ -516,13 +516,13 @@ void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody&body) int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx& q, const vecx& u, const KinUpdateType type) { if (q.size() != m_num_dofs || u.size() != m_num_dofs) { - error_message("wrong vector dimension. system has %d DOFs,\n" + bt_id_error_message("wrong vector dimension. system has %d DOFs,\n" "but dim(q)= %d, dim(u)= %d\n", m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size())); return -1; } if(type != POSITION_ONLY && type != POSITION_VELOCITY) { - error_message("invalid type %d\n", type); + bt_id_error_message("invalid type %d\n", type); return -1; } @@ -606,7 +606,7 @@ static inline int jointNumDoFs(const JointType &type) { return 6; } // this should never happen - error_message("invalid joint type\n"); + bt_id_error_message("invalid joint type\n"); // TODO add configurable abort/crash function abort(); return 0; @@ -626,7 +626,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs || mass_matrix->cols() != m_num_dofs) { - error_message("Dimension error. System has %d DOFs,\n" + bt_id_error_message("Dimension error. System has %d DOFs,\n" "but dim(q)= %d, dim(mass_matrix)= %d x %d\n", m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()), static_cast<int>(mass_matrix->cols())); @@ -734,7 +734,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool // 1. for multi-dof joints, rest of the dofs of this body for (int row = col - 1; row >= q_index_min; row--) { if (FLOATING != body.m_joint_type) { - error_message("??\n"); + bt_id_error_message("??\n"); return -1; } setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT); @@ -788,7 +788,7 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool #define CHECK_IF_BODY_INDEX_IS_VALID(index) \ do { \ if (index < 0 || index >= m_num_bodies) { \ - error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \ + bt_id_error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \ return -1; \ } \ } while (0) diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp index 47b4ab3890..e9511b7076 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp @@ -29,13 +29,13 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind m_num_dofs += 6; break; default: - error_message("unknown joint type %d\n", joint_type); + bt_id_error_message("unknown joint type %d\n", joint_type); return -1; } if(-1 == parent_index) { if(m_root_index>=0) { - error_message("trying to add body %d as root, but already added %d as root body\n", + bt_id_error_message("trying to add body %d as root, but already added %d as root body\n", body_index, m_root_index); return -1; } @@ -63,7 +63,7 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind } int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const { if (index < 0 || index > static_cast<int>(m_inertias.size())) { - error_message("index out of range\n"); + bt_id_error_message("index out of range\n"); return -1; } @@ -73,7 +73,7 @@ int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inert int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const { if (index < 0 || index > static_cast<int>(m_user_int.size())) { - error_message("index out of range\n"); + bt_id_error_message("index out of range\n"); return -1; } *user_int = m_user_int[index]; @@ -82,7 +82,7 @@ int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const { int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const { if (index < 0 || index > static_cast<int>(m_user_ptr.size())) { - error_message("index out of range\n"); + bt_id_error_message("index out of range\n"); return -1; } *user_ptr = m_user_ptr[index]; @@ -91,7 +91,7 @@ int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const { if (index < 0 || index > static_cast<int>(m_joints.size())) { - error_message("index out of range\n"); + bt_id_error_message("index out of range\n"); return -1; } *joint = m_joints[index]; |