60 num_tris_allocated(0),
61 num_vertices_allocated(0),
62 num_vertex_updated(0) {}
93 if (other_ptr ==
nullptr)
return false;
99 if (!result)
return false;
101 for (
size_t k = 0; k < static_cast<size_t>(
num_tris); ++k)
104 for (
size_t k = 0; k < static_cast<size_t>(
num_vertices); ++k)
108 for (
size_t k = 0; k < static_cast<size_t>(
num_vertices); ++k) {
133 const char* qhullCommand) {
139 template <
typename BV>
142 bv_splitter(other.bv_splitter),
143 bv_fitter(other.bv_fitter) {
145 unsigned int num_primitives = 0;
171 unsigned int num_vertices_) {
184 if (num_tris_ <= 0) num_tris_ = 8;
185 if (num_vertices_ <= 0) num_vertices_ = 8;
193 std::cerr <<
"BVH Error! Out of memory for tri_indices array on " 202 <<
"BVH Error! Out of memory for vertices array on BeginModel() call!" 209 <<
"BVH Warning! Call beginModel() on a BVHModel that is not empty. " 210 "This model was cleared and previous triangles/vertices were lost." 223 std::cerr <<
"BVH Warning! Call addVertex() in a wrong order. addVertex() " 224 "was ignored. Must do a beginModel() to clear the model for " 225 "addition of new vertices." 234 <<
"BVH Error! Out of memory for vertices array on addVertex() call!" 253 std::cerr <<
"BVH Warning! Call addSubModel() in a wrong order. " 254 "addSubModel() was ignored. Must do a beginModel() to clear " 255 "the model for addition of new vertices." 260 const unsigned int num_tris_to_add = (
unsigned int)triangles.rows();
265 std::cerr <<
"BVH Error! Out of memory for tri_indices array on " 266 "addSubModel() call!" 277 for (Eigen::DenseIndex i = 0; i < triangles.rows(); ++i) {
278 const Matrixx3i::ConstRowXpr triangle = triangles.row(i);
280 static_cast<Triangle::index_type>(triangle[1]),
281 static_cast<Triangle::index_type>(triangle[2]));
289 std::cerr <<
"BVH Warning! Call addVertex() in a wrong order. " 290 "addVertices() was ignored. Must do a beginModel() to clear " 291 "the model for addition of new vertices." 301 <<
"BVH Error! Out of memory for vertices array on addVertex() call!" 311 for (Eigen::DenseIndex
id = 0;
id < points.rows(); ++id)
320 std::cerr <<
"BVH Warning! Call addTriangle() in a wrong order. " 321 "addTriangle() was ignored. Must do a beginModel() to clear " 322 "the model for addition of new triangles." 330 std::cerr <<
"BVH Error! Out of memory for vertices array on " 331 "addTriangle() call!" 354 std::cerr <<
"BVH Error! Out of memory for tri_indices array on " 355 "addTriangle() call!" 376 std::cerr <<
"BVH Warning! Call addSubModel() in a wrong order. " 377 "addSubModel() was ignored. Must do a beginModel() to clear " 378 "the model for addition of new vertices." 383 const unsigned int num_vertices_to_add = (
unsigned int)ps.size();
389 std::cerr <<
"BVH Error! Out of memory for vertices array on " 390 "addSubModel() call!" 402 for (
size_t i = 0; i < (size_t)num_vertices_to_add; ++i) {
411 const std::vector<Triangle>& ts) {
413 std::cerr <<
"BVH Warning! Call addSubModel() in a wrong order. " 414 "addSubModel() was ignored. Must do a beginModel() to clear " 415 "the model for addition of new vertices." 420 const unsigned int num_vertices_to_add = (
unsigned int)ps.size();
426 std::cerr <<
"BVH Error! Out of memory for vertices array on " 427 "addSubModel() call!" 441 for (
size_t i = 0; i < (size_t)num_vertices_to_add; ++i) {
446 const unsigned int num_tris_to_add = (
unsigned int)ts.size();
451 std::cerr <<
"BVH Error! Out of memory for tri_indices array on " 452 "addSubModel() call!" 463 for (
size_t i = 0; i < (size_t)num_tris_to_add; ++i) {
466 t[2] + (
size_t)offset);
475 std::cerr <<
"BVH Warning! Call endModel() in wrong order. endModel() was " 482 std::cerr <<
"BVH Error! endModel() called on model with no triangles and " 492 std::cerr <<
"BVH Error! Out of memory for tri_indices array in " 512 <<
"BVH Error! Out of memory for vertices array in endModel() call!" 535 std::cerr <<
"BVH Error! Call beginReplaceModel() on a BVHModel that has " 553 std::cerr <<
"BVH Warning! Call replaceVertex() in a wrong order. " 554 "replaceVertex() was ignored. Must do a beginReplaceModel() " 555 "for initialization." 569 std::cerr <<
"BVH Warning! Call replaceTriangle() in a wrong order. " 570 "replaceTriangle() was ignored. Must do a beginReplaceModel() " 571 "for initialization." 587 std::cerr <<
"BVH Warning! Call replaceSubModel() in a wrong order. " 588 "replaceSubModel() was ignored. Must do a beginReplaceModel() " 589 "for initialization." 594 for (
unsigned int i = 0; i < ps.size(); ++i) {
603 std::cerr <<
"BVH Warning! Call endReplaceModel() in a wrong order. " 604 "endReplaceModel() was ignored. " 610 std::cerr <<
"BVH Error! The replaced model should have the same number of " 611 "vertices as the old model." 632 std::cerr <<
"BVH Error! Call beginUpdatemodel() on a BVHModel that has no " 657 <<
"BVH Warning! Call updateVertex() in a wrong order. updateVertex() " 658 "was ignored. Must do a beginUpdateModel() for initialization." 672 std::cerr <<
"BVH Warning! Call updateTriangle() in a wrong order. " 673 "updateTriangle() was ignored. Must do a beginUpdateModel() " 674 "for initialization." 690 std::cerr <<
"BVH Warning! Call updateSubModel() in a wrong order. " 691 "updateSubModel() was ignored. Must do a beginUpdateModel() " 692 "for initialization." 697 for (
unsigned int i = 0; i < ps.size(); ++i) {
706 std::cerr <<
"BVH Warning! Call endUpdateModel() in a wrong order. " 707 "endUpdateModel() was ignored. " 713 std::cerr <<
"BVH Error! The updated model should have the same number of " 714 "vertices as the old model." 756 template <
typename BV>
766 template <
typename BV>
775 template <
typename BV>
778 unsigned int num_bvs_to_be_allocated = 0;
782 num_bvs_to_be_allocated = 2 *
num_tris - 1;
787 std::cerr <<
"BVH Error! Out of memory for BV array in endModel()!" 796 template <
typename BV>
798 unsigned int mem_bv_list = (
unsigned int)
sizeof(BV) *
num_bvs;
802 unsigned int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list +
805 std::cerr <<
"Total for model " << total_mem <<
" bytes." << std::endl;
806 std::cerr <<
"BVs: " <<
num_bvs <<
" allocated." << std::endl;
807 std::cerr <<
"Tris: " <<
num_tris <<
" allocated." << std::endl;
808 std::cerr <<
"Vertices: " <<
num_vertices <<
" allocated." << std::endl;
811 return static_cast<int>(total_mem);
814 template <
typename BV>
823 unsigned int num_primitives = 0;
826 num_primitives = (
unsigned int)
num_tris;
832 std::cerr <<
"BVH Error: Model type not supported!" << std::endl;
845 template <
typename BV>
847 unsigned int num_primitives) {
853 BV bv =
bv_fitter->fit(cur_primitive_indices, num_primitives);
854 bv_splitter->computeRule(bv, cur_primitive_indices, num_primitives);
860 if (num_primitives == 1) {
861 bvnode->
first_child = -((int)(*cur_primitive_indices) + 1);
867 for (
unsigned int i = 0; i < num_primitives; ++i) {
870 p =
vertices[cur_primitive_indices[i]];
877 p = (p1 + p2 + p3) / 3.;
879 std::cerr <<
"BVH Error: Model type not supported!" << std::endl;
893 unsigned int temp = cur_primitive_indices[i];
894 cur_primitive_indices[i] = cur_primitive_indices[c1];
895 cur_primitive_indices[c1] = temp;
900 if ((c1 == 0) || (c1 == num_primitives)) c1 = num_primitives / 2;
902 const unsigned int num_first_half = c1;
906 num_primitives - num_first_half);
912 template <
typename BV>
920 template <
typename BV>
933 template <
typename BV>
969 for (
int i = 0; i < 3; ++i) {
978 std::cerr <<
"BVH Error: Model type not supported!" << std::endl;
994 template <
typename BV>
997 for (
unsigned int i = 0; i <
num_bvs; ++i) {
999 bvs[i].num_primitives);
1010 const Vec3f& parent_c) {
1012 if (!
bvs[bv_id].isLeaf()) {
1020 obb.
axes.applyOnTheLeft(parent_axes.transpose());
1023 obb.
To.noalias() = parent_axes.transpose() * t;
1028 const Vec3f& parent_c) {
1029 RSS& rss =
bvs[bv_id].bv;
1030 if (!
bvs[bv_id].isLeaf()) {
1038 rss.
axes.applyOnTheLeft(parent_axes.transpose());
1041 rss.
Tr.noalias() = parent_axes.transpose() * t;
1047 const Vec3f& parent_c) {
1049 RSS& rss =
bvs[bv_id].bv.rss;
1050 if (!
bvs[bv_id].isLeaf()) {
1057 rss.
axes.noalias() = parent_axes.transpose() * obb.
axes;
1061 obb.
To.noalias() = parent_axes.transpose() * t;
int updateSubModel(const std::vector< Vec3f > &ps)
Update a set of points in the old BVH model.
Vec3f * prev_vertices
Geometry point data in previous frame.
shared_ptr< ConvexBase > convex
Convex<Triangle> representation of this object.
int addTriangles(const Matrixx3i &triangles)
Add triangles in the new BVH model.
unsigned int first_primitive
The start id the primitive belonging to the current node. The index is referred to the primitive_indi...
unsigned int num_vertex_updated
int replaceSubModel(const std::vector< Vec3f > &ps)
Replace a set of points in the old BVH model.
int buildTree()
Build the bounding volume hierarchy.
int refitTree_topdown()
Refit the bounding volume hierarchy in a top-down way (slow but more compact)
static ConvexBase * convexHull(const Vec3f *points, unsigned int num_points, bool keepTriangles, const char *qhullCommand=NULL)
Build a convex hull based on Qhull library and store the vertices and optionally the triangles...
Vec3f center() const
Center of the AABB.
after tree has been build, ready for cd use
Cannot allocate memory for vertices and triangles.
BVH construction does not follow correct sequence.
BV bv
bounding volume storing the geometry
Matrix3f axes
Orientation of RSS. axis[i] is the ith column of the orientation matrix for the RSS; it is also the i...
int addTriangle(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3)
Add one triangle in the new BVH model.
shared_ptr< BVFitter< BV > > bv_fitter
Fitting rule to fit a BV node to a set of geometry primitives.
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
int addSubModel(const std::vector< Vec3f > &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
int addVertices(const Matrixx3f &points)
Add points in the new BVH model.
int endUpdateModel(bool refit=true, bool bottomup=true)
End BVH model update, will also refit or rebuild the bounding volume hierarchy.
int updateVertex(const Vec3f &p)
Update one point in the old BVH model.
A base class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewe...
int addVertex(const Vec3f &p)
Add one point in the new BVH model.
unsigned int num_vertices
Number of points.
unsigned int num_vertices_allocated
int first_child
An index for first child node or primitive If the value is positive, it is the index of the first chi...
int replaceTriangle(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3)
Replace one triangle in the old BVH model.
int recursiveRefitTree_bottomup(int bv_id)
Recursive kernel for bottomup refitting.
void fit(Vec3f *ps, unsigned int n, BV &bv)
Compute a bounding volume that fits a set of n points.
bool buildConvexHull(bool keepTriangle, const char *qhullCommand=NULL)
Build a convex hull and store it in attribute convex.
void makeParentRelativeRecurse(int bv_id, Matrix3f &parent_axes, const Vec3f &parent_c)
int refitTree(bool bottomup)
Refit the bounding volume hierarchy.
unsigned int * primitive_indices
Triangle * tri_indices
Geometry triangle index data, will be NULL for point clouds.
AABB aabb_local
AABB in local coordinate, used for tight AABB when only translation transform.
int replaceVertex(const Vec3f &p)
Replace one point in the old BVH model.
Vec3f * vertices
Geometry point data.
unsigned int num_tris
Number of triangles.
Vec3f Tr
Origin of the rectangle in RSS.
void buildConvexRepresentation(bool share_memory)
Build this Convex<Triangle> representation of this model. The result is stored in attribute convex...
int beginModel(unsigned int num_tris=0, unsigned int num_vertices=0)
Begin a new BVH model.
int recursiveBuildTree(int bv_id, unsigned int first_primitive, unsigned int num_primitives)
Recursive kernel for hierarchy construction.
Eigen::Matrix< Eigen::DenseIndex, Eigen::Dynamic, 3 > Matrixx3i
int beginReplaceModel()
Replace the geometry information of current frame (i.e. should have the same mesh topology with the p...
BVHModelType getModelType() const
Model type described by the instance.
int updateTriangle(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3)
Update one triangle in the old BVH model.
unsigned int num_tris_allocated
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, 3 > Matrixx3f
BVHModelType
BVH model type.
unsigned int num_bvs
Number of BV nodes in bounding volume hierarchy.
A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and...
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
FCL_REAL aabb_radius
AABB radius.
int leftChild() const
Return the index of the first child. The index is referred to the bounding volume array (i...
int endReplaceModel(bool refit=true, bool bottomup=true)
End BVH model replacement, will also refit or rebuild the bounding volume hierarchy.
virtual bool isEqual(const CollisionGeometry &other) const
for ccd vertex update
void set(index_type p1, index_type p2, index_type p3)
Set the vertex indices of the triangle.
Triangle with 3 indices for points.
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, cylinder, convex, plane, triangle), and octree
Vec3f aabb_center
AABB center in local coordinate.
NODE_TYPE getNodeType() const
Get the BV type: default is unknown.
int memUsage(const bool msg) const
Check the number of memory used.
bool isLeaf() const
Whether current node is a leaf node (i.e. contains a primitive index.
int refitTree_bottomup()
Refit the bounding volume hierarchy in a bottom-up way (fast but less compact)
BVHBuildState build_state
The state of BVH building process.
BVNode< BV > * bvs
Bounding volume hierarchy.
unsigned int num_primitives
The number of primitives belonging to the current node.
unsigned int num_bvs_allocated
shared_ptr< BVSplitter< BV > > bv_splitter
Split rule to split one BV node into two children.
The class for the default algorithm fitting a bounding volume to a set of points. ...
A class describing the split rule that splits each BV node.
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
int rightChild() const
Return the index of the second child. The index is referred to the bounding volume array (i...
BVH geometry in previous frame is not prepared.
The geometry for the object for collision or distance computation.
Matrix3f axes
Orientation of OBB. axis[i] is the ith column of the orientation matrix for the box; it is also the i...
BVHModel()
Default constructor to build an empty BVH.
empty state, immediately after constructor
int beginUpdateModel()
Replace the geometry information of current frame (i.e. should have the same mesh topology with the p...
void computeLocalAABB()
Compute the AABB for the BVH, used for broad-phase collision.
BVH geometry is not prepared.
BVHModelBase()
Constructing an empty BVH.
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
Oriented bounding box class.