diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp index 446c5cb7751e5191e68128e24d0a6907bc876dde..e230a096def6b9090edb899ef0e7804939753d62 100644 --- a/src/multibody/geometry.hpp +++ b/src/multibody/geometry.hpp @@ -241,6 +241,15 @@ namespace se3 /// attached to the body from the joint center. /// std::vector<double> radius; + + /// + /// \brief index of the collision pair + /// + /// It is used by some method to return additional information. For instance, + /// isColliding() sets it to the first colliding pair. + /// + Index collisionPairIndex; + GeometryData(const GeometryModel & modelGeom) : model_geom(modelGeom) , oMg(model_geom.ngeoms) @@ -289,12 +298,8 @@ namespace se3 /// \brief Check if at least one of the collision pairs has its two collision objects in collision. /// The results are stored in the vector GeometryData::collision_results. /// - /// \param[out] the index of the colliding pair if the function returns true. + /// Set collisionPairIndex to the index of the first colliding pair in collision. /// - bool isColliding(Index& collidingPair); - - /// \brief See bool isColliding(Index&) - /// As isColliding(Index&) without argument bool isColliding(); /// diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx index 7db7e94553604a4495ea3d349ae71f6849306439..ace3204772a9c38ba76d5afe289a47c9116b62df 100644 --- a/src/multibody/geometry.hxx +++ b/src/multibody/geometry.hxx @@ -232,8 +232,9 @@ namespace se3 } } - inline bool GeometryData::isColliding(Index& i) + inline bool GeometryData::isColliding() { + Index& i = collisionPairIndex; for(i = 0; i<model_geom.collisionPairs.size(); ++i) { if (activeCollisionPairs[i] && computeCollision(i)) @@ -242,12 +243,6 @@ namespace se3 return false; } - inline bool GeometryData::isColliding() - { - Index pair; - return isColliding(pair); - } - inline DistanceResult GeometryData::computeDistance(const CollisionPair & pair) const { const Index & co1 = pair.first; assert(co1<collisionObjects.size());