Skip to content

GitLab

Commit 47fb8b34 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Rename BVHIntersection into BVHExtract and add method generic extract

parent f3a8710f
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 47fb8b34
......@@ -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
View file @ 47fb8b34
......@@ -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 0 → 100644
View file @ 47fb8b34
// 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
View file @ 47fb8b34
......@@ -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
View file @ 47fb8b34
......@@ -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 0 → 100644
View file @ 47fb8b34
// 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
View file @ 47fb8b34
......@@ -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
View file @ 47fb8b34
......@@ -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";
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment