diff --git a/src/algorithm/geometry.hxx b/src/algorithm/geometry.hxx
index 83853400d2ec172281445a97369242f3e0f62049..d4c6a3d455d3884749df73aaf7703808dbd3032c 100644
--- a/src/algorithm/geometry.hxx
+++ b/src/algorithm/geometry.hxx
@@ -206,21 +206,38 @@ namespace se3
);
}
- /// 2. Update the inner/outer objects
- typedef GeometryModel::GeomIndexList GeomIndexList;
+ /// 2. add the collision pairs between geomModel1 and geomModel2.
+ for (Index i = 0; i < nGeom1; ++i) {
+ for (Index j = 0; j < nGeom2; ++j) {
+ geomModel1.collisionPairs.push_back(CollisionPair(i, nGeom1 + j));
+ }
+ }
+ }
+
+ inline void appendGeometryModel(GeometryModel & geomModel1,
+ GeometryData & geomData1,
+ const GeometryModel & geomModel2,
+ const GeometryData & geomData2)
+ {
+ /// 1&2. Call previous function.
+ appendGeometryModel(geomModel1,geomModel2);
+
+ /// 3. Update the inner/outer objects
+ Index nGeom1 = geomModel1.geometryObjects.size();
+ typedef GeometryData::GeomIndexList GeomIndexList;
typedef std::map < JointIndex, GeomIndexList > Map_t;
- BOOST_FOREACH(const Map_t::value_type& innerObject, geomModel2.innerObjects)
+ BOOST_FOREACH(const Map_t::value_type& innerObject, geomData2.innerObjects)
{
- GeomIndexList& innerGeoms = geomModel1.innerObjects[innerObject.first];
+ GeomIndexList& innerGeoms = geomData1.innerObjects[innerObject.first];
innerGeoms.reserve(innerGeoms.size() + innerObject.second.size());
BOOST_FOREACH(const GeomIndex& gid, innerObject.second)
{
innerGeoms.push_back(nGeom1 + gid);
}
}
- BOOST_FOREACH(const Map_t::value_type& outerObject, geomModel2.outerObjects)
+ BOOST_FOREACH(const Map_t::value_type& outerObject, geomData2.outerObjects)
{
- GeomIndexList& outerGeoms = geomModel1.outerObjects[outerObject.first];
+ GeomIndexList& outerGeoms = geomData1.outerObjects[outerObject.first];
outerGeoms.reserve(outerGeoms.size() + outerObject.second.size());
BOOST_FOREACH(const GeomIndex& gid, outerObject.second)
{
@@ -228,12 +245,9 @@ namespace se3
}
}
- /// 3. add the collision pairs between geomModel1 and geomModel2.
- for (Index i = 0; i < nGeom1; ++i) {
- for (Index j = 0; j < nGeom2; ++j) {
- geomModel1.collisionPairs.push_back(CollisionPair(i, nGeom1 + j));
- }
- }
+ // FIXME: I copy here the previous algo. Maybe it would be safer to simply use the
+ // fillInnerOuterObjectMaps() method (however marginally less efficient: is it important?).
+
}
} // namespace se3
diff --git a/src/multibody/fcl.hpp b/src/multibody/fcl.hpp
index 9862862273b348643d2cb37909bc3d773d526ac3..b564c5319fe7104af5a1343022b13c04e0da1e69 100644
--- a/src/multibody/fcl.hpp
+++ b/src/multibody/fcl.hpp
@@ -66,6 +66,7 @@ namespace se3
friend std::ostream & operator << (std::ostream & os, const CollisionPair & X);
}; // struct CollisionPair
+
typedef std::vector<CollisionPair> CollisionPairsVector_t;
#ifndef WITH_HPP_FCL
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index ecd5816f412c06311066c2617f656cdb63ecdf4b..49dacec98daeab0c3d8bacbbbbfc20e16bb0fe5b 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -37,8 +37,6 @@ namespace se3
struct GeometryModel
{
- typedef std::vector<GeomIndex> GeomIndexList;
-
/// \brief The number of GeometryObjects
Index ngeoms;
@@ -47,22 +45,12 @@ namespace se3
///
/// \brief Vector of collision pairs.
///
- CollisionPairsVector_t collisionPairs;
+ std::vector<CollisionPair> collisionPairs;
- /// \brief A list of associated collision GeometryObjects to a given joint Id.
- /// Inner objects can be seen as geometry objects that directly move when the associated joint moves
- std::map < JointIndex, GeomIndexList > innerObjects;
-
- /// \brief A list of associated collision GeometryObjects to a given joint Id
- /// Outer objects can be seen as geometry objects that may often be obstacles to the Inner objects of given joint
- std::map < JointIndex, GeomIndexList > outerObjects;
-
GeometryModel()
: ngeoms(0)
, geometryObjects()
, collisionPairs()
- , innerObjects()
- , outerObjects()
{
const std::size_t num_max_collision_pairs = (ngeoms * (ngeoms-1))/2;
collisionPairs.reserve(num_max_collision_pairs);
@@ -177,21 +165,6 @@ namespace se3
void displayCollisionPairs() const;
#endif // WITH_HPP_FCL
- /**
- * @brief Associate a GeometryObject of type COLLISION to a joint's inner objects list
- *
- * @param[in] joint Index of the joint
- * @param[in] inner_object Index of the GeometryObject that will be an inner object
- */
- void addInnerObject(const JointIndex joint, const GeomIndex inner_object);
-
- /**
- * @brief Associate a GeometryObject of type COLLISION to a joint's outer objects list
- *
- * @param[in] joint Index of the joint
- * @param[in] inner_object Index of the GeometryObject that will be an outer object
- */
- void addOutterObject(const JointIndex joint, const GeomIndex outer_object);
friend std::ostream& operator<<(std::ostream & os, const GeometryModel & model_geom);
}; // struct GeometryModel
@@ -260,6 +233,20 @@ namespace se3
///
Index collisionPairIndex;
+ typedef std::vector<GeomIndex> GeomIndexList;
+
+ /// \brief Map over vector GeomModel::geometryObjects, indexed by joints.
+ ///
+ /// The map lists the collision GeometryObjects associated to a given joint Id.
+ /// Inner objects can be seen as geometry objects that directly move when the associated joint moves
+ std::map < JointIndex, GeomIndexList > innerObjects;
+
+ /// \brief A list of associated collision GeometryObjects to a given joint Id
+ ///
+ /// Outer objects can be seen as geometry objects that may often be
+ /// obstacles to the Inner objects of given joint
+ std::map < JointIndex, GeomIndexList > outerObjects;
+
GeometryData(const GeometryModel & modelGeom)
: model_geom(modelGeom)
, oMg(model_geom.ngeoms)
@@ -269,12 +256,15 @@ namespace se3
, collisionRequest (1, false, false, 1, false, true, fcl::GST_INDEP)
, collision_results(modelGeom.collisionPairs.size())
, radius()
-
+ , collisionPairIndex(-1)
+ , innerObjects()
+ , outerObjects()
{
collisionObjects.reserve(modelGeom.geometryObjects.size());
BOOST_FOREACH( const GeometryObject & geom, modelGeom.geometryObjects)
{ collisionObjects.push_back
(fcl::CollisionObject(geom.collision_geometry)); }
+ fillInnerOuterObjectMaps();
}
#else
GeometryData(const GeometryModel & modelGeom)
@@ -335,6 +325,14 @@ namespace se3
void computeAllDistances() PINOCCHIO_DEPRECATED;
void resetDistances();
+
+ /// Fill both innerObjects and outerObjects maps, from vectors collisionObjects and
+ /// collisionPairs.
+ ///
+ /// \warning Outer objects are not duplicated (i.e. if a is in outerObjects[b], then
+ /// b is not in outerObjects[a]).
+ void fillInnerOuterObjectMaps();
+
#endif //WITH_HPP_FCL
friend std::ostream & operator<<(std::ostream & os, const GeometryData & data_geom);
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index b42615045842f914acc6c4e57329af8a4907558b..a7380129680f47bf7732dcdb807fa973c9fb88ca 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -36,7 +36,6 @@ namespace se3
{
Index idx = (Index) (ngeoms ++);
geometryObjects.push_back(object);
- addInnerObject(object.parentJoint, idx);
return idx;
}
@@ -82,24 +81,50 @@ namespace se3
}
- inline void GeometryModel::addInnerObject(const JointIndex joint_id, const GeomIndex inner_object)
+ /**
+ * @brief Associate a GeometryObject of type COLLISION to a joint's inner objects list
+ *
+ * @param[in] joint Index of the joint
+ * @param[in] inner_object Index of the GeometryObject that will be an inner object
+ */
+ // inline void GeometryModel::addInnerObject(const JointIndex joint_id, const GeomIndex inner_object)
+ // {
+ // if (std::find(innerObjects[joint_id].begin(),
+ // innerObjects[joint_id].end(),
+ // inner_object) == innerObjects[joint_id].end())
+ // innerObjects[joint_id].push_back(inner_object);
+ // else
+ // std::cout << "inner object already added" << std::endl;
+ // }
+
+ /**
+ * @brief Associate a GeometryObject of type COLLISION to a joint's outer objects list
+ *
+ * @param[in] joint Index of the joint
+ * @param[in] inner_object Index of the GeometryObject that will be an outer object
+ */
+ // inline void GeometryModel::addOutterObject (const JointIndex joint, const GeomIndex outer_object)
+ // {
+ // if (std::find(outerObjects[joint].begin(),
+ // outerObjects[joint].end(),
+ // outer_object) == outerObjects[joint].end())
+ // outerObjects[joint].push_back(outer_object);
+ // else
+ // std::cout << "outer object already added" << std::endl;
+ // }
+
+ inline void GeometryData::fillInnerOuterObjectMaps()
{
- if (std::find(innerObjects[joint_id].begin(),
- innerObjects[joint_id].end(),
- inner_object) == innerObjects[joint_id].end())
- innerObjects[joint_id].push_back(inner_object);
- else
- std::cout << "inner object already added" << std::endl;
- }
+ innerObjects.clear();
+ outerObjects.clear();
- inline void GeometryModel::addOutterObject (const JointIndex joint, const GeomIndex outer_object)
- {
- if (std::find(outerObjects[joint].begin(),
- outerObjects[joint].end(),
- outer_object) == outerObjects[joint].end())
- outerObjects[joint].push_back(outer_object);
- else
- std::cout << "outer object already added" << std::endl;
+ for( GeomIndex gid = 0; gid<model_geom.geometryObjects.size(); gid++)
+ innerObjects[model_geom.geometryObjects[gid].parentJoint].push_back(gid);
+
+ BOOST_FOREACH( const CollisionPair & pair, model_geom.collisionPairs )
+ {
+ outerObjects[model_geom.geometryObjects[pair.first].parentJoint].push_back(pair.second);
+ }
}
inline std::ostream & operator<< (std::ostream & os, const GeometryModel & model_geom)