diff --git a/CMakeLists.txt b/CMakeLists.txt
index 855a860ed41eaf624e804f3fccf35cdda4a9e997..b7bb0971df4e36227b85e84ca4094dcc36af620e 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/include/hpp/fcl/BVH/BVH_utility.h b/include/hpp/fcl/BVH/BVH_utility.h
index 8cff3a9150f52de22c3e92cb14b318f8961f15ce..94d77d9da6e6dda5a217255871122bff5fd31081 100644
--- a/include/hpp/fcl/BVH/BVH_utility.h
+++ b/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);
diff --git a/include/hpp/fcl/collision_utility.h b/include/hpp/fcl/collision_utility.h
new file mode 100644
index 0000000000000000000000000000000000000000..d9a701718811792265c619ed6cfbe45da95bc6ac
--- /dev/null
+++ b/include/hpp/fcl/collision_utility.h
@@ -0,0 +1,27 @@
+// 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
diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp
index 960bac2b9dfcd8501cf97e70ff7fe0b6da98f1e6..48ae3405b22dea8c0b0cc1331e8e4bfc31ee5635 100644
--- a/src/BVH/BVH_utility.cpp
+++ b/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)
{
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b785cc9436760d067bf6f0f0087cd68b3e1af4f9..15b893f9dfe1c570a4895f9cf12722377867fcac 100644
--- a/src/CMakeLists.txt
+++ b/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
diff --git a/src/collision_utility.cpp b/src/collision_utility.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..45f92f1ad69a76cc3ee92c476b1fd59a5ee3ad5b
--- /dev/null
+++ b/src/collision_utility.cpp
@@ -0,0 +1,54 @@
+// 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");
+ }
+ }
+}
diff --git a/test/test_fcl_bvh_models.cpp b/test/test_fcl_bvh_models.cpp
index 7a85cc995e85cf5b3eca2e51ce8cb6ad79274595..05c121cfcf8ad008b2e78c8dd660db2259a5045f 100644
--- a/test/test_fcl_bvh_models.cpp
+++ b/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>
diff --git a/test/test_fcl_profiling.cpp b/test/test_fcl_profiling.cpp
index 69be4b139ba5eac072d2fb3bada6b90a1b4b0c6c..b02f679f46719bff5a3b2b91a916636415b81248 100644
--- a/test/test_fcl_profiling.cpp
+++ b/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";