Rename BVHIntersection into BVHExtract and add method generic extract

CMakeLists.txt

@@ -137,6 +137,7 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/BVH/BVH_utility.h
   include/hpp/fcl/intersect.h
   include/hpp/fcl/collision_object.h
+  include/hpp/fcl/collision_utility.h
   include/hpp/fcl/octree.h
   include/hpp/fcl/fwd.hh
   include/hpp/fcl/mesh_loader/assimp.h

include/hpp/fcl/BVH/BVH_utility.h

@@ -77,9 +77,10 @@ void BVHExpand(BVHModel<OBB>& model, const Variance3f* ucs, FCL_REAL r);
 /// @brief Expand the BVH bounding boxes according to the corresponding variance information, for RSS
 void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r);
 
-/// @brief Return the intersection of a BVHModel and a AABB.
+/// @brief Extract the part of the BVHModel that is inside an AABB.
+/// A triangle in collision with the AABB is considered inside.
 template<typename BV>
-BVHModel<BV>* BVHIntersection(const BVHModel<BV>& model, const Transform3f& pose, const AABB& aabb);
+BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose, const AABB& aabb);
 
 /// @brief Compute the covariance matrix for a set or subset of points. if ts = null, then indices refer to points directly; otherwise refer to triangles
 void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M);

include/hpp/fcl/collision_utility.h

+// Copyright (c) 2017, Joseph Mirabel
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-fcl.
+// hpp-fcl 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-fcl 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-fcl. If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef FCL_COLLISION_UTILITY_H
+#define FCL_COLLISION_UTILITY_H
+
+#include <hpp/fcl/collision_object.h>
+
+namespace fcl
+{
+  CollisionGeometry* extract(const CollisionGeometry* model, const Transform3f& pose, const AABB& aabb);
+}
+
+#endif // FCL_COLLISION_UTILITY_H

src/BVH/BVH_utility.cpp

@@ -37,6 +37,8 @@
 
 
 #include <hpp/fcl/BVH/BVH_utility.h>
+#include <hpp/fcl/narrowphase/narrowphase.h>
+#include <hpp/fcl/shape/geometric_shapes_utility.h>
 
 namespace fcl
 {
@@ -104,12 +106,18 @@ void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r = 1.0)
 }
 
 template<typename BV>
