Verified Commit 083ce965 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

core: use functor for distance and collision request

parent 9f5c2229
......@@ -59,7 +59,7 @@ namespace pinocchio
PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.first < geom_model.ngeoms );
PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.second < geom_model.ngeoms );
fcl::CollisionResult& collisionResult = geom_data.collisionResults[pair_id];
fcl::CollisionResult & collisionResult = geom_data.collisionResults[pair_id];
collisionResult.clear();
fcl::Transform3f oM1 (toFclTransform3f(geom_data.oMg[pair.first ])),
......@@ -67,10 +67,10 @@ namespace pinocchio
try
{
fcl::collide(geom_model.geometryObjects[pair.first ].geometry.get(), oM1,
geom_model.geometryObjects[pair.second].geometry.get(), oM2,
geom_data.collisionRequests[pair_id],
collisionResult);
const GeometryData::ComputeCollision & do_computations = geom_data.collision_functors[pair_id];
do_computations(oM1, oM2,
geom_data.collisionRequests[pair_id],
collisionResult);
}
catch(std::invalid_argument & e)
{
......@@ -149,10 +149,10 @@ namespace pinocchio
try
{
fcl::distance( geom_model.geometryObjects[pair.first ].geometry.get(), oM1,
geom_model.geometryObjects[pair.second].geometry.get(), oM2,
geom_data.distanceRequests[pair_id],
geom_data.distanceResults[pair_id]);
const GeometryData::ComputeDistance & do_computations = geom_data.distance_functors[pair_id];
do_computations(oM1, oM2,
geom_data.distanceRequests[pair_id],
geom_data.distanceResults[pair_id]);
}
catch(std::invalid_argument & e)
{
......
......@@ -188,6 +188,9 @@ namespace pinocchio
typedef Eigen::Matrix<bool,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXb;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
typedef ::hpp::fcl::ComputeCollision ComputeCollision;
typedef ::hpp::fcl::ComputeDistance ComputeDistance;
///
/// \brief Vector gathering the SE3 placements of the geometry objects relative to the world.
/// See updateGeometryPlacements to update the placements.
......@@ -237,7 +240,14 @@ namespace pinocchio
/// the algo computeCollisions() sets it to the first colliding pair.
///
PairIndex collisionPairIndex;
#endif // PINOCCHIO_WITH_HPP_FCL
/// \brief Functoqr associated to the computation of collisions.
PINOCCHIO_ALIGNED_STD_VECTOR(ComputeCollision) collision_functors;
/// \brief Functoqr associated to the computation of distances.
PINOCCHIO_ALIGNED_STD_VECTOR(ComputeDistance) distance_functors;
#endif // PINOCCHIO_WITH_HPP_FCL
/// \brief Map over vector GeomModel::geometryObjects, indexed by joints.
///
......
......@@ -47,6 +47,19 @@ namespace pinocchio
dreq.enable_cached_gjk_guess = true;
}
#endif
collision_functors.reserve(geom_model.collisionPairs.size());
distance_functors.reserve(geom_model.collisionPairs.size());
for(size_t cp_index = 0;
cp_index < geom_model.collisionPairs.size(); ++cp_index)
{
const CollisionPair & cp = geom_model.collisionPairs[cp_index];
const GeometryObject & obj_1 = geom_model.geometryObjects[cp.first];
const GeometryObject & obj_2 = geom_model.geometryObjects[cp.second];
collision_functors.push_back(ComputeCollision(obj_1.geometry.get(),obj_2.geometry.get()));
distance_functors.push_back(ComputeDistance(obj_1.geometry.get(),obj_2.geometry.get()));
}
#endif
fillInnerOuterObjectMaps(geom_model);
}
......@@ -61,6 +74,8 @@ namespace pinocchio
, collisionResults (other.collisionResults)
, radius (other.radius)
, collisionPairIndex (other.collisionPairIndex)
, collision_functors (other.collision_functors)
, distance_functors (other.distance_functors)
#endif // PINOCCHIO_WITH_HPP_FCL
, innerObjects (other.innerObjects)
, outerObjects (other.outerObjects)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment