Commit f3a8710f authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add BVHIntersection

parent 4548c2ed
......@@ -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);
......
......@@ -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)
{
......
......@@ -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;
......
......@@ -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>
......
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