diff --git a/include/hpp/fcl/BVH/BVH_utility.h b/include/hpp/fcl/BVH/BVH_utility.h
index dc9796e42d6cbe2269774f91e31edb8731e7bb71..8cff3a9150f52de22c3e92cb14b318f8961f15ce 100644
--- a/include/hpp/fcl/BVH/BVH_utility.h
+++ b/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);
diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp
index 0651f13c5f7552563e08d5d298f1cde3d536727b..960bac2b9dfcd8501cf97e70ff7fe0b6da98f1e6 100644
--- a/src/BVH/BVH_utility.cpp
+++ b/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)
{
diff --git a/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp
index 1cce58f1ac10896ffa10858ba8e79568db920106..4932c2281c8bd70e4ec8ba60f7739864332220f6 100644
--- a/src/BVH/BV_splitter.cpp
+++ b/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;
diff --git a/test/test_fcl_bvh_models.cpp b/test/test_fcl_bvh_models.cpp
index 7ad321efa7dc5eef63b145a409a80ebe2d9612e4..7a85cc995e85cf5b3eca2e51ce8cb6ad79274595 100644
--- a/test/test_fcl_bvh_models.cpp
+++ b/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>