BVH_model.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2015, Open Source Robotics Foundation
6  * Copyright (c) 2020, INRIA
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Open Source Robotics Foundation nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  */
36 
39 #include <hpp/fcl/BVH/BVH_model.h>
40 
41 #include <iostream>
42 #include <string.h>
43 
44 #include <hpp/fcl/BV/BV.h>
45 #include <hpp/fcl/shape/convex.h>
46 
49 
50 namespace hpp {
51 namespace fcl {
52 
54  : vertices(NULL),
55  tri_indices(NULL),
56  prev_vertices(NULL),
57  num_tris(0),
58  num_vertices(0),
59  build_state(BVH_BUILD_STATE_EMPTY),
60  num_tris_allocated(0),
61  num_vertices_allocated(0),
62  num_vertex_updated(0) {}
63 
65  : CollisionGeometry(other),
66  num_tris(other.num_tris),
68  build_state(other.build_state),
71  if (other.vertices) {
73  std::copy(other.vertices, other.vertices + num_vertices, vertices);
74  } else
75  vertices = nullptr;
76 
77  if (other.tri_indices) {
79  std::copy(other.tri_indices, other.tri_indices + num_tris, tri_indices);
80  } else
81  tri_indices = nullptr;
82 
83  if (other.prev_vertices) {
85  std::copy(other.prev_vertices, other.prev_vertices + num_vertices,
87  } else
88  prev_vertices = nullptr;
89 }
90 
91 bool BVHModelBase::isEqual(const CollisionGeometry& _other) const {
92  const BVHModelBase* other_ptr = dynamic_cast<const BVHModelBase*>(&_other);
93  if (other_ptr == nullptr) return false;
94  const BVHModelBase& other = *other_ptr;
95 
96  bool result =
97  num_tris == other.num_tris && num_vertices == other.num_vertices;
98 
99  if (!result) return false;
100 
101  for (size_t k = 0; k < static_cast<size_t>(num_tris); ++k)
102  if (tri_indices[k] != other.tri_indices[k]) return false;
103 
104  for (size_t k = 0; k < static_cast<size_t>(num_vertices); ++k)
105  if (vertices[k] != other.vertices[k]) return false;
106 
107  if (prev_vertices != nullptr && other.prev_vertices != nullptr) {
108  for (size_t k = 0; k < static_cast<size_t>(num_vertices); ++k) {
109  if (prev_vertices[k] != other.prev_vertices[k]) return false;
110  }
111  }
112 
113  return true;
114 }
115 
117  if (!convex) {
118  Vec3f* points = vertices;
119  Triangle* polygons = tri_indices;
120  if (!share_memory) {
121  points = new Vec3f[num_vertices];
122  std::copy(vertices, vertices + num_vertices, points);
123 
124  polygons = new Triangle[num_tris];
125  std::copy(tri_indices, tri_indices + num_tris, polygons);
126  }
127  convex.reset(new Convex<Triangle>(!share_memory, points, num_vertices,
128  polygons, num_tris));
129  }
130 }
131 
132 bool BVHModelBase::buildConvexHull(bool keepTriangle,
133  const char* qhullCommand) {
134  convex.reset(ConvexBase::convexHull(vertices, num_vertices, keepTriangle,
135  qhullCommand));
136  return num_vertices == convex->num_points;
137 }
138 
139 template <typename BV>
141  : BVHModelBase(other),
142  bv_splitter(other.bv_splitter),
143  bv_fitter(other.bv_fitter) {
144  if (other.primitive_indices) {
145  unsigned int num_primitives = 0;
146  switch (other.getModelType()) {
147  case BVH_MODEL_TRIANGLES:
148  num_primitives = num_tris;
149  break;
151  num_primitives = num_vertices;
152  break;
153  default:;
154  }
155 
156  primitive_indices = new unsigned int[num_primitives];
157  std::copy(other.primitive_indices, other.primitive_indices + num_primitives,
159  } else
160  primitive_indices = nullptr;
161 
163  if (other.bvs) {
164  bvs = new BVNode<BV>[num_bvs];
165  std::copy(other.bvs, other.bvs + num_bvs, bvs);
166  } else
167  bvs = nullptr;
168 }
169 
170 int BVHModelBase::beginModel(unsigned int num_tris_,
171  unsigned int num_vertices_) {
173  delete[] vertices;
174  vertices = nullptr;
175  delete[] tri_indices;
176  tri_indices = nullptr;
177  delete[] prev_vertices;
178  prev_vertices = nullptr;
179 
181  deleteBVs();
182  }
183 
184  if (num_tris_ <= 0) num_tris_ = 8;
185  if (num_vertices_ <= 0) num_vertices_ = 8;
186 
187  num_vertices_allocated = num_vertices_;
188  num_tris_allocated = num_tris_;
189 
191 
192  if (!tri_indices) {
193  std::cerr << "BVH Error! Out of memory for tri_indices array on "
194  "BeginModel() call!"
195  << std::endl;
197  }
198 
200  if (!vertices) {
201  std::cerr
202  << "BVH Error! Out of memory for vertices array on BeginModel() call!"
203  << std::endl;
205  }
206 
208  std::cerr
209  << "BVH Warning! Call beginModel() on a BVHModel that is not empty. "
210  "This model was cleared and previous triangles/vertices were lost."
211  << std::endl;
214  }
215 
217 
218  return BVH_OK;
219 }
220 
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."
226  << std::endl;
228  }
229 
231  Vec3f* temp = new Vec3f[num_vertices_allocated * 2];
232  if (!temp) {
233  std::cerr
234  << "BVH Error! Out of memory for vertices array on addVertex() call!"
235  << std::endl;
237  }
238 
239  std::copy(vertices, vertices + num_vertices, temp);
240  delete[] vertices;
241  vertices = temp;
243  }
244 
245  vertices[num_vertices] = p;
246  num_vertices += 1;
247 
248  return BVH_OK;
249 }
250 
251 int BVHModelBase::addTriangles(const Matrixx3i& triangles) {
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."
256  << std::endl;
258  }
259 
260  const unsigned int num_tris_to_add = (unsigned int)triangles.rows();
261 
262  if (num_tris + num_tris_to_add > num_tris_allocated) {
263  Triangle* temp = new Triangle[num_tris_allocated * 2 + num_tris_to_add];
264  if (!temp) {
265  std::cerr << "BVH Error! Out of memory for tri_indices array on "
266  "addSubModel() call!"
267  << std::endl;
269  }
270 
271  std::copy(tri_indices, tri_indices + num_tris, temp);
272  delete[] tri_indices;
273  tri_indices = temp;
274  num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add;
275  }
276 
277  for (Eigen::DenseIndex i = 0; i < triangles.rows(); ++i) {
278  const Matrixx3i::ConstRowXpr triangle = triangles.row(i);
279  tri_indices[num_tris++].set(static_cast<Triangle::index_type>(triangle[0]),
280  static_cast<Triangle::index_type>(triangle[1]),
281  static_cast<Triangle::index_type>(triangle[2]));
282  }
283 
284  return BVH_OK;
285 }
286 
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."
292  << std::endl;
294  }
295 
296  if (num_vertices + points.rows() > num_vertices_allocated) {
297  num_vertices_allocated = num_vertices + (unsigned int)points.rows();
298  Vec3f* temp = new Vec3f[num_vertices_allocated];
299  if (!temp) {
300  std::cerr
301  << "BVH Error! Out of memory for vertices array on addVertex() call!"
302  << std::endl;
304  }
305 
306  std::copy(vertices, vertices + num_vertices, temp);
307  delete[] vertices;
308  vertices = temp;
309  }
310 
311  for (Eigen::DenseIndex id = 0; id < points.rows(); ++id)
312  vertices[num_vertices++] = points.row(id).transpose();
313 
314  return BVH_OK;
315 }
316 
317 int BVHModelBase::addTriangle(const Vec3f& p1, const Vec3f& p2,
318  const Vec3f& p3) {
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."
323  << std::endl;
325  }
326 
328  Vec3f* temp = new Vec3f[num_vertices_allocated * 2 + 2];
329  if (!temp) {
330  std::cerr << "BVH Error! Out of memory for vertices array on "
331  "addTriangle() call!"
332  << std::endl;
334  }
335 
336  std::copy(vertices, vertices + num_vertices, temp);
337  delete[] vertices;
338  vertices = temp;
340  }
341 
342  const unsigned int offset = num_vertices;
343 
345  num_vertices++;
346  vertices[num_vertices] = p2;
347  num_vertices++;
348  vertices[num_vertices] = p3;
349  num_vertices++;
350 
351  if (num_tris >= num_tris_allocated) {
352  Triangle* temp = new Triangle[num_tris_allocated * 2];
353  if (!temp) {
354  std::cerr << "BVH Error! Out of memory for tri_indices array on "
355  "addTriangle() call!"
356  << std::endl;
358  }
359 
360  std::copy(tri_indices, tri_indices + num_tris, temp);
361  delete[] tri_indices;
362  tri_indices = temp;
363  num_tris_allocated *= 2;
364  }
365 
367  (Triangle::index_type)(offset + 1),
368  (Triangle::index_type)(offset + 2));
369  num_tris++;
370 
371  return BVH_OK;
372 }
373 
374 int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps) {
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."
379  << std::endl;
381  }
382 
383  const unsigned int num_vertices_to_add = (unsigned int)ps.size();
384 
385  if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) {
386  Vec3f* temp =
387  new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1];
388  if (!temp) {
389  std::cerr << "BVH Error! Out of memory for vertices array on "
390  "addSubModel() call!"
391  << std::endl;
393  }
394 
395  std::copy(vertices, vertices + num_vertices, temp);
396  delete[] vertices;
397  vertices = temp;
399  num_vertices_allocated * 2 + num_vertices_to_add - 1;
400  }
401 
402  for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) {
403  vertices[num_vertices] = ps[i];
404  num_vertices++;
405  }
406 
407  return BVH_OK;
408 }
409 
410 int BVHModelBase::addSubModel(const std::vector<Vec3f>& ps,
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."
416  << std::endl;
418  }
419 
420  const unsigned int num_vertices_to_add = (unsigned int)ps.size();
421 
422  if (num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated) {
423  Vec3f* temp =
424  new Vec3f[num_vertices_allocated * 2 + num_vertices_to_add - 1];
425  if (!temp) {
426  std::cerr << "BVH Error! Out of memory for vertices array on "
427  "addSubModel() call!"
428  << std::endl;
430  }
431 
432  std::copy(vertices, vertices + num_vertices, temp);
433  delete[] vertices;
434  vertices = temp;
436  num_vertices_allocated * 2 + num_vertices_to_add - 1;
437  }
438 
439  const unsigned int offset = num_vertices;
440 
441  for (size_t i = 0; i < (size_t)num_vertices_to_add; ++i) {
442  vertices[num_vertices] = ps[i];
443  num_vertices++;
444  }
445 
446  const unsigned int num_tris_to_add = (unsigned int)ts.size();
447 
448  if (num_tris + num_tris_to_add - 1 >= num_tris_allocated) {
449  Triangle* temp = new Triangle[num_tris_allocated * 2 + num_tris_to_add - 1];
450  if (!temp) {
451  std::cerr << "BVH Error! Out of memory for tri_indices array on "
452  "addSubModel() call!"
453  << std::endl;
455  }
456 
457  std::copy(tri_indices, tri_indices + num_tris, temp);
458  delete[] tri_indices;
459  tri_indices = temp;
460  num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add - 1;
461  }
462 
463  for (size_t i = 0; i < (size_t)num_tris_to_add; ++i) {
464  const Triangle& t = ts[i];
465  tri_indices[num_tris].set(t[0] + (size_t)offset, t[1] + (size_t)offset,
466  t[2] + (size_t)offset);
467  num_tris++;
468  }
469 
470  return BVH_OK;
471 }
472 
475  std::cerr << "BVH Warning! Call endModel() in wrong order. endModel() was "
476  "ignored."
477  << std::endl;
479  }
480 
481  if (num_tris == 0 && num_vertices == 0) {
482  std::cerr << "BVH Error! endModel() called on model with no triangles and "
483  "vertices."
484  << std::endl;
486  }
487 
489  if (num_tris > 0) {
490  Triangle* new_tris = new Triangle[num_tris];
491  if (!new_tris) {
492  std::cerr << "BVH Error! Out of memory for tri_indices array in "
493  "endModel() call!"
494  << std::endl;
496  }
497  std::copy(tri_indices, tri_indices + num_tris, new_tris);
498  delete[] tri_indices;
499  tri_indices = new_tris;
501  } else {
502  delete[] tri_indices;
503  tri_indices = NULL;
505  }
506  }
507 
509  Vec3f* new_vertices = new Vec3f[num_vertices];
510  if (!new_vertices) {
511  std::cerr
512  << "BVH Error! Out of memory for vertices array in endModel() call!"
513  << std::endl;
515  }
516  std::copy(vertices, vertices + num_vertices, new_vertices);
517  delete[] vertices;
518  vertices = new_vertices;
520  }
521 
522  // construct BVH tree
524 
525  buildTree();
526 
527  // finish constructing
529 
530  return BVH_OK;
531 }
532 
535  std::cerr << "BVH Error! Call beginReplaceModel() on a BVHModel that has "
536  "no previous frame."
537  << std::endl;
539  }
540 
541  if (prev_vertices) delete[] prev_vertices;
542  prev_vertices = NULL;
543 
544  num_vertex_updated = 0;
545 
547 
548  return BVH_OK;
549 }
550 
553  std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. "
554  "replaceVertex() was ignored. Must do a beginReplaceModel() "
555  "for initialization."
556  << std::endl;
558  }
559 
562 
563  return BVH_OK;
564 }
565 
567  const Vec3f& p3) {
569  std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. "
570  "replaceTriangle() was ignored. Must do a beginReplaceModel() "
571  "for initialization."
572  << std::endl;
574  }
575 
582  return BVH_OK;
583 }
584 
585 int BVHModelBase::replaceSubModel(const std::vector<Vec3f>& ps) {
587  std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. "
588  "replaceSubModel() was ignored. Must do a beginReplaceModel() "
589  "for initialization."
590  << std::endl;
592  }
593 
594  for (unsigned int i = 0; i < ps.size(); ++i) {
595  vertices[num_vertex_updated] = ps[i];
597  }
598  return BVH_OK;
599 }
600 
601 int BVHModelBase::endReplaceModel(bool refit, bool bottomup) {
603  std::cerr << "BVH Warning! Call endReplaceModel() in a wrong order. "
604  "endReplaceModel() was ignored. "
605  << std::endl;
607  }
608 
610  std::cerr << "BVH Error! The replaced model should have the same number of "
611  "vertices as the old model."
612  << std::endl;
613  return BVH_ERR_INCORRECT_DATA;
614  }
615 
616  if (refit) // refit, do not change BVH structure
617  {
618  refitTree(bottomup);
619  } else // reconstruct bvh tree based on current frame data
620  {
621  buildTree();
622  }
623 
625 
626  return BVH_OK;
627 }
628 
632  std::cerr << "BVH Error! Call beginUpdatemodel() on a BVHModel that has no "
633  "previous frame."
634  << std::endl;
636  }
637 
638  if (prev_vertices) {
639  Vec3f* temp = prev_vertices;
641  vertices = temp;
642  } else {
644  vertices = new Vec3f[num_vertices];
645  }
646 
647  num_vertex_updated = 0;
648 
650 
651  return BVH_OK;
652 }
653 
656  std::cerr
657  << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() "
658  "was ignored. Must do a beginUpdateModel() for initialization."
659  << std::endl;
661  }
662 
665 
666  return BVH_OK;
667 }
668 
670  const Vec3f& p3) {
672  std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. "
673  "updateTriangle() was ignored. Must do a beginUpdateModel() "
674  "for initialization."
675  << std::endl;
677  }
678 
685  return BVH_OK;
686 }
687 
688 int BVHModelBase::updateSubModel(const std::vector<Vec3f>& ps) {
690  std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. "
691  "updateSubModel() was ignored. Must do a beginUpdateModel() "
692  "for initialization."
693  << std::endl;
695  }
696 
697  for (unsigned int i = 0; i < ps.size(); ++i) {
698  vertices[num_vertex_updated] = ps[i];
700  }
701  return BVH_OK;
702 }
703 
704 int BVHModelBase::endUpdateModel(bool refit, bool bottomup) {
706  std::cerr << "BVH Warning! Call endUpdateModel() in a wrong order. "
707  "endUpdateModel() was ignored. "
708  << std::endl;
710  }
711 
713  std::cerr << "BVH Error! The updated model should have the same number of "
714  "vertices as the old model."
715  << std::endl;
716  return BVH_ERR_INCORRECT_DATA;
717  }
718 
719  if (refit) // refit, do not change BVH structure
720  {
721  refitTree(bottomup);
722  } else // reconstruct bvh tree based on current frame data
723  {
724  buildTree();
725 
726  // then refit
727 
728  refitTree(bottomup);
729  }
730 
732 
733  return BVH_OK;
734 }
735 
737  AABB aabb_;
738  for (unsigned int i = 0; i < num_vertices; ++i) {
739  aabb_ += vertices[i];
740  }
741 
742  aabb_center = aabb_.center();
743 
744  aabb_radius = 0;
745  for (unsigned int i = 0; i < num_vertices; ++i) {
746  FCL_REAL r = (aabb_center - vertices[i]).squaredNorm();
747  if (r > aabb_radius) aabb_radius = r;
748  }
749 
750  aabb_radius = sqrt(aabb_radius);
751 
752  aabb_local = aabb_;
753 }
754 
756 template <typename BV>
758  : BVHModelBase(),
760  bv_fitter(new BVFitter<BV>()),
762  primitive_indices(NULL),
763  bvs(NULL),
764  num_bvs(0) {}
765 
766 template <typename BV>
768  delete[] bvs;
769  bvs = NULL;
770  delete[] primitive_indices;
771  primitive_indices = NULL;
773 }
774 
775 template <typename BV>
777  // construct BVH tree
778  unsigned int num_bvs_to_be_allocated = 0;
779  if (num_tris == 0)
780  num_bvs_to_be_allocated = 2 * num_vertices - 1;
781  else
782  num_bvs_to_be_allocated = 2 * num_tris - 1;
783 
784  bvs = new BVNode<BV>[num_bvs_to_be_allocated];
785  primitive_indices = new unsigned int[num_bvs_to_be_allocated];
786  if (!bvs || !primitive_indices) {
787  std::cerr << "BVH Error! Out of memory for BV array in endModel()!"
788  << std::endl;
789  return false;
790  }
791  num_bvs_allocated = num_bvs_to_be_allocated;
792  num_bvs = 0;
793  return true;
794 }
795 
796 template <typename BV>
797 int BVHModel<BV>::memUsage(const bool msg) const {
798  unsigned int mem_bv_list = (unsigned int)sizeof(BV) * num_bvs;
799  unsigned int mem_tri_list = (unsigned int)sizeof(Triangle) * num_tris;
800  unsigned int mem_vertex_list = (unsigned int)sizeof(Vec3f) * num_vertices;
801 
802  unsigned int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list +
803  (unsigned int)sizeof(BVHModel<BV>);
804  if (msg) {
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;
809  }
810 
811  return static_cast<int>(total_mem);
812 }
813 
814 template <typename BV>
816  // set BVFitter
818  // set SplitRule
820 
821  num_bvs = 1;
822 
823  unsigned int num_primitives = 0;
824  switch (getModelType()) {
825  case BVH_MODEL_TRIANGLES:
826  num_primitives = (unsigned int)num_tris;
827  break;
829  num_primitives = (unsigned int)num_vertices;
830  break;
831  default:
832  std::cerr << "BVH Error: Model type not supported!" << std::endl;
834  }
835 
836  for (unsigned int i = 0; i < num_primitives; ++i) primitive_indices[i] = i;
837  recursiveBuildTree(0, 0, num_primitives);
838 
839  bv_fitter->clear();
840  bv_splitter->clear();
841 
842  return BVH_OK;
843 }
844 
845 template <typename BV>
846 int BVHModel<BV>::recursiveBuildTree(int bv_id, unsigned int first_primitive,
847  unsigned int num_primitives) {
849  BVNode<BV>* bvnode = bvs + bv_id;
850  unsigned int* cur_primitive_indices = primitive_indices + first_primitive;
851 
852  // constructing BV
853  BV bv = bv_fitter->fit(cur_primitive_indices, num_primitives);
854  bv_splitter->computeRule(bv, cur_primitive_indices, num_primitives);
855 
856  bvnode->bv = bv;
857  bvnode->first_primitive = first_primitive;
858  bvnode->num_primitives = num_primitives;
859 
860  if (num_primitives == 1) {
861  bvnode->first_child = -((int)(*cur_primitive_indices) + 1);
862  } else {
863  bvnode->first_child = (int)num_bvs;
864  num_bvs += 2;
865 
866  unsigned int c1 = 0;
867  for (unsigned int i = 0; i < num_primitives; ++i) {
868  Vec3f p;
869  if (type == BVH_MODEL_POINTCLOUD)
870  p = vertices[cur_primitive_indices[i]];
871  else if (type == BVH_MODEL_TRIANGLES) {
872  const Triangle& t = tri_indices[cur_primitive_indices[i]];
873  const Vec3f& p1 = vertices[t[0]];
874  const Vec3f& p2 = vertices[t[1]];
875  const Vec3f& p3 = vertices[t[2]];
876 
877  p = (p1 + p2 + p3) / 3.;
878  } else {
879  std::cerr << "BVH Error: Model type not supported!" << std::endl;
881  }
882 
883  // loop invariant: up to (but not including) index c1 in group 1,
884  // then up to (but not including) index i in group 2
885  //
886  // [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x]
887  // c1 i
888  //
889  if (bv_splitter->apply(p)) // in the right side
890  {
891  // do nothing
892  } else {
893  unsigned int temp = cur_primitive_indices[i];
894  cur_primitive_indices[i] = cur_primitive_indices[c1];
895  cur_primitive_indices[c1] = temp;
896  c1++;
897  }
898  }
899 
900  if ((c1 == 0) || (c1 == num_primitives)) c1 = num_primitives / 2;
901 
902  const unsigned int num_first_half = c1;
903 
904  recursiveBuildTree(bvnode->leftChild(), first_primitive, num_first_half);
905  recursiveBuildTree(bvnode->rightChild(), first_primitive + num_first_half,
906  num_primitives - num_first_half);
907  }
908 
909  return BVH_OK;
910 }
911 
912 template <typename BV>
913 int BVHModel<BV>::refitTree(bool bottomup) {
914  if (bottomup)
915  return refitTree_bottomup();
916  else
917  return refitTree_topdown();
918 }
919 
920 template <typename BV>
922  // TODO the recomputation of the BV is done manually, without using
923  // bv_fitter. The manual BV recomputation seems bugged. Using bv_fitter
924  // seems to correct the bug.
925  // bv_fitter->set(vertices, tri_indices, getModelType());
926 
928 
929  // bv_fitter->clear();
930  return res;
931 }
932 
933 template <typename BV>
935  BVNode<BV>* bvnode = bvs + bv_id;
936  if (bvnode->isLeaf()) {
938  int primitive_id = -(bvnode->first_child + 1);
939  if (type == BVH_MODEL_POINTCLOUD) {
940  BV bv;
941 
942  if (prev_vertices) {
943  Vec3f v[2];
944  v[0] = prev_vertices[primitive_id];
945  v[1] = vertices[primitive_id];
946  fit(v, 2, bv);
947  } else
948  fit(vertices + primitive_id, 1, bv);
949 
950  bvnode->bv = bv;
951  } else if (type == BVH_MODEL_TRIANGLES) {
952  BV bv;
953  const Triangle& triangle = tri_indices[primitive_id];
954 
955  if (prev_vertices) {
956  Vec3f v[6];
957  for (Triangle::index_type i = 0; i < 3; ++i) {
958  v[i] = prev_vertices[triangle[i]];
959  v[i + 3] = vertices[triangle[i]];
960  }
961 
962  fit(v, 6, bv);
963  } else {
964  // TODO use bv_fitter to build BV. See comment in refitTree_bottomup
965  // unsigned int* cur_primitive_indices = primitive_indices +
966  // bvnode->first_primitive; bv = bv_fitter->fit(cur_primitive_indices,
967  // bvnode->num_primitives);
968  Vec3f v[3];
969  for (int i = 0; i < 3; ++i) {
970  v[i] = vertices[triangle[(Triangle::index_type)i]];
971  }
972 
973  fit(v, 3, bv);
974  }
975 
976  bvnode->bv = bv;
977  } else {
978  std::cerr << "BVH Error: Model type not supported!" << std::endl;
980  }
981  } else {
984  bvnode->bv = bvs[bvnode->leftChild()].bv + bvs[bvnode->rightChild()].bv;
985  // TODO use bv_fitter to build BV. See comment in refitTree_bottomup
986  // unsigned int* cur_primitive_indices = primitive_indices +
987  // bvnode->first_primitive; bvnode->bv =
988  // bv_fitter->fit(cur_primitive_indices, bvnode->num_primitives);
989  }
990 
991  return BVH_OK;
992 }
993 
994 template <typename BV>
997  for (unsigned int i = 0; i < num_bvs; ++i) {
998  BV bv = bv_fitter->fit(primitive_indices + bvs[i].first_primitive,
999  bvs[i].num_primitives);
1000  bvs[i].bv = bv;
1001  }
1002 
1003  bv_fitter->clear();
1004 
1005  return BVH_OK;
1006 }
1007 
1008 template <>
1010  const Vec3f& parent_c) {
1011  OBB& obb = bvs[bv_id].bv;
1012  if (!bvs[bv_id].isLeaf()) {
1013  makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axes, obb.To);
1014 
1015  makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axes, obb.To);
1016  }
1017 
1018  // make self parent relative
1019  // obb.axes = parent_axes.transpose() * obb.axes;
1020  obb.axes.applyOnTheLeft(parent_axes.transpose());
1021 
1022  Vec3f t(obb.To - parent_c);
1023  obb.To.noalias() = parent_axes.transpose() * t;
1024 }
1025 
1026 template <>
1028  const Vec3f& parent_c) {
1029  RSS& rss = bvs[bv_id].bv;
1030  if (!bvs[bv_id].isLeaf()) {
1031  makeParentRelativeRecurse(bvs[bv_id].first_child, rss.axes, rss.Tr);
1032 
1033  makeParentRelativeRecurse(bvs[bv_id].first_child + 1, rss.axes, rss.Tr);
1034  }
1035 
1036  // make self parent relative
1037  // rss.axes = parent_axes.transpose() * rss.axes;
1038  rss.axes.applyOnTheLeft(parent_axes.transpose());
1039 
1040  Vec3f t(rss.Tr - parent_c);
1041  rss.Tr.noalias() = parent_axes.transpose() * t;
1042 }
1043 
1044 template <>
1046  Matrix3f& parent_axes,
1047  const Vec3f& parent_c) {
1048  OBB& obb = bvs[bv_id].bv.obb;
1049  RSS& rss = bvs[bv_id].bv.rss;
1050  if (!bvs[bv_id].isLeaf()) {
1051  makeParentRelativeRecurse(bvs[bv_id].first_child, obb.axes, obb.To);
1052 
1053  makeParentRelativeRecurse(bvs[bv_id].first_child + 1, obb.axes, obb.To);
1054  }
1055 
1056  // make self parent relative
1057  rss.axes.noalias() = parent_axes.transpose() * obb.axes;
1058  obb.axes = rss.axes;
1059 
1060  Vec3f t(obb.To - parent_c);
1061  obb.To.noalias() = parent_axes.transpose() * t;
1062  rss.Tr = obb.To;
1063 }
1064 
1065 template <>
1067  return BV_AABB;
1068 }
1069 
1070 template <>
1072  return BV_OBB;
1073 }
1074 
1075 template <>
1077  return BV_RSS;
1078 }
1079 
1080 template <>
1082  return BV_kIOS;
1083 }
1084 
1085 template <>
1087  return BV_OBBRSS;
1088 }
1089 
1090 template <>
1092  return BV_KDOP16;
1093 }
1094 
1095 template <>
1097  return BV_KDOP18;
1098 }
1099 
1100 template <>
1102  return BV_KDOP24;
1103 }
1104 
1105 template class BVHModel<KDOP<16> >;
1106 template class BVHModel<KDOP<18> >;
1107 template class BVHModel<KDOP<24> >;
1108 template class BVHModel<OBB>;
1109 template class BVHModel<AABB>;
1110 template class BVHModel<RSS>;
1111 template class BVHModel<kIOS>;
1112 template class BVHModel<OBBRSS>;
1113 
1114 } // namespace fcl
1115 
1116 } // namespace hpp
int updateSubModel(const std::vector< Vec3f > &ps)
Update a set of points in the old BVH model.
Definition: BVH_model.cpp:688
Vec3f * prev_vertices
Geometry point data in previous frame.
Definition: BVH/BVH_model.h:72
shared_ptr< ConvexBase > convex
Convex<Triangle> representation of this object.
Definition: BVH/BVH_model.h:84
int addTriangles(const Matrixx3i &triangles)
Add triangles in the new BVH model.
Definition: BVH_model.cpp:251
unsigned int first_primitive
The start id the primitive belonging to the current node. The index is referred to the primitive_indi...
Definition: BV/BV_node.h:64
unsigned int num_vertex_updated
int replaceSubModel(const std::vector< Vec3f > &ps)
Replace a set of points in the old BVH model.
Definition: BVH_model.cpp:585
int buildTree()
Build the bounding volume hierarchy.
Definition: BVH_model.cpp:815
int refitTree_topdown()
Refit the bounding volume hierarchy in a top-down way (slow but more compact)
Definition: BVH_model.cpp:995
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...
tuple p1
Definition: octree.py:55
Vec3f center() const
Center of the AABB.
Definition: BV/AABB.h:157
Main namespace.
t
after tree has been build, ready for cd use
Definition: BVH_internal.h:55
Cannot allocate memory for vertices and triangles.
Definition: BVH_internal.h:68
BVH construction does not follow correct sequence.
Definition: BVH_internal.h:70
BV bv
bounding volume storing the geometry
Definition: BV/BV_node.h:113
Matrix3f axes
Orientation of RSS. axis[i] is the ith column of the orientation matrix for the RSS; it is also the i...
Definition: BV/RSS.h:59
int addTriangle(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3)
Add one triangle in the new BVH model.
Definition: BVH_model.cpp:317
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
Definition: data_types.h:68
int addSubModel(const std::vector< Vec3f > &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
Definition: BVH_model.cpp:410
int addVertices(const Matrixx3f &points)
Add points in the new BVH model.
Definition: BVH_model.cpp:287
int endUpdateModel(bool refit=true, bool bottomup=true)
End BVH model update, will also refit or rebuild the bounding volume hierarchy.
Definition: BVH_model.cpp:704
int updateVertex(const Vec3f &p)
Update one point in the old BVH model.
Definition: BVH_model.cpp:654
A base class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewe...
Definition: BVH/BVH_model.h:63
int addVertex(const Vec3f &p)
Add one point in the new BVH model.
Definition: BVH_model.cpp:221
unsigned int num_vertices
Number of points.
Definition: BVH/BVH_model.h:78
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...
Definition: BV/BV_node.h:59
int replaceTriangle(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3)
Replace one triangle in the old BVH model.
Definition: BVH_model.cpp:566
int recursiveRefitTree_bottomup(int bv_id)
Recursive kernel for bottomup refitting.
Definition: BVH_model.cpp:934
list v
Definition: obb.py:45
unknown model type
Definition: BVH_internal.h:82
std::size_t index_type
Definition: data_types.h:98
void fit(Vec3f *ps, unsigned int n, BV &bv)
Compute a bounding volume that fits a set of n points.
Definition: BV_fitter.h:52
bool buildConvexHull(bool keepTriangle, const char *qhullCommand=NULL)
Build a convex hull and store it in attribute convex.
Definition: BVH_model.cpp:132
void makeParentRelativeRecurse(int bv_id, Matrix3f &parent_axes, const Vec3f &parent_c)
int refitTree(bool bottomup)
Refit the bounding volume hierarchy.
Definition: BVH_model.cpp:913
unsigned int * primitive_indices
Triangle * tri_indices
Geometry triangle index data, will be NULL for point clouds.
Definition: BVH/BVH_model.h:69
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.
Definition: BVH_model.cpp:551
Vec3f * vertices
Geometry point data.
Definition: BVH/BVH_model.h:66
unsigned int num_tris
Number of triangles.
Definition: BVH/BVH_model.h:75
Vec3f Tr
Origin of the rectangle in RSS.
Definition: BV/RSS.h:62
double FCL_REAL
Definition: data_types.h:65
void buildConvexRepresentation(bool share_memory)
Build this Convex<Triangle> representation of this model. The result is stored in attribute convex...
Definition: BVH_model.cpp:116
int beginModel(unsigned int num_tris=0, unsigned int num_vertices=0)
Begin a new BVH model.
Definition: BVH_model.cpp:170
int recursiveBuildTree(int bv_id, unsigned int first_primitive, unsigned int num_primitives)
Recursive kernel for hierarchy construction.
Definition: BVH_model.cpp:846
Eigen::Matrix< Eigen::DenseIndex, Eigen::Dynamic, 3 > Matrixx3i
Definition: data_types.h:70
int beginReplaceModel()
Replace the geometry information of current frame (i.e. should have the same mesh topology with the p...
Definition: BVH_model.cpp:533
BVHModelType getModelType() const
Model type described by the instance.
Definition: BVH/BVH_model.h:87
A class for rectangle sphere-swept bounding volume.
Definition: BV/RSS.h:53
int updateTriangle(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3)
Update one triangle in the old BVH model.
Definition: BVH_model.cpp:669
unsigned int num_tris_allocated
Eigen::Matrix< FCL_REAL, Eigen::Dynamic, 3 > Matrixx3f
Definition: data_types.h:69
BVHModelType
BVH model type.
Definition: BVH_internal.h:80
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...
Definition: BV/BV_node.h:109
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...
Definition: BV/AABB.h:54
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...
Definition: BV/BV_node.h:98
int endReplaceModel(bool refit=true, bool bottomup=true)
End BVH model replacement, will also refit or rebuild the bounding volume hierarchy.
Definition: BVH_model.cpp:601
virtual bool isEqual(const CollisionGeometry &other) const
for ccd vertex update
Definition: BVH_model.cpp:91
void set(index_type p1, index_type p2, index_type p3)
Set the vertex indices of the triangle.
Definition: data_types.h:108
Triangle with 3 indices for points.
Definition: data_types.h:96
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.
res
Definition: obb.py:1
NODE_TYPE getNodeType() const
Get the BV type: default is unknown.
int memUsage(const bool msg) const
Check the number of memory used.
Definition: BVH_model.cpp:797
bool isLeaf() const
Whether current node is a leaf node (i.e. contains a primitive index.
Definition: BV/BV_node.h:90
int refitTree_bottomup()
Refit the bounding volume hierarchy in a bottom-up way (fast but less compact)
Definition: BVH_model.cpp:921
Vec3f To
Center of OBB.
BVHBuildState build_state
The state of BVH building process.
Definition: BVH/BVH_model.h:81
BVNode< BV > * bvs
Bounding volume hierarchy.
unsigned int num_primitives
The number of primitives belonging to the current node.
Definition: BV/BV_node.h:67
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. ...
Definition: BVH/BVH_model.h:57
A class describing the split rule that splits each BV node.
Definition: BVH/BVH_model.h:59
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
int rightChild() const
Return the index of the second child. The index is referred to the bounding volume array (i...
Definition: BV/BV_node.h:102
Convex polytope.
Definition: shape/convex.h:50
BVH geometry in previous frame is not prepared.
Definition: BVH_internal.h:73
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.
Definition: BVH_model.cpp:757
empty state, immediately after constructor
Definition: BVH_internal.h:52
int beginUpdateModel()
Replace the geometry information of current frame (i.e. should have the same mesh topology with the p...
Definition: BVH_model.cpp:629
BVH model update failed.
Definition: BVH_internal.h:75
void computeLocalAABB()
Compute the AABB for the BVH, used for broad-phase collision.
Definition: BVH_model.cpp:736
BVH geometry is not prepared.
Definition: BVH_internal.h:71
BVHModelBase()
Constructing an empty BVH.
Definition: BVH_model.cpp:53
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
Definition: BVH_model.cpp:473
Oriented bounding box class.


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:00