Skip to content

Commit

Permalink
Merge pull request #450 from lmontaut/topic/hppfcl3x/mesh-double-desc…
Browse files Browse the repository at this point in the history 
…ription

Adding mesh double description
  • Loading branch information
@jcarpent
jcarpent authored Sep 22, 2023
2 parents 9bc6a72 + 3ea3001 commit 3766c24
Show file tree
Hide file tree
Showing 21 changed files with 985 additions and 630 deletions.
94 changes: 69 additions & 25 deletions include/hpp/fcl/BVH/BVH_model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,13 +64,13 @@ class BVSplitter;
class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry {
public:
/// @brief Geometry point data
std::shared_ptr<Vec3f> vertices;
std::shared_ptr<std::vector<Vec3f>> vertices;

/// @brief Geometry triangle index data, will be NULL for point clouds
std::shared_ptr<Triangle> tri_indices;
std::shared_ptr<std::vector<Triangle>> tri_indices;

/// @brief Geometry point data in previous frame
std::shared_ptr<Vec3f> prev_vertices;
std::shared_ptr<std::vector<Vec3f>> prev_vertices;

/// @brief Number of triangles
unsigned int num_tris;
Expand Down Expand Up @@ -200,8 +200,21 @@ class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry {
Vec3f computeCOM() const {
FCL_REAL vol = 0;
Vec3f com(0, 0, 0);
const Vec3f* vertices_ = vertices.get();
const Triangle* tri_indices_ = tri_indices.get();
if (!(vertices.get())) {
std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
"vertices."
<< std::endl;
return com;
}
const std::vector<Vec3f>& vertices_ = *vertices;
if (!(tri_indices.get())) {
std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
"triangles."
<< std::endl;
return com;
}
const std::vector<Triangle>& tri_indices_ = *tri_indices;

for (unsigned int i = 0; i < num_tris; ++i) {
const Triangle& tri = tri_indices_[i];
FCL_REAL d_six_vol =
Expand All @@ -216,8 +229,20 @@ class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry {

FCL_REAL computeVolume() const {
FCL_REAL vol = 0;
const Vec3f* vertices_ = vertices.get();
const Triangle* tri_indices_ = tri_indices.get();
if (!(vertices.get())) {
std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
"vertices."
<< std::endl;
return vol;
}
const std::vector<Vec3f>& vertices_ = *vertices;
if (!(tri_indices.get())) {
std::cerr << "BVH Error in `computeCOM`! The BVHModel does not contain "
"triangles."
<< std::endl;
return vol;
}
const std::vector<Triangle>& tri_indices_ = *tri_indices;
for (unsigned int i = 0; i < num_tris; ++i) {
const Triangle& tri = tri_indices_[i];
FCL_REAL d_six_vol =
Expand All @@ -235,8 +260,20 @@ class HPP_FCL_DLLAPI BVHModelBase : public CollisionGeometry {
C_canonical << 1 / 60.0, 1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0,
1 / 120.0, 1 / 120.0, 1 / 120.0, 1 / 60.0;

const Vec3f* vertices_ = vertices.get();
const Triangle* tri_indices_ = tri_indices.get();
if (!(vertices.get())) {
std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
"not contain vertices."
<< std::endl;
return C;
}
const std::vector<Vec3f>& vertices_ = *vertices;
if (!(vertices.get())) {
std::cerr << "BVH Error in `computeMomentofInertia`! The BVHModel does "
"not contain vertices."
<< std::endl;
return C;
}
const std::vector<Triangle>& tri_indices_ = *tri_indices;
for (unsigned int i = 0; i < num_tris; ++i) {
const Triangle& tri = tri_indices_[i];
const Vec3f& v1 = vertices_[tri[0]];
Expand Down Expand Up @@ -304,13 +341,13 @@ class HPP_FCL_DLLAPI BVHModel : public BVHModelBase {
/// @brief Access the bv giving the its index
const BVNode<BV>& getBV(unsigned int i) const {
assert(i < num_bvs);
return bvs.get()[i];
return (*bvs)[i];
}

/// @brief Access the bv giving the its index
BVNode<BV>& getBV(unsigned int i) {
assert(i < num_bvs);
return bvs.get()[i];
return (*bvs)[i];
}

/// @brief Get the number of bv in the BVH
Expand All @@ -336,10 +373,10 @@ class HPP_FCL_DLLAPI BVHModel : public BVHModelBase {
bool allocateBVs();

unsigned int num_bvs_allocated;
std::shared_ptr<unsigned int> primitive_indices;
std::shared_ptr<std::vector<unsigned int>> primitive_indices;

/// @brief Bounding volume hierarchy
std::shared_ptr<BVNode<BV>> bvs;
std::shared_ptr<std::vector<BVNode<BV>>> bvs;

/// @brief Number of BV nodes in bounding volume hierarchy
unsigned int num_bvs;
Expand Down Expand Up @@ -370,16 +407,19 @@ class HPP_FCL_DLLAPI BVHModel : public BVHModelBase {
/// OBBRSS), special implementation is provided.
void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes,
const Vec3f& parent_c) {
BVNode<BV>* bvs_ = bvs.get();
if (!bvs_[bv_id].isLeaf()) {
makeParentRelativeRecurse(bvs_[bv_id].first_child, parent_axes,
bvs_[bv_id].getCenter());

makeParentRelativeRecurse(bvs_[bv_id].first_child + 1, parent_axes,
bvs_[bv_id].getCenter());
std::vector<BVNode<BV>>& bvs_ = *bvs;
if (!bvs_[static_cast<size_t>(bv_id)].isLeaf()) {
makeParentRelativeRecurse(bvs_[static_cast<size_t>(bv_id)].first_child,
parent_axes,
bvs_[static_cast<size_t>(bv_id)].getCenter());

makeParentRelativeRecurse(
bvs_[static_cast<size_t>(bv_id)].first_child + 1, parent_axes,
bvs_[static_cast<size_t>(bv_id)].getCenter());
}

bvs_[bv_id].bv = translate(bvs_[bv_id].bv, -parent_c);
bvs_[static_cast<size_t>(bv_id)].bv =
translate(bvs_[static_cast<size_t>(bv_id)].bv, -parent_c);
}

private:
Expand Down Expand Up @@ -436,10 +476,14 @@ class HPP_FCL_DLLAPI BVHModel : public BVHModelBase {

if (num_bvs != other.num_bvs) return false;

BVNode<BV>* bvs_ = bvs.get();
BVNode<BV>* other_bvs_ = other.bvs.get();
for (unsigned int k = 0; k < num_bvs; ++k) {
if (bvs_[k] != other_bvs_[k]) return false;
if ((!(bvs.get()) && other.bvs.get()) || (bvs.get() && !(other.bvs.get())))
return false;
if (bvs.get() && other.bvs.get()) {
const std::vector<BVNode<BV>>& bvs_ = *bvs;
const std::vector<BVNode<BV>>& other_bvs_ = *(other.bvs);
for (unsigned int k = 0; k < num_bvs; ++k) {
if (bvs_[k] != other_bvs_[k]) return false;
}
}

return true;
Expand Down
5 changes: 3 additions & 2 deletions include/hpp/fcl/data_types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,8 +68,9 @@ typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f;
typedef Eigen::Matrix<FCL_REAL, 6, 1> Vec6f;
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 1> VecXf;
typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f;
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3> Matrixx3f;
typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 3> Matrixx3i;
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, 3, Eigen::RowMajor> Matrixx3f;
typedef Eigen::Matrix<Eigen::DenseIndex, Eigen::Dynamic, 3, Eigen::RowMajor>
Matrixx3i;
typedef Eigen::Matrix<FCL_REAL, Eigen::Dynamic, Eigen::Dynamic> MatrixXf;
typedef Eigen::Vector2i support_func_guess_t;

Expand Down
118 changes: 59 additions & 59 deletions include/hpp/fcl/internal/traversal_node_hfield_shape.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,25 +74,25 @@ Convex<Quadrilateral> buildConvexQuadrilateral(const HFNode<BV>& node,
"max_height is lower than min_height"); // Check whether the geometry
// is degenerated

std::shared_ptr<Vec3f> pts(new Vec3f[8]);
Vec3f* pts_ = pts.get();
pts_[0] = Vec3f(x0, y0, min_height);
pts_[1] = Vec3f(x0, y1, min_height);
pts_[2] = Vec3f(x1, y1, min_height);
pts_[3] = Vec3f(x1, y0, min_height);
pts_[4] = Vec3f(x0, y0, cell(0, 0));
pts_[5] = Vec3f(x0, y1, cell(1, 0));
pts_[6] = Vec3f(x1, y1, cell(1, 1));
pts_[7] = Vec3f(x1, y0, cell(0, 1));

std::shared_ptr<Quadrilateral> polygons(new Quadrilateral[6]);
Quadrilateral* polygons_ = polygons.get();
polygons_[0].set(0, 3, 2, 1); // x+ side
polygons_[1].set(0, 1, 5, 4); // y- side
polygons_[2].set(1, 2, 6, 5); // x- side
polygons_[3].set(2, 3, 7, 6); // y+ side
polygons_[4].set(3, 0, 4, 7); // z- side
polygons_[5].set(4, 5, 6, 7); // z+ side
std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
Vec3f(x0, y0, min_height),
Vec3f(x0, y1, min_height),
Vec3f(x1, y1, min_height),
Vec3f(x1, y0, min_height),
Vec3f(x0, y0, cell(0, 0)),
Vec3f(x0, y1, cell(1, 0)),
Vec3f(x1, y1, cell(1, 1)),
Vec3f(x1, y0, cell(0, 1)),
}));

std::shared_ptr<std::vector<Quadrilateral>> polygons(
new std::vector<Quadrilateral>(6));
(*polygons)[0].set(0, 3, 2, 1); // x+ side
(*polygons)[1].set(0, 1, 5, 4); // y- side
(*polygons)[2].set(1, 2, 6, 5); // x- side
(*polygons)[3].set(2, 3, 7, 6); // y+ side
(*polygons)[4].set(3, 0, 4, 7); // z- side
(*polygons)[5].set(4, 5, 6, 7); // z+ side

return Convex<Quadrilateral>(pts, // points
8, // num points
Expand Down Expand Up @@ -123,25 +123,25 @@ void buildConvexTriangles(const HFNode<BV>& node, const HeightField<BV>& model,
HPP_FCL_UNUSED_VARIABLE(max_height);

{
std::shared_ptr<Vec3f> pts(new Vec3f[6]);
Vec3f* pts_ = pts.get();
pts_[0] = Vec3f(x0, y0, min_height); //
pts_[1] = Vec3f(x0, y1, min_height); //
pts_[2] = Vec3f(x1, y0, min_height); //
pts_[3] = Vec3f(x0, y0, cell(0, 0)); //
pts_[4] = Vec3f(x0, y1, cell(1, 0)); //
pts_[5] = Vec3f(x1, y0, cell(0, 1)); //

std::shared_ptr<Triangle> triangles(new Triangle[8]);
Triangle* triangles_ = triangles.get();
triangles_[0].set(0, 1, 2); // bottom
triangles_[1].set(3, 5, 4); // top
triangles_[2].set(0, 3, 1);
triangles_[3].set(3, 4, 1);
triangles_[4].set(1, 5, 2);
triangles_[5].set(1, 4, 5);
triangles_[6].set(0, 2, 5);
triangles_[7].set(5, 3, 0);
std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
Vec3f(x0, y0, min_height),
Vec3f(x0, y1, min_height),
Vec3f(x1, y0, min_height),
Vec3f(x0, y0, cell(0, 0)),
Vec3f(x0, y1, cell(1, 0)),
Vec3f(x1, y0, cell(0, 1)),
}));

std::shared_ptr<std::vector<Triangle>> triangles(
new std::vector<Triangle>(8));
(*triangles)[0].set(0, 1, 2); // bottom
(*triangles)[1].set(3, 5, 4); // top
(*triangles)[2].set(0, 3, 1);
(*triangles)[3].set(3, 4, 1);
(*triangles)[4].set(1, 5, 2);
(*triangles)[5].set(1, 4, 5);
(*triangles)[6].set(0, 2, 5);
(*triangles)[7].set(5, 3, 0);

convex1.set(pts, // points
6, // num points
Expand All @@ -151,25 +151,25 @@ void buildConvexTriangles(const HFNode<BV>& node, const HeightField<BV>& model,
}

{
std::shared_ptr<Vec3f> pts(new Vec3f[6]);
Vec3f* pts_ = pts.get();
pts_[0] = Vec3f(x0, y1, min_height);
pts_[1] = Vec3f(x1, y1, min_height);
pts_[2] = Vec3f(x1, y0, min_height);
pts_[3] = Vec3f(x0, y1, cell(1, 0));
pts_[4] = Vec3f(x1, y1, cell(1, 1));
pts_[5] = Vec3f(x1, y0, cell(0, 1));

std::shared_ptr<Triangle> triangles(new Triangle[8]);
Triangle* triangles_ = triangles.get();
triangles_[0].set(2, 0, 1); // bottom
triangles_[1].set(3, 5, 4); // top
triangles_[2].set(0, 3, 1);
triangles_[3].set(3, 4, 1);
triangles_[4].set(0, 2, 5);
triangles_[5].set(0, 5, 3);
triangles_[6].set(1, 5, 2);
triangles_[7].set(4, 2, 1);
std::shared_ptr<std::vector<Vec3f>> pts(new std::vector<Vec3f>({
Vec3f(x0, y1, min_height),
Vec3f(x1, y1, min_height),
Vec3f(x1, y0, min_height),
Vec3f(x0, y1, cell(1, 0)),
Vec3f(x1, y1, cell(1, 1)),
Vec3f(x1, y0, cell(0, 1)),
}));

std::shared_ptr<std::vector<Triangle>> triangles(
new std::vector<Triangle>(8));
(*triangles)[0].set(2, 0, 1); // bottom
(*triangles)[1].set(3, 5, 4); // top
(*triangles)[2].set(0, 3, 1);
(*triangles)[3].set(3, 4, 1);
(*triangles)[4].set(0, 2, 5);
(*triangles)[5].set(0, 5, 3);
(*triangles)[6].set(1, 5, 2);
(*triangles)[7].set(4, 2, 1);

convex2.set(pts, // points
6, // num points
Expand All @@ -195,8 +195,8 @@ bool binCorrection(const Convex<Polygone>& convex, const Shape& shape,
const Transform3f& shape_pose, FCL_REAL& distance,
Vec3f& contact_1, Vec3f& contact_2, Vec3f& normal,
Vec3f& normal_top, bool& is_collision) {
const Polygone& top_triangle = convex.polygons.get()[1];
const Vec3f* points = convex.points.get();
const Polygone& top_triangle = (*(convex.polygons))[1];
const std::vector<Vec3f>& points = *(convex.points);
const Vec3f pointA = points[top_triangle[0]];
const Vec3f pointB = points[top_triangle[1]];
const Vec3f pointC = points[top_triangle[2]];
Expand Down
30 changes: 16 additions & 14 deletions include/hpp/fcl/internal/traversal_node_octree.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -370,19 +370,21 @@ class HPP_FCL_DLLAPI OcTreeSolver {
Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);

int primitive_id = tree2->getBV(root2).primitiveId();
const Triangle& tri_id = tree2->tri_indices.get()[primitive_id];
const Vec3f& p1 = tree2->vertices.get()[tri_id[0]];
const Vec3f& p2 = tree2->vertices.get()[tri_id[1]];
const Vec3f& p3 = tree2->vertices.get()[tri_id[2]];
size_t primitive_id =
static_cast<size_t>(tree2->getBV(root2).primitiveId());
const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id];
const Vec3f& p1 = (*(tree2->vertices))[tri_id[0]];
const Vec3f& p2 = (*(tree2->vertices))[tri_id[1]];
const Vec3f& p3 = (*(tree2->vertices))[tri_id[2]];

FCL_REAL dist;
Vec3f closest_p1, closest_p2, normal;
solver->shapeTriangleInteraction(box, box_tf, p1, p2, p3, tf2, dist,
closest_p1, closest_p2, normal);

dresult->update(dist, tree1, tree2, (int)(root1 - tree1->getRoot()),
primitive_id, closest_p1, closest_p2, normal);
static_cast<int>(primitive_id), closest_p1, closest_p2,
normal);

return drequest->isSatisfied(*dresult);
} else
Expand Down Expand Up @@ -483,11 +485,11 @@ class HPP_FCL_DLLAPI OcTreeSolver {
Transform3f box_tf;
constructBox(bv1, tf1, box, box_tf);

int primitive_id = bvn2.primitiveId();
const Triangle& tri_id = tree2->tri_indices.get()[primitive_id];
const Vec3f& p1 = tree2->vertices.get()[tri_id[0]];
const Vec3f& p2 = tree2->vertices.get()[tri_id[1]];
const Vec3f& p3 = tree2->vertices.get()[tri_id[2]];
size_t primitive_id = static_cast<size_t>(bvn2.primitiveId());
const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id];
const Vec3f& p1 = (*(tree2->vertices))[tri_id[0]];
const Vec3f& p2 = (*(tree2->vertices))[tri_id[1]];
const Vec3f& p3 = (*(tree2->vertices))[tri_id[2]];

Vec3f c1, c2, normal;
FCL_REAL distance;
Expand All @@ -498,9 +500,9 @@ class HPP_FCL_DLLAPI OcTreeSolver {

if (cresult->numContacts() < crequest->num_max_contacts) {
if (distToCollision <= crequest->collision_distance_threshold) {
cresult->addContact(Contact(tree1, tree2,
(int)(root1 - tree1->getRoot()),
primitive_id, c1, c2, normal, distance));
cresult->addContact(Contact(
tree1, tree2, (int)(root1 - tree1->getRoot()),
static_cast<int>(primitive_id), c1, c2, normal, distance));
}
}
internal::updateDistanceLowerBoundFromLeaf(*crequest, *cresult,
Expand Down
Loading

0 comments on commit 3766c24

Please sign in to comment.