Add CollisionPair.

CMakeLists.txt

@@ -47,6 +47,7 @@ FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework)
 # Declare Headers
 SET(${PROJECT_NAME}_HEADERS
   include/hpp/core/bi-rrt-planner.hh
+  include/hpp/core/collision-pair.hh
   include/hpp/core/collision-path-validation-report.hh
   include/hpp/core/collision-validation.hh
   include/hpp/core/relative-motion.hh

include/hpp/core/collision-pair.hh

+//
+// Copyright (c) 2021 CNRS
+// Authors: Joseph Mirabel
+//
+// This file is part of hpp-core
+// hpp-core is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-core is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-core  If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_CORE_COLLISION_PAIR_HH
+# define HPP_CORE_COLLISION_PAIR_HH
+
+#include <hpp/fcl/collision.h>
+#include <pinocchio/spatial/fcl-pinocchio-conversions.hpp>
+
+#include <hpp/pinocchio/device-data.hh>
+#include <hpp/pinocchio/collision-object.hh>
+#include <hpp/core/fwd.hh>
+
+namespace hpp {
+namespace core {
+
+struct CollisionPair {
+  CollisionObjectConstPtr_t first;
+  CollisionObjectConstPtr_t second;
+  fcl::ComputeCollision computeCollision;
+
+  inline CollisionPair (CollisionObjectConstPtr_t f, CollisionObjectConstPtr_t s)
+    : first(f), second(s), computeCollision(f->geometry().get(), s->geometry().get())
+  {}
+
+  inline auto collide(fcl::CollisionRequest& request, fcl::CollisionResult& result) const
+    //decltype(computeCollision(tf1,tf2,request,result))
+  {
+    assert(!first ->getTransform(d).translation().hasNaN());
+    assert(!first ->getTransform(d).rotation   ().hasNaN());
+    assert(!second->getTransform(d).translation().hasNaN());
+    assert(!second->getTransform(d).rotation   ().hasNaN());
+    return computeCollision(
+        first ->getFclTransform(),
+        second->getFclTransform(),
+        request,result);
+  }
+
+  inline auto collide(const pinocchio::DeviceData& d,
+      fcl::CollisionRequest& request, fcl::CollisionResult& result) const
+    //decltype(computeCollision(tf1,tf2,request,result))
+  {
+    using ::pinocchio::toFclTransform3f;
+    assert(!first ->getTransform(d).translation().hasNaN());
+    assert(!first ->getTransform(d).rotation   ().hasNaN());
+    assert(!second->getTransform(d).translation().hasNaN());
+    assert(!second->getTransform(d).rotation   ().hasNaN());
+    return computeCollision(
+        toFclTransform3f(first ->getTransform (d)),
+        toFclTransform3f(second->getTransform (d)),
+        request,result);
+  }
+};
+
+} // namespace core
+} // namespace hpp
+#endif // HPP_CORE_COLLISION_PAIR_HH

include/hpp/core/collision-validation-report.hh

@@ -24,6 +24,7 @@
 # include <hpp/pinocchio/collision-object.hh>
 # include <hpp/core/validation-report.hh>
 # include <hpp/fcl/collision_data.h>
+# include <hpp/core/collision-pair.hh>
 
 namespace hpp {
   namespace core {

include/hpp/core/continuous-validation/body-pair-collision.hh

@@ -24,6 +24,7 @@
 
 # include <hpp/fcl/collision_data.h>
 
+# include <hpp/core/collision-pair.hh>
 # include <hpp/core/collision-validation-report.hh>
 # include <hpp/core/continuous-validation/interval-validation.hh>
 
@@ -51,9 +52,6 @@ namespace hpp {
       class BodyPairCollision : public IntervalValidation
       {
       public:
-        typedef std::pair<CollisionObjectConstPtr_t, CollisionObjectConstPtr_t> CollisionPair_t;
-        typedef std::vector<CollisionPair_t> CollisionPairs_t;
-
         /// Validate interval centered on a path parameter
         /// \param t parameter value in the path interval of definition
         /// \param[in,out] interval as input, interval over which

include/hpp/core/fwd.hh

@@ -216,8 +216,8 @@ namespace hpp {
     CenterOfMassComputationMap_t;
 
     // Collision pairs
-    typedef std::pair <CollisionObjectConstPtr_t, CollisionObjectConstPtr_t>
-    CollisionPair_t;
+    struct CollisionPair;
+    typedef CollisionPair CollisionPair_t; // For backward compatibility.
     typedef std::vector <CollisionPair_t> CollisionPairs_t;
 
     class ExtractedPath;

include/hpp/core/obstacle-user.hh

@@ -23,6 +23,7 @@
 # include <hpp/core/config.hh>
 # include <hpp/core/fwd.hh>
 # include <hpp/core/relative-motion.hh>
+# include <hpp/core/collision-pair.hh>
 
 namespace hpp {
   namespace core {
@@ -166,8 +167,6 @@ namespace hpp {
       public:
         virtual ~ObstacleUser () = default;
 
-        typedef std::pair<CollisionObjectConstPtr_t, CollisionObjectConstPtr_t> CollisionPair_t;
-        typedef std::vector<CollisionPair_t> CollisionPairs_t;
         typedef std::vector<fcl::CollisionRequest> CollisionRequests_t;
 
         static bool collide (const CollisionPairs_t& pairs,

src/collision-validation.cc

@@ -50,13 +50,16 @@ namespace hpp {
 
       fcl::CollisionResult collisionResult;
       std::size_t iPair = 0;
-      const ObstacleUser::CollisionPairs_t* pairs (&cPairs_);
+      const CollisionPairs_t* pairs (&cPairs_);
+      ObstacleUser::CollisionRequests_t* requests (&cRequests_);
+
       bool collide = ObstacleUser::collide (cPairs_, cRequests_,
           collisionResult, iPair, device.d());
       if (!collide && checkParameterized_) {
         collide = ObstacleUser::collide (pPairs_, pRequests_,
             collisionResult, iPair, device.d());
         pairs = &pPairs_;
+        requests = &pRequests_;
       }
       if (collide) {
         CollisionValidationReportPtr_t report (
@@ -69,12 +72,10 @@ namespace hpp {
           allReport->collisionReports.push_back(report);
           // then loop over all the remaining pairs :
           ++iPair;
-          for (;iPair < cPairs_.size(); ++iPair) {
+          for (;iPair < pairs->size(); ++iPair) {
             collisionResult.clear();
-            if (fcl::collide (
-                  cPairs_[iPair].first ->fcl (device.d()),
-                  cPairs_[iPair].second->fcl (device.d()),
-                  cRequests_[iPair], collisionResult) != 0) {
+            if ((*pairs)[iPair].collide(device.d(),
+                  (*requests)[iPair], collisionResult) != 0) {
               CollisionValidationReportPtr_t report (
                   new CollisionValidationReport ((*pairs)[iPair], collisionResult));
               allReport->collisionReports.push_back(report);

src/continuous-validation/body-pair-collision.cc

@@ -23,6 +23,7 @@
 
 #include <hpp/fcl/collision_data.h>
 #include <hpp/fcl/collision.h>
+#include <pinocchio/spatial/fcl-pinocchio-conversions.hpp>
 #include <hpp/pinocchio/body.hh>
 #include <hpp/pinocchio/collision-object.hh>
 
@@ -32,6 +33,7 @@
 namespace hpp {
   namespace core {
     namespace continuousValidation {
+      using ::pinocchio::toFclTransform3f;
 
       bool BodyPairCollision::validateConfiguration (const value_type& t, interval_t& interval,
                 ValidationReportPtr_t& report,
@@ -150,15 +152,12 @@ namespace hpp {
         distanceLowerBound = numeric_limits <value_type>::infinity ();
         assert (collisionRequest_.enable_distance_lower_bound == true);
         const CollisionPairs_t& prs (pairs());
-        for (CollisionPairs_t::const_iterator _pair = prs.begin();
-            _pair != prs.end(); ++_pair) {
-          pinocchio::FclConstCollisionObjectPtr_t object_a = _pair->first ->fcl (data);
-          pinocchio::FclConstCollisionObjectPtr_t object_b = _pair->second->fcl (data);
+        for (const auto& pair : prs) {
           fcl::CollisionResult result;
-          fcl::collide (object_a, object_b, collisionRequest_, result);
+          pair.collide(data, collisionRequest_, result);
           // Get result
           if (result.isCollision ()) {
-            setReport(report, result, *_pair);
+            setReport(report, result, pair);
             return false;
           }
           if (result.distance_lower_bound < distanceLowerBound) {

src/distance-between-objects.cc

@@ -25,6 +25,8 @@
 #include <hpp/pinocchio/device.hh>
 #include <hpp/pinocchio/joint.hh>
 
+#include <hpp/core/collision-pair.hh>
+
 namespace hpp {
   namespace core {
     void DistanceBetweenObjects::addObstacle
@@ -36,7 +38,7 @@ namespace hpp {
 	BodyPtr_t body = joint->linkedBody ();
 	if (body) {
           for (size_type j = 0; j < body->nbInnerObjects(); ++j) {
-	    collisionPairs_.push_back (CollisionPair_t (body->innerObjectAt(j), object));
+	    collisionPairs_.emplace_back(body->innerObjectAt(j), object);
 	  }
 	}
       }
@@ -60,7 +62,8 @@ namespace hpp {
     const CollisionObjectConstPtr_t& obj1 = itCol->first;
     const CollisionObjectConstPtr_t& obj2 = itCol->second;
     distanceResults_[rank].clear ();
-    fcl::distance (obj1->fcl (), obj2->fcl (),
+    fcl::distance (obj1->geometry().get(), obj1->getFclTransform(),
+               obj2->geometry().get(), obj2->getFclTransform(),
                distanceRequest, distanceResults_[rank]);
     ++rank;
       }

src/obstacle-user.cc

@@ -38,16 +38,8 @@ namespace hpp {
     {
       for (i = 0; i < pairs.size(); ++i) {
         res.clear();
-        const CollisionObjectConstPtr_t& a (pairs[i].first );
-        const CollisionObjectConstPtr_t& b (pairs[i].second);
-        assert(!a->getTransform(data).translation().hasNaN());
-        assert(!a->getTransform(data).rotation   ().hasNaN());
-        assert(!b->getTransform(data).translation().hasNaN());
-        assert(!b->getTransform(data).rotation   ().hasNaN());
-        if (fcl::collide (
-              a->geometry().get(), toFclTransform3f(a->getTransform (data)),
-              b->geometry().get(), toFclTransform3f(b->getTransform (data)),
-              reqs[i], res) != 0) return true;
+        if (pairs[i].collide(data, reqs[i], res) != 0)
+          return true;
       }
       return false;
     }
@@ -147,8 +139,7 @@ namespace hpp {
               hppDout(info, "Disabling collision between "
                   << pair.first ->name() << " and "
                   << pair.second->name());
-              if (fcl::collide (pair.first ->fcl (), pair.second->fcl (),
-                    cRequests_[i], unused) != 0) {
+              if (pair.collide(cRequests_[i], unused) != 0) {
                 hppDout(warning, "Disabling collision detection between two "
                     "bodies in collision.");
               }

src/path-optimization/spline-gradient-based/collision-constraint.hh

@@ -125,7 +125,9 @@ namespace hpp {
 	 fcl::CollisionResult result;
          fcl::CollisionRequestFlag flag = enableContact? fcl::CONTACT : fcl::NO_REQUEST ;
          fcl::CollisionRequest collisionRequest (flag, 1);
-         fcl::collide (object1_->fcl (), object2_->fcl (), collisionRequest, result);
+         fcl::collide (object1_->geometry().get(), object1_->getFclTransform(),
+             object2_->geometry().get(), object2_->getFclTransform(),
+             collisionRequest, result);
          return result;
        }