-BVHModel<BV>* BVHIntersection(const BVHModel<BV>& model, const Transform3f& pose, const AABB& _aabb)
+BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose, const AABB& _aabb)
 {
   assert(model.getModelType() == BVH_MODEL_TRIANGLES);
   const Quaternion3f& q = pose.getQuatRotation();
   AABB aabb = translate (_aabb, - pose.getTranslation());
 
+  Transform3f box_pose; Box box;
+  constructBox(_aabb, box, box_pose);
+  box_pose = pose.inverseTimes (box_pose);
+
+  GJKSolver_indep gjk;
+
   // Check what triangles should be kept.
   // TODO use the BV hierarchy
   std::vector<bool> keep_vertex(model.num_vertices, false);
@@ -120,9 +128,6 @@ BVHModel<BV>* BVHIntersection(const BVHModel<BV>& model, const Transform3f& pose
 
     bool keep_this_tri = keep_vertex[t[0]] || keep_vertex[t[1]] || keep_vertex[t[2]];
 
-    // const Vec3f& p0 = vertices[t[0]];
-    // const Vec3f& p1 = vertices[t[1]];
-    // const Vec3f& p2 = vertices[t[2]];
     if (!keep_this_tri) {
       for (std::size_t j = 0; j < 3; ++j) {
         if (aabb.contain(q * model.vertices[t[j]])) {
@@ -130,6 +135,12 @@ BVHModel<BV>* BVHIntersection(const BVHModel<BV>& model, const Transform3f& pose
           break;
         }
       }
+      const Vec3f& p0 = model.vertices[t[0]];
+      const Vec3f& p1 = model.vertices[t[1]];
+      const Vec3f& p2 = model.vertices[t[2]];
+      if (!keep_this_tri && gjk.shapeTriangleIntersect(box, box_pose, p0, p1, p2, NULL, NULL, NULL)) {
+        keep_this_tri = true;
+      }
     }
     if (keep_this_tri) {
       keep_vertex[t[0]] = keep_vertex[t[1]] = keep_vertex[t[2]] = true;
@@ -168,14 +179,14 @@ BVHModel<BV>* BVHIntersection(const BVHModel<BV>& model, const Transform3f& pose
   }
   return new_model;
 }
-template BVHModel<OBB      >* BVHIntersection(const BVHModel<OBB      >& model, const Transform3f& pose, const AABB& aabb);
-template BVHModel<AABB     >* BVHIntersection(const BVHModel<AABB     >& model, const Transform3f& pose, const AABB& aabb);
-template BVHModel<RSS      >* BVHIntersection(const BVHModel<RSS      >& model, const Transform3f& pose, const AABB& aabb);
-template BVHModel<kIOS     >* BVHIntersection(const BVHModel<kIOS     >& model, const Transform3f& pose, const AABB& aabb);
-template BVHModel<OBBRSS   >* BVHIntersection(const BVHModel<OBBRSS   >& model, const Transform3f& pose, const AABB& aabb);
-template BVHModel<KDOP<16> >* BVHIntersection(const BVHModel<KDOP<16> >& model, const Transform3f& pose, const AABB& aabb);
-template BVHModel<KDOP<18> >* BVHIntersection(const BVHModel<KDOP<18> >& model, const Transform3f& pose, const AABB& aabb);
-template BVHModel<KDOP<24> >* BVHIntersection(const BVHModel<KDOP<24> >& model, const Transform3f& pose, const AABB& aabb);
+template BVHModel<OBB      >* BVHExtract(const BVHModel<OBB      >& model, const Transform3f& pose, const AABB& aabb);
+template BVHModel<AABB     >* BVHExtract(const BVHModel<AABB     >& model, const Transform3f& pose, const AABB& aabb);
+template BVHModel<RSS      >* BVHExtract(const BVHModel<RSS      >& model, const Transform3f& pose, const AABB& aabb);
+template BVHModel<kIOS     >* BVHExtract(const BVHModel<kIOS     >& model, const Transform3f& pose, const AABB& aabb);
+template BVHModel<OBBRSS   >* BVHExtract(const BVHModel<OBBRSS   >& model, const Transform3f& pose, const AABB& aabb);
+template BVHModel<KDOP<16> >* BVHExtract(const BVHModel<KDOP<16> >& model, const Transform3f& pose, const AABB& aabb);
+template BVHModel<KDOP<18> >* BVHExtract(const BVHModel<KDOP<18> >& model, const Transform3f& pose, const AABB& aabb);
+template BVHModel<KDOP<24> >* BVHExtract(const BVHModel<KDOP<24> >& model, const Transform3f& pose, const AABB& aabb);
 
 void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M)
 {

src/CMakeLists.txt

@@ -71,6 +71,7 @@ set(${LIBRARY_NAME}_SOURCES
   BVH/BVH_model.cpp
   BVH/BV_splitter.cpp
   collision_func_matrix.cpp
+  collision_utility.cpp
   )
 
 # Declare boost include directories

src/collision_utility.cpp

+// Copyright (c) 2017, Joseph Mirabel
+// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
+//
+// This file is part of hpp-fcl.
+// hpp-fcl 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-fcl 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-fcl. If not, see <http://www.gnu.org/licenses/>.
+
+#include <hpp/fcl/collision_utility.h>
+
+#include <hpp/fcl/BVH/BVH_utility.h>
+
+namespace fcl
+{
+  namespace details {
+    template<typename NT>
+    inline CollisionGeometry* extractBVHtpl (const CollisionGeometry* model, const Transform3f& pose, const AABB& aabb)
+    {
+      const BVHModel<NT>* m = static_cast<const BVHModel<NT>*>(model);
+      return BVHExtract(*m, pose, aabb);
+    }
+    CollisionGeometry* extractBVH (const CollisionGeometry* model, const Transform3f& pose, const AABB& aabb)
+    {
+      switch (model->getNodeType()) {
+        case BV_AABB  : return extractBVHtpl<AABB     >(model, pose, aabb);
+        case BV_OBB   : return extractBVHtpl<OBB      >(model, pose, aabb);
+        case BV_RSS   : return extractBVHtpl<RSS      >(model, pose, aabb);
+        case BV_kIOS  : return extractBVHtpl<kIOS     >(model, pose, aabb);
+        case BV_OBBRSS: return extractBVHtpl<OBBRSS   >(model, pose, aabb);
+        case BV_KDOP16: return extractBVHtpl<KDOP<16> >(model, pose, aabb);
+        case BV_KDOP18: return extractBVHtpl<KDOP<18> >(model, pose, aabb);
+        case BV_KDOP24: return extractBVHtpl<KDOP<24> >(model, pose, aabb);
+        default: throw std::runtime_error("Unknown type of bounding volume");
+      }
+    }
+  }
+
+  CollisionGeometry* extract(const CollisionGeometry* model, const Transform3f& pose, const AABB& aabb)
+  {
+    switch (model->getObjectType()) {
+      case OT_BVH: return details::extractBVH (model, pose, aabb);
+      // case OT_GEOM: return model;
+      default: throw std::runtime_error("Extraction is not implemented for this type of object");
+    }
+  }
+}

test/test_fcl_bvh_models.cpp

@@ -155,13 +155,15 @@ void testBVHModelTriangles()
   BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
 
   Transform3f pose;
-  boost::shared_ptr<BVHModel<BV> > cropped(BVHIntersection(*model, pose, aabb));
+  boost::shared_ptr<BVHModel<BV> > cropped(BVHExtract(*model, pose, aabb));
+  BOOST_REQUIRE(cropped);
   BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED);
   BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
   BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
 
   pose.setTranslation(Vec3f(0,1,0));
-  cropped.reset(BVHIntersection(*model, pose, aabb));
+  cropped.reset(BVHExtract(*model, pose, aabb));
+  BOOST_REQUIRE(cropped);
   BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED);
   BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
   BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
@@ -169,15 +171,23 @@ void testBVHModelTriangles()
   pose.setTranslation(Vec3f(0,0,0));
   FCL_REAL sqrt2_2 = std::sqrt(2)/2;
   pose.setQuatRotation(Quaternion3f(sqrt2_2,sqrt2_2,0,0));
-  cropped.reset(BVHIntersection(*model, pose, aabb));
+  cropped.reset(BVHExtract(*model, pose, aabb));
+  BOOST_REQUIRE(cropped);
   BOOST_CHECK(cropped->build_state == BVH_BUILD_STATE_PROCESSED);
   BOOST_CHECK_EQUAL(cropped->num_vertices, model->num_vertices - 6);
   BOOST_CHECK_EQUAL(cropped->num_tris, model->num_tris - 2);
 
   pose.setTranslation(-Vec3f(1,1,1));
   pose.setQuatRotation(Quaternion3f::Identity());
-  cropped.reset(BVHIntersection(*model, pose, aabb));
+  cropped.reset(BVHExtract(*model, pose, aabb));
   BOOST_CHECK(!cropped);
+
+  aabb = AABB(Vec3f(-0.1,-0.1,-0.1), Vec3f(0.1,0.1,0.1));
+  pose.setTranslation(Vec3f(-0.5,-0.5,0));
+  cropped.reset(BVHExtract(*model, pose, aabb));
+  BOOST_REQUIRE(cropped);
+  BOOST_CHECK_EQUAL(cropped->num_tris, 2);
+  BOOST_CHECK_EQUAL(cropped->num_vertices, 6);
 }
 
 template<typename BV>

