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
......@@ -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
......
......@@ -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);
......
// 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
......@@ -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)
{
......
......@@ -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
......
// 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");
}
}
}
......@@ -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>
......
......@@ -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";
......
Supports Markdown
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