From be1c5699ae5113f6e358b2f4af13503440243bd8 Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Tue, 6 Sep 2016 17:20:13 +0200
Subject: [PATCH] [C++] Geom: Removed DistanceResult from GeomData. Issue
281.1.
---
src/algorithm/geometry.hxx | 6 ++---
src/multibody/fcl.hpp | 52 +-------------------------------------
src/multibody/fcl.hxx | 16 ------------
src/multibody/geometry.hpp | 22 +++++++++++-----
src/multibody/geometry.hxx | 28 ++++++++++----------
5 files changed, 35 insertions(+), 89 deletions(-)
diff --git a/src/algorithm/geometry.hxx b/src/algorithm/geometry.hxx
index 0edbadbc0..83853400d 100644
--- a/src/algorithm/geometry.hxx
+++ b/src/algorithm/geometry.hxx
@@ -102,11 +102,11 @@ namespace se3
{
if(data_geom.activeCollisionPairs[cpt])
{
- data_geom.distance_results[cpt] = data_geom.computeDistance(geomModel.collisionPairs[cpt]);
- if (COMPUTE_SHORTEST && data_geom.distance_results[cpt].distance() < min_dist)
+ data_geom.computeDistance(cpt);
+ if (COMPUTE_SHORTEST && data_geom.distance_results[cpt].min_distance < min_dist)
{
min_index = cpt;
- min_dist = data_geom.distance_results[cpt].distance();
+ min_dist = data_geom.distance_results[cpt].min_distance;
}
}
}
diff --git a/src/multibody/fcl.hpp b/src/multibody/fcl.hpp
index a211492d6..986286227 100644
--- a/src/multibody/fcl.hpp
+++ b/src/multibody/fcl.hpp
@@ -68,55 +68,7 @@ namespace se3
}; // struct CollisionPair
typedef std::vector<CollisionPair> CollisionPairsVector_t;
-#ifdef WITH_HPP_FCL
- /**
- * @brief Result of distance computation between two CollisionObjects
- */
- struct DistanceResult
- {
-
- DistanceResult() : fcl_distance_result(), object1(0), object2(0) {}
- DistanceResult(fcl::DistanceResult dist_fcl, const GeomIndex co1, const GeomIndex co2)
- : fcl_distance_result(dist_fcl), object1(co1), object2(co2)
- {}
-
-
- ///
- /// @brief Return the minimal distance between two geometry objects
- ///
- double distance () const;
-
- ///
- /// \brief Return the witness point on the inner object expressed in global frame.
- ///
- Eigen::Vector3d closestPointInner () const;
-
- ///
- /// \brief Return the witness point on the outer object expressed in global frame.
- ///
- Eigen::Vector3d closestPointOuter () const;
-
- bool operator == (const DistanceResult & other) const
- {
- return (distance() == other.distance()
- && closestPointInner() == other.closestPointInner()
- && closestPointOuter() == other.closestPointOuter()
- && object1 == other.object1
- && object2 == other.object2);
- }
-
- /// \brief The FCL result of the distance computation
- fcl::DistanceResult fcl_distance_result;
-
- /// \brief Index of the first colision object
- GeomIndex object1;
-
- /// \brief Index of the second colision object
- GeomIndex object2;
-
- }; // struct DistanceResult
-
-#else
+#ifndef WITH_HPP_FCL
namespace fcl
{
@@ -196,8 +148,6 @@ struct GeometryObject
};
-
-
} // namespace se3
/* --- Details -------------------------------------------------------------- */
diff --git a/src/multibody/fcl.hxx b/src/multibody/fcl.hxx
index ba078f3e1..7451bd073 100644
--- a/src/multibody/fcl.hxx
+++ b/src/multibody/fcl.hxx
@@ -33,22 +33,6 @@ namespace se3
#ifdef WITH_HPP_FCL
-
- inline double DistanceResult::distance () const
- {
- return fcl_distance_result.min_distance;
- }
-
- inline Eigen::Vector3d DistanceResult::closestPointInner () const
- {
- return toVector3d(fcl_distance_result.nearest_points [0]);
- }
-
- inline Eigen::Vector3d DistanceResult::closestPointOuter () const
- {
- return toVector3d(fcl_distance_result.nearest_points [1]);
- }
-
inline bool operator == (const fcl::CollisionObject & lhs, const fcl::CollisionObject & rhs)
{
return lhs.collisionGeometry() == rhs.collisionGeometry()
diff --git a/src/multibody/geometry.hpp b/src/multibody/geometry.hpp
index 045fba2a9..ecd5816f4 100644
--- a/src/multibody/geometry.hpp
+++ b/src/multibody/geometry.hpp
@@ -226,10 +226,15 @@ namespace se3
///
std::vector<bool> activeCollisionPairs;
+ ///
+ /// \brief Defines what information should be computed by distance computation.
+ ///
+ fcl::DistanceRequest distanceRequest;
+
///
/// \brief Vector gathering the result of the distance computation for all the collision pairs.
///
- std::vector <DistanceResult> distance_results;
+ std::vector <fcl::DistanceResult> distance_results;
///
/// \brief Defines what information should be computed by collision test.
@@ -259,6 +264,7 @@ namespace se3
: model_geom(modelGeom)
, oMg(model_geom.ngeoms)
, activeCollisionPairs(modelGeom.collisionPairs.size(), true)
+ , distanceRequest (true, 0, 0, fcl::GST_INDEP)
, distance_results(modelGeom.collisionPairs.size())
, collisionRequest (1, false, false, 1, false, true, fcl::GST_INDEP)
, collision_results(modelGeom.collisionPairs.size())
@@ -311,18 +317,22 @@ namespace se3
///
/// \brief Compute the minimal distance between collision objects of a collison pair
///
- /// \param[in] pair The collsion pair.
+ /// \param[in] pairId The index of the collision pair in geom model.
///
- /// \return An fcl struct containing the distance result.
+ /// \return A reference on fcl struct containing the distance result, referring an element
+ /// of vector geomData::distance_results.
///
- DistanceResult computeDistance(const CollisionPair & pair) const;
+ fcl::DistanceResult & computeDistance(const Index & pairId);
///
/// \brief Compute the distance result for all collision pairs according to
/// the current placements of the geometries stored in GeometryData::oMg.
- /// The results are stored in the vector GeometryData::distance_results.
///
- void computeAllDistances();
+ /// The method indeed calls computeDistance for each collision
+ /// pair. Consequently the results are stored in the vector
+ /// GeometryData::distance_results.
+ ///
+ void computeAllDistances() PINOCCHIO_DEPRECATED;
void resetDistances();
#endif //WITH_HPP_FCL
diff --git a/src/multibody/geometry.hxx b/src/multibody/geometry.hxx
index 7048c9439..b42615045 100644
--- a/src/multibody/geometry.hxx
+++ b/src/multibody/geometry.hxx
@@ -179,7 +179,7 @@ namespace se3
collisionPairs.end(),
pair);
- return (Index) distance(collisionPairs.begin(), it);
+ return (Index) std::distance(collisionPairs.begin(), it);
}
inline void GeometryModel::displayCollisionPairs() const
@@ -242,31 +242,33 @@ namespace se3
return false;
}
- inline DistanceResult GeometryData::computeDistance(const CollisionPair & pair) const
+ inline fcl::DistanceResult & GeometryData::computeDistance(const Index & pairId )
{
- const Index & co1 = pair.first; assert(co1<collisionObjects.size());
- const Index & co2 = pair.second; assert(co2<collisionObjects.size());
-
- fcl::DistanceRequest distanceRequest (true, 0, 0, fcl::GST_INDEP);
- fcl::DistanceResult result;
- fcl::distance ( &collisionObjects[co1],&collisionObjects[co2],
- distanceRequest, result);
+ assert( pairId < model_geom.collisionPairs.size() );
+ const CollisionPair & pair = model_geom.collisionPairs[pairId];
+
+ assert( pair < distance_results.size() );
+ assert( pair.first < collisionObjects.size() );
+ assert( pair.second < collisionObjects.size() );
- return DistanceResult (result, co1, co2);
+ fcl::distance ( &collisionObjects[pair.first],&collisionObjects[pair.second],
+ distanceRequest, distance_results[pairId]);
+
+ return distance_results[pairId];
}
inline void GeometryData::computeAllDistances ()
{
+ assert( activeCollisionPairs.size() == model_geom.collisionPairs.size() );
for(size_t i = 0; i<activeCollisionPairs.size(); ++i)
{
- if (activeCollisionPairs[i])
- distance_results[i] = computeDistance(model_geom.collisionPairs[i]);
+ if (activeCollisionPairs[i]) computeDistance(i);
}
}
inline void GeometryData::resetDistances()
{
- std::fill(distance_results.begin(), distance_results.end(), DistanceResult( fcl::DistanceResult(), 0, 0) );
+ std::fill(distance_results.begin(), distance_results.end(), fcl::DistanceResult() );
}
#endif //WITH_HPP_FCL
} // namespace se3
--
GitLab