Add BVHIntersection

include/hpp/fcl/BVH/BVH_utility.h

@@ -77,6 +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.
+template<typename BV>
+BVHModel<BV>* BVHIntersection(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);
 

src/BVH/BVH_utility.cpp

@@ -103,6 +103,79 @@ 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)
+{
+  assert(model.getModelType() == BVH_MODEL_TRIANGLES);
+  const Quaternion3f& q = pose.getQuatRotation();
+  AABB aabb = translate (_aabb, - pose.getTranslation());
+
+  // Check what triangles should be kept.
+  // TODO use the BV hierarchy
+  std::vector<bool> keep_vertex(model.num_vertices, false);
+  std::vector<bool> keep_tri   (model.num_tris,     false);
+  std::size_t ntri = 0;
+  for (std::size_t i = 0; i < model.num_tris; ++i) {
+    const Triangle& t = model.tri_indices[i];
+
+    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]])) {
+          keep_this_tri = true;
+          break;
+        }
+      }
+    }
+    if (keep_this_tri) {
+      keep_vertex[t[0]] = keep_vertex[t[1]] = keep_vertex[t[2]] = true;
+      keep_tri[i] = true;
+      ntri++;
+    }
+  }
+
+  if (ntri == 0) return NULL;
+
+  BVHModel<BV>* new_model (new BVHModel<BV>());
+  new_model->beginModel(ntri, std::min((int)ntri * 3, model.num_vertices));
+  std::vector<std::size_t> idxConversion (model.num_vertices);
+  assert(new_model->num_vertices == 0);
+  for (std::size_t i = 0; i < keep_vertex.size(); ++i) {
+    if (keep_vertex[i]) {
+      idxConversion[i] = new_model->num_vertices;
+      new_model->vertices[new_model->num_vertices] = model.vertices[i];
+      new_model->num_vertices++;
+    }
+  }
+  assert(new_model->num_tris == 0);
+  for (std::size_t i = 0; i < keep_tri.size(); ++i) {
+    if (keep_tri[i]) {
+      new_model->tri_indices[new_model->num_tris].set (
+          idxConversion[model.tri_indices[i][0]],
+          idxConversion[model.tri_indices[i][1]],
+          idxConversion[model.tri_indices[i][2]]
+          );
+      new_model->num_tris++;
+    }
+  }
+  if (new_model->endModel() != BVH_OK) {
+    delete new_model;
+    return NULL;
+  }
+  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);
 
 void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M)
 {

src/BVH/BV_splitter.cpp

@@ -100,7 +100,7 @@ void computeSplitValue_mean(const BV& bv, Vec3f* vertices, Triangle* triangles,
   FCL_REAL sum = 0.0;
   if(type == BVH_MODEL_TRIANGLES)
   {
-    FCL_REAL c[3] = {0.0, 0.0, 0.0};
+    Vec3f c (Vec3f::Zero());
 
     for(int i = 0; i < num_primitives; ++i)
     {
@@ -109,19 +109,16 @@ void computeSplitValue_mean(const BV& bv, Vec3f* vertices, Triangle* triangles,
       const Vec3f& p2 = vertices[t[1]];
       const Vec3f& p3 = vertices[t[2]];
 
-      c[0] += (p1[0] + p2[0] + p3[0]);
-      c[1] += (p1[1] + p2[1] + p3[1]);
-      c[2] += (p1[2] + p2[2] + p3[2]);
+      c += p1 + p2 + p3;
     }
-    split_value = (c[0] * split_vector[0] + c[1] * split_vector[1] + c[2] * split_vector[2]) / (3 * num_primitives);
+    split_value = c.dot(split_vector) / (3 * num_primitives);
   }
   else if(type == BVH_MODEL_POINTCLOUD)
   {
     for(int i = 0; i < num_primitives; ++i)
     {
       const Vec3f& p = vertices[primitive_indices[i]];
-      Vec3f v(p[0], p[1], p[2]);
-      sum += v.dot(split_vector);
+      sum += p.dot(split_vector);
     }
 
     split_value = sum / num_primitives;

test/test_fcl_bvh_models.cpp

@@ -41,6 +41,7 @@
 #include <boost/utility/binary.hpp>
 
 #include "hpp/fcl/BVH/BVH_model.h"
+#include "hpp/fcl/BVH/BVH_utility.h"
 #include "hpp/fcl/math/transform.h"
 #include "hpp/fcl/shape/geometric_shapes.h"
 #include "test_fcl_utility.h"
@@ -103,7 +104,8 @@ template<typename BV>
 void testBVHModelTriangles()
 {
   boost::shared_ptr<BVHModel<BV> > model(new BVHModel<BV>);
-  Box box;
+  Box box(1,1,1);
+  AABB aabb (Vec3f(-1,0,-1), Vec3f(1,1,1));
 
   double a = box.side[0];
   double b = box.side[1];
@@ -151,6 +153,31 @@ void testBVHModelTriangles()
   BOOST_CHECK_EQUAL(model->num_vertices, 12 * 3);
   BOOST_CHECK_EQUAL(model->num_tris, 12);
   BOOST_CHECK_EQUAL(model->build_state, BVH_BUILD_STATE_PROCESSED);
+
+  Transform3f pose;
+  boost::shared_ptr<BVHModel<BV> > cropped(BVHIntersection(*model, pose, aabb));
+  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));
+  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,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));
+  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));
+  BOOST_CHECK(!cropped);
 }
 
 template<typename BV>