test/test_fcl_profiling.cpp

@@ -21,6 +21,7 @@
 #include <hpp/fcl/fwd.hh>
 #include <hpp/fcl/collision.h>
 #include <hpp/fcl/BVH/BVH_model.h>
+#include <hpp/fcl/collision_utility.h>
 #include <hpp/fcl/shape/geometric_shapes.h>
 #include <hpp/fcl/collision_func_matrix.h>
 #include <hpp/fcl/narrowphase/narrowphase.h>
@@ -121,7 +122,9 @@ void printResults (const Geometry& g1, const Geometry& g2, const Results& rs)
 
 int Ntransform = 100;
 FCL_REAL limit = 20;
+bool verbose = false;
 
+#define OUT(x) if (verbose) std::cout << x << std::endl
 #define CHECK_PARAM_NB(NB, NAME) \
     if (iarg + NB >= argc) throw std::invalid_argument(#NAME " requires " #NB " numbers")
 void handleParam (int& iarg, const int& argc, char** argv, CollisionRequest& request)
@@ -131,6 +134,7 @@ void handleParam (int& iarg, const int& argc, char** argv, CollisionRequest& req
     if (a == "-nb_transform") {
       CHECK_PARAM_NB(1, nb_transform);
       Ntransform = atoi(argv[iarg+1]);
+      OUT("nb_transform = " << Ntransform);
       iarg += 2;
     } else if (a == "-enable_distance_lower_bound") {
       CHECK_PARAM_NB(1, enable_distance_lower_bound);
@@ -140,6 +144,9 @@ void handleParam (int& iarg, const int& argc, char** argv, CollisionRequest& req
       CHECK_PARAM_NB(1, limit);
       limit = atof(argv[iarg+1]);
       iarg += 2;
+    } else if (a == "-verbose") {
+      verbose = true;
+      iarg += 1;
     } else {
       break;
     }
@@ -169,17 +176,38 @@ Geometry makeGeomFromParam(int& iarg, const int& argc, char** argv)
     type = "sphere";
   } else if (a == "-mesh") {
     CHECK_PARAM_NB(2, Mesh);
+    OUT("Loading " << argv[iarg+2] << " as BVHModel<" << argv[iarg+1] << ">...");
     if (strcmp(argv[iarg+1], "obb") == 0) {
       o = meshToGeom<OBB>(argv[iarg+2]);
+      OUT("Mesh has " << boost::dynamic_pointer_cast<BVHModel<OBB> >(o)->num_tris << " triangles");
       type = "mesh_obb";
     }
     else if (strcmp(argv[iarg+1], "obbrss") == 0) {
       o = meshToGeom<OBBRSS>(argv[iarg+2]);
+      OUT("Mesh has " << boost::dynamic_pointer_cast<BVHModel<OBBRSS> >(o)->num_tris << " triangles");
       type = "mesh_obbrss";
     }
     else
       throw std::invalid_argument ("BV type must be obb or obbrss");
+    OUT("done.");
     iarg += 3;
+    if (iarg < argc && strcmp(argv[iarg], "crop") == 0) {
+      CHECK_PARAM_NB(6, Crop);
+      fcl::AABB aabb(
+          Vec3f(atof(argv[iarg+1]),
+                atof(argv[iarg+2]),
+                atof(argv[iarg+3])),
+          Vec3f(atof(argv[iarg+4]),
+                atof(argv[iarg+5]),
+                atof(argv[iarg+6])));
+      OUT("Cropping " << aabb.min_.transpose() << " ---- " << aabb.max_.transpose() << " ...");
+      o->computeLocalAABB();
+      OUT("Mesh AABB is " << o->aabb_local.min_.transpose() << " ---- " << o->aabb_local.max_.transpose() << " ...");
+      o.reset(extract(o.get(), Transform3f(), aabb));
+      if (!o) throw std::invalid_argument ("Failed to crop.");
+      OUT("Crop has " << boost::dynamic_pointer_cast<BVHModel<OBB> >(o)->num_tris << " triangles");
+      iarg += 7;
+    }
   } else if (a == "-capsule") {
     CREATE_SHAPE_2(o, Capsule);
     type = "capsule";