diff --git a/src/multibody/fwd.hpp b/src/multibody/fwd.hpp
index 66604d80c7813e3f6c4b341b022a2bb5515084f7..c504e0cf80e23bc5bb036f6d2a01b9cac2203ecc 100644
--- a/src/multibody/fwd.hpp
+++ b/src/multibody/fwd.hpp
@@ -26,6 +26,7 @@ namespace se3
typedef Index JointIndex;
typedef Index GeomIndex;
typedef Index FrameIndex;
+ typedef Index PairIndex;
struct Frame;
struct Model;
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index 1726423b10acde1de67788a2eb047c5f2605be81..734223f4cbc23727df09e1fe65b7ca2cb9d53893 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -159,10 +159,8 @@ namespace se3
///
/// \return The index of the CollisionPair in collisionPairs.
///
- Index findCollisionPair (const CollisionPair & pair) const;
+ PairIndex findCollisionPair (const CollisionPair & pair) const;
- /// \brief Display on std::cout the list of pairs (is it really useful?).
- void displayCollisionPairs() const;
#endif // WITH_HPP_FCL
friend std::ostream& operator<<(std::ostream & os, const GeometryModel & model_geom);
@@ -231,7 +229,7 @@ namespace se3
/// It is used by some method to return additional information. For instance,
/// isColliding() sets it to the first colliding pair.
///
- Index collisionPairIndex;
+ PairIndex collisionPairIndex;
typedef std::vector<GeomIndex> GeomIndexList;
@@ -286,13 +284,13 @@ namespace se3
/// manner.
/// \param[in] pairId the index of the pair in GeomModel::collisionPairs vector.
/// \param[in] new value of the activation boolean (true by default).
- void activateCollisionPair(const Index pairId,const bool flag=true);
+ void activateCollisionPair(const PairIndex pairId,const bool flag=true);
/// Deactivate a collision pair.
///
/// Calls indeed GeomData::activateCollisionPair(pairId,false)
/// \sa activateCollisionPair
- void deactivateCollisionPair(const Index pairId);
+ void deactivateCollisionPair(const PairIndex pairId);
///
/// \brief Compute the collision status between two collision objects of a given collision pair.
@@ -302,7 +300,7 @@ namespace se3
///
/// \return Return true is the collision objects are colliding.
///
- bool computeCollision(const Index & pairId);
+ bool computeCollision(const PairIndex & pairId);
///
/// \brief Check if at least one of the collision pairs has its two collision objects in collision.
@@ -320,7 +318,7 @@ namespace se3
/// \return A reference on fcl struct containing the distance result, referring an element
/// of vector geomData::distance_results.
///
- fcl::DistanceResult & computeDistance(const Index & pairId);
+ fcl::DistanceResult & computeDistance(const PairIndex & pairId);
/// Reset the vector distance_results.
/// TODO: should this be called automatically before calling computeDistance?
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index f0e96ea122fb8681f425ea030ab4e61377e2775f..3caeab6f93e9abec330c65e542b6aa8d331e747e 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -34,7 +34,7 @@ namespace se3
inline GeomIndex GeometryModel::addGeometryObject(const GeometryObject& object)
{
- Index idx = (Index) (ngeoms ++);
+ GeomIndex idx = (GeomIndex) (ngeoms ++);
geometryObjects.push_back(object);
return idx;
}
@@ -131,7 +131,7 @@ namespace se3
{
os << "Nb geometry objects = " << model_geom.ngeoms << std::endl;
- for(Index i=0;i<(Index)(model_geom.ngeoms);++i)
+ for(GeomIndex i=0;i<(GeomIndex)(model_geom.ngeoms);++i)
{
os << model_geom.geometryObjects[i] <<std::endl;
}
@@ -144,7 +144,7 @@ namespace se3
#ifdef WITH_HPP_FCL
os << "Nb collision pairs = " << data_geom.activeCollisionPairs.size() << std::endl;
- for(Index i=0;i<(Index)(data_geom.activeCollisionPairs.size());++i)
+ for(PairIndex i=0;i<(PairIndex)(data_geom.activeCollisionPairs.size());++i)
{
os << "collision object in position " << data_geom.model_geom.collisionPairs[i] << std::endl;
}
@@ -167,10 +167,10 @@ namespace se3
inline void GeometryModel::addAllCollisionPairs()
{
removeAllCollisionPairs();
- for (Index i = 0; i < ngeoms; ++i)
+ for (GeomIndex i = 0; i < ngeoms; ++i)
{
const JointIndex& joint_i = geometryObjects[i].parentJoint;
- for (Index j = i+1; j < ngeoms; ++j)
+ for (GeomIndex j = i+1; j < ngeoms; ++j)
{
const JointIndex& joint_j = geometryObjects[j].parentJoint;
if (joint_i != joint_j)
@@ -198,45 +198,36 @@ namespace se3
pair) != collisionPairs.end());
}
- inline Index GeometryModel::findCollisionPair (const CollisionPair & pair) const
+ inline PairIndex GeometryModel::findCollisionPair (const CollisionPair & pair) const
{
CollisionPairsVector_t::const_iterator it = std::find(collisionPairs.begin(),
collisionPairs.end(),
pair);
- return (Index) std::distance(collisionPairs.begin(), it);
+ return (PairIndex) std::distance(collisionPairs.begin(), it);
}
- inline void GeometryModel::displayCollisionPairs() const
- {
- for (CollisionPairsVector_t::const_iterator it = collisionPairs.begin();
- it != collisionPairs.end(); ++it)
- {
- std::cout << it-> first << "\t" << it->second << std::endl;
- }
- }
-
- inline void GeometryData::activateCollisionPair(const Index pairId,const bool flag)
+ inline void GeometryData::activateCollisionPair(const PairIndex pairId,const bool flag)
{
assert( activeCollisionPairs.size() == model_geom.collisionPairs.size() );
assert( pairId < activeCollisionPairs.size() );
activeCollisionPairs[pairId] = flag;
}
- inline void GeometryData::deactivateCollisionPair(const Index pairId)
+ inline void GeometryData::deactivateCollisionPair(const PairIndex pairId)
{
assert( activeCollisionPairs.size() == model_geom.collisionPairs.size() );
assert( pairId < activeCollisionPairs.size() );
activeCollisionPairs[pairId] = false;
}
- inline bool GeometryData::computeCollision(const Index& pairId)
+ inline bool GeometryData::computeCollision(const PairIndex& pairId)
{
const CollisionPair & pair = model_geom.collisionPairs[pairId];
fcl::CollisionResult& collisionResult = collision_results[pairId];
- const Index & co1 = pair.first; assert(co1<collisionObjects.size());
- const Index & co2 = pair.second; assert(co2<collisionObjects.size());
+ const PairIndex & co1 = pair.first; assert(co1<collisionObjects.size());
+ const PairIndex & co2 = pair.second; assert(co2<collisionObjects.size());
collisionResult.clear();
fcl::collide (&collisionObjects[co1],&collisionObjects[co2],
@@ -247,7 +238,7 @@ namespace se3
inline bool GeometryData::isColliding()
{
- Index& i = collisionPairIndex;
+ PairIndex& i = collisionPairIndex;
for(i = 0; i<model_geom.collisionPairs.size(); ++i)
{
if (activeCollisionPairs[i] && computeCollision(i))
@@ -256,7 +247,7 @@ namespace se3
return false;
}
- inline fcl::DistanceResult & GeometryData::computeDistance(const Index & pairId )
+ inline fcl::DistanceResult & GeometryData::computeDistance(const PairIndex & pairId )
{
assert( pairId < model_geom.collisionPairs.size() );
const CollisionPair & pair = model_geom.collisionPairs[pairId];