FCL  0.6.0
Flexible Collision Library
BVH_model-inl.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef FCL_BVH_MODEL_INL_H
39 #define FCL_BVH_MODEL_INL_H
40 
41 #include "fcl/geometry/bvh/BVH_model.h"
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 template <typename BV>
49 {
50  if(num_tris && num_vertices)
51  return BVH_MODEL_TRIANGLES;
52  else if(num_vertices)
53  return BVH_MODEL_POINTCLOUD;
54  else
55  return BVH_MODEL_UNKNOWN;
56 }
57 
58 //==============================================================================
59 template <typename BV>
60 BVHModel<BV>::BVHModel() : vertices(nullptr),
61  tri_indices(nullptr),
62  prev_vertices(nullptr),
63  num_tris(0),
64  num_vertices(0),
65  build_state(BVH_BUILD_STATE_EMPTY),
66  bv_splitter(new detail::BVSplitter<BV>(detail::SPLIT_METHOD_MEAN)),
67  bv_fitter(new detail::BVFitter<BV>()),
68  num_tris_allocated(0),
69  num_vertices_allocated(0),
70  num_bvs_allocated(0),
71  num_vertex_updated(0),
72  primitive_indices(nullptr),
73  bvs(nullptr),
74  num_bvs(0)
75 {
76  // Do nothing
77 }
78 
79 //==============================================================================
80 template <typename BV>
82  : CollisionGeometry<S>(other),
83  num_tris(other.num_tris),
85  build_state(other.build_state),
86  bv_splitter(other.bv_splitter),
87  bv_fitter(other.bv_fitter),
88  num_tris_allocated(other.num_tris),
89  num_vertices_allocated(other.num_vertices)
90 {
91  if(other.vertices)
92  {
93  vertices = new Vector3<S>[num_vertices];
94  memcpy(vertices, other.vertices, sizeof(Vector3<S>) * num_vertices);
95  }
96  else
97  vertices = nullptr;
98 
99  if(other.tri_indices)
100  {
102  memcpy(tri_indices, other.tri_indices, sizeof(Triangle) * num_tris);
103  }
104  else
105  tri_indices = nullptr;
106 
107  if(other.prev_vertices)
108  {
109  prev_vertices = new Vector3<S>[num_vertices];
110  memcpy(prev_vertices, other.prev_vertices, sizeof(Vector3<S>) * num_vertices);
111  }
112  else
113  prev_vertices = nullptr;
114 
115  if(other.primitive_indices)
116  {
117  int num_primitives = 0;
118  switch(other.getModelType())
119  {
120  case BVH_MODEL_TRIANGLES:
121  num_primitives = num_tris;
122  break;
124  num_primitives = num_vertices;
125  break;
126  default:
127  ;
128  }
129 
130  primitive_indices = new unsigned int[num_primitives];
131  memcpy(primitive_indices, other.primitive_indices, sizeof(unsigned int) * num_primitives);
132  }
133  else
134  primitive_indices = nullptr;
135 
136  num_bvs = num_bvs_allocated = other.num_bvs;
137  if(other.bvs)
138  {
139  bvs = new BVNode<BV>[num_bvs];
140  memcpy(bvs, other.bvs, sizeof(BVNode<BV>) * num_bvs);
141  }
142  else
143  bvs = nullptr;
144 }
145 
146 //==============================================================================
147 template <typename BV>
149 {
150  delete [] vertices;
151  delete [] tri_indices;
152  delete [] bvs;
153 
154  delete [] prev_vertices;
155  delete [] primitive_indices;
156 }
157 
158 //==============================================================================
159 template <typename BV>
160 const BVNode<BV>& BVHModel<BV>::getBV(int id) const
161 {
162  return bvs[id];
163 }
164 
165 //==============================================================================
166 template <typename BV>
168 {
169  return bvs[id];
170 }
171 
172 //==============================================================================
173 template <typename BV>
175 {
176  return num_bvs;
177 }
178 
179 //==============================================================================
180 template <typename BV>
182 {
183  return OT_BVH;
184 }
185 
186 //==============================================================================
187 template <typename BV>
189 {
190  static NODE_TYPE run()
191  {
192  return BV_UNKNOWN;
193  }
194 };
195 
196 //==============================================================================
197 template <typename BV>
199 {
200  return GetNodeTypeImpl<BV>::run();
201 }
202 
203 //==============================================================================
204 template <typename BV>
205 int BVHModel<BV>::beginModel(int num_tris_, int num_vertices_)
206 {
207  if(build_state != BVH_BUILD_STATE_EMPTY)
208  {
209  delete [] vertices; vertices = nullptr;
210  delete [] tri_indices; tri_indices = nullptr;
211  delete [] bvs; bvs = nullptr;
212  delete [] prev_vertices; prev_vertices = nullptr;
213  delete [] primitive_indices; primitive_indices = nullptr;
214 
215  num_vertices_allocated = num_vertices = num_tris_allocated = num_tris = num_bvs_allocated = num_bvs = 0;
216  }
217 
218  if(num_tris_ <= 0) num_tris_ = 8;
219  if(num_vertices_ <= 0) num_vertices_ = 8;
220 
221  num_vertices_allocated = num_vertices_;
222  num_tris_allocated = num_tris_;
223 
224  tri_indices = new Triangle[num_tris_allocated];
225  vertices = new Vector3<S>[num_vertices_allocated];
226 
227  if(!tri_indices)
228  {
229  std::cerr << "BVH Error! Out of memory for tri_indices array on BeginModel() call!" << std::endl;
231  }
232  if(!vertices)
233  {
234  std::cerr << "BVH Error! Out of memory for vertices array on BeginModel() call!" << std::endl;
236  }
237 
238  if(build_state != BVH_BUILD_STATE_EMPTY)
239  {
240  std::cerr << "BVH Warning! Call beginModel() on a BVHModel that is not empty. This model was cleared and previous triangles/vertices were lost." << std::endl;
241  build_state = BVH_BUILD_STATE_EMPTY;
243  }
244 
246 
247  return BVH_OK;
248 }
249 
250 //==============================================================================
251 template <typename BV>
252 int BVHModel<BV>::addVertex(const Vector3<S>& p)
253 {
255  {
256  std::cerr << "BVH Warning! Call addVertex() in a wrong order. addVertex() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl;
258  }
259 
260  if(num_vertices >= num_vertices_allocated)
261  {
262  Vector3<S>* temp = new Vector3<S>[num_vertices_allocated * 2];
263  if(!temp)
264  {
265  std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" << std::endl;
267  }
268 
269  memcpy(temp, vertices, sizeof(Vector3<S>) * num_vertices);
270  delete [] vertices;
271  vertices = temp;
272  num_vertices_allocated *= 2;
273  }
274 
275  vertices[num_vertices] = p;
276  num_vertices += 1;
277 
278  return BVH_OK;
279 }
280 
281 //==============================================================================
282 template <typename BV>
283 int BVHModel<BV>::addTriangle(const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3)
284 {
286  {
287  std::cerr << "BVH Warning! Call addTriangle() in a wrong order. addTriangle() was ignored. Must do a beginModel() to clear the model for addition of new triangles." << std::endl;
289  }
290 
291  if(num_vertices + 2 >= num_vertices_allocated)
292  {
293  Vector3<S>* temp = new Vector3<S>[num_vertices_allocated * 2 + 2];
294  if(!temp)
295  {
296  std::cerr << "BVH Error! Out of memory for vertices array on addTriangle() call!" << std::endl;
298  }
299 
300  memcpy(temp, vertices, sizeof(Vector3<S>) * num_vertices);
301  delete [] vertices;
302  vertices = temp;
303  num_vertices_allocated = num_vertices_allocated * 2 + 2;
304  }
305 
306  int offset = num_vertices;
307 
308  vertices[num_vertices] = p1;
309  num_vertices++;
310  vertices[num_vertices] = p2;
311  num_vertices++;
312  vertices[num_vertices] = p3;
313  num_vertices++;
314 
315  if(num_tris >= num_tris_allocated)
316  {
317  Triangle* temp = new Triangle[num_tris_allocated * 2];
318  if(!temp)
319  {
320  std::cerr << "BVH Error! Out of memory for tri_indices array on addTriangle() call!" << std::endl;
322  }
323 
324  memcpy(temp, tri_indices, sizeof(Triangle) * num_tris);
325  delete [] tri_indices;
326  tri_indices = temp;
327  num_tris_allocated *= 2;
328  }
329 
330  tri_indices[num_tris].set(offset, offset + 1, offset + 2);
331  num_tris++;
332 
333  return BVH_OK;
334 }
335 
336 //==============================================================================
337 template <typename BV>
338 int BVHModel<BV>::addSubModel(const std::vector<Vector3<S>>& ps)
339 {
341  {
342  std::cerr << "BVH Warning! Call addSubModel() in a wrong order. addSubModel() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl;
344  }
345 
346  int num_vertices_to_add = ps.size();
347 
348  if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated)
349  {
350  Vector3<S>* temp = new Vector3<S>[num_vertices_allocated * 2 + num_vertices_to_add - 1];
351  if(!temp)
352  {
353  std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl;
355  }
356 
357  memcpy(temp, vertices, sizeof(Vector3<S>) * num_vertices);
358  delete [] vertices;
359  vertices = temp;
360  num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1;
361  }
362 
363  for(int i = 0; i < num_vertices_to_add; ++i)
364  {
365  vertices[num_vertices] = ps[i];
366  num_vertices++;
367  }
368 
369  return BVH_OK;
370 }
371 
372 //==============================================================================
373 template <typename BV>
374 int BVHModel<BV>::addSubModel(const std::vector<Vector3<S>>& ps, const std::vector<Triangle>& ts)
375 {
377  {
378  std::cerr << "BVH Warning! Call addSubModel() in a wrong order. addSubModel() was ignored. Must do a beginModel() to clear the model for addition of new vertices." << std::endl;
380  }
381 
382  int num_vertices_to_add = ps.size();
383 
384  if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated)
385  {
386  Vector3<S>* temp = new Vector3<S>[num_vertices_allocated * 2 + num_vertices_to_add - 1];
387  if(!temp)
388  {
389  std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl;
391  }
392 
393  memcpy(temp, vertices, sizeof(Vector3<S>) * num_vertices);
394  delete [] vertices;
395  vertices = temp;
396  num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1;
397  }
398 
399  int offset = num_vertices;
400 
401  for(int i = 0; i < num_vertices_to_add; ++i)
402  {
403  vertices[num_vertices] = ps[i];
404  num_vertices++;
405  }
406 
407 
408  int num_tris_to_add = ts.size();
409 
410  if(num_tris + num_tris_to_add - 1 >= num_tris_allocated)
411  {
412  Triangle* temp = new Triangle[num_tris_allocated * 2 + num_tris_to_add - 1];
413  if(!temp)
414  {
415  std::cerr << "BVH Error! Out of memory for tri_indices array on addSubModel() call!" << std::endl;
417  }
418 
419  memcpy(temp, tri_indices, sizeof(Triangle) * num_tris);
420  delete [] tri_indices;
421  tri_indices = temp;
422  num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add - 1;
423  }
424 
425  for(int i = 0; i < num_tris_to_add; ++i)
426  {
427  const Triangle& t = ts[i];
428  tri_indices[num_tris].set(t[0] + offset, t[1] + offset, t[2] + offset);
429  num_tris++;
430  }
431 
432  return BVH_OK;
433 }
434 
435 //==============================================================================
436 template <typename BV>
438 {
440  {
441  std::cerr << "BVH Warning! Call endModel() in wrong order. endModel() was ignored." << std::endl;
443  }
444 
445  if(num_tris == 0 && num_vertices == 0)
446  {
447  std::cerr << "BVH Error! endModel() called on model with no triangles and vertices." << std::endl;
449  }
450 
451  if(num_tris_allocated > num_tris)
452  {
453  Triangle* new_tris = new Triangle[num_tris];
454  if(!new_tris)
455  {
456  std::cerr << "BVH Error! Out of memory for tri_indices array in endModel() call!" << std::endl;
458  }
459  memcpy(new_tris, tri_indices, sizeof(Triangle) * num_tris);
460  delete [] tri_indices;
461  tri_indices = new_tris;
462  num_tris_allocated = num_tris;
463  }
464 
465  if(num_vertices_allocated > num_vertices)
466  {
467  Vector3<S>* new_vertices = new Vector3<S>[num_vertices];
468  if(!new_vertices)
469  {
470  std::cerr << "BVH Error! Out of memory for vertices array in endModel() call!" << std::endl;
472  }
473  memcpy(new_vertices, vertices, sizeof(Vector3<S>) * num_vertices);
474  delete [] vertices;
475  vertices = new_vertices;
476  num_vertices_allocated = num_vertices;
477  }
478 
479 
480  // construct BVH tree
481  int num_bvs_to_be_allocated = 0;
482  if(num_tris == 0)
483  num_bvs_to_be_allocated = 2 * num_vertices - 1;
484  else
485  num_bvs_to_be_allocated = 2 * num_tris - 1;
486 
487 
488  bvs = new BVNode<BV> [num_bvs_to_be_allocated];
489  primitive_indices = new unsigned int [num_bvs_to_be_allocated];
490  if(!bvs || !primitive_indices)
491  {
492  std::cerr << "BVH Error! Out of memory for BV array in endModel()!" << std::endl;
494  }
495  num_bvs_allocated = num_bvs_to_be_allocated;
496  num_bvs = 0;
497 
498  buildTree();
499 
500  // finish constructing
502 
503  return BVH_OK;
504 }
505 
506 //==============================================================================
507 template <typename BV>
509 {
511  {
512  std::cerr << "BVH Error! Call beginReplaceModel() on a BVHModel that has no previous frame." << std::endl;
514  }
515 
516  if(prev_vertices) delete [] prev_vertices; prev_vertices = nullptr;
517 
518  num_vertex_updated = 0;
519 
521 
522  return BVH_OK;
523 }
524 
525 //==============================================================================
526 template <typename BV>
527 int BVHModel<BV>::replaceVertex(const Vector3<S>& p)
528 {
530  {
531  std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. replaceVertex() was ignored. Must do a beginReplaceModel() for initialization." << std::endl;
533  }
534 
535  vertices[num_vertex_updated] = p;
536  num_vertex_updated++;
537 
538  return BVH_OK;
539 }
540 
541 //==============================================================================
542 template <typename BV>
543 int BVHModel<BV>::replaceTriangle(const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3)
544 {
546  {
547  std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. replaceTriangle() was ignored. Must do a beginReplaceModel() for initialization." << std::endl;
549  }
550 
551  vertices[num_vertex_updated] = p1; num_vertex_updated++;
552  vertices[num_vertex_updated] = p2; num_vertex_updated++;
553  vertices[num_vertex_updated] = p3; num_vertex_updated++;
554  return BVH_OK;
555 }
556 
557 //==============================================================================
558 template <typename BV>
559 int BVHModel<BV>::replaceSubModel(const std::vector<Vector3<S>>& ps)
560 {
562  {
563  std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. replaceSubModel() was ignored. Must do a beginReplaceModel() for initialization." << std::endl;
565  }
566 
567  for(unsigned int i = 0; i < ps.size(); ++i)
568  {
569  vertices[num_vertex_updated] = ps[i];
570  num_vertex_updated++;
571  }
572  return BVH_OK;
573 }
574 
575 //==============================================================================
576 template <typename BV>
577 int BVHModel<BV>::endReplaceModel(bool refit, bool bottomup)
578 {
580  {
581  std::cerr << "BVH Warning! Call endReplaceModel() in a wrong order. endReplaceModel() was ignored. " << std::endl;
583  }
584 
585  if(num_vertex_updated != num_vertices)
586  {
587  std::cerr << "BVH Error! The replaced model should have the same number of vertices as the old model." << std::endl;
588  return BVH_ERR_INCORRECT_DATA;
589  }
590 
591  if(refit) // refit, do not change BVH structure
592  {
593  refitTree(bottomup);
594  }
595  else // reconstruct bvh tree based on current frame data
596  {
597  buildTree();
598  }
599 
601 
602  return BVH_OK;
603 }
604 
605 //==============================================================================
606 template <typename BV>
608 {
610  {
611  std::cerr << "BVH Error! Call beginUpdatemodel() on a BVHModel that has no previous frame." << std::endl;
613  }
614 
615  if(prev_vertices)
616  {
617  Vector3<S>* temp = prev_vertices;
619  vertices = temp;
620  }
621  else
622  {
624  vertices = new Vector3<S>[num_vertices];
625  }
626 
627  num_vertex_updated = 0;
628 
630 
631  return BVH_OK;
632 }
633 
634 //==============================================================================
635 template <typename BV>
636 int BVHModel<BV>::updateVertex(const Vector3<S>& p)
637 {
639  {
640  std::cerr << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() was ignored. Must do a beginUpdateModel() for initialization." << std::endl;
642  }
643 
644  vertices[num_vertex_updated] = p;
645  num_vertex_updated++;
646 
647  return BVH_OK;
648 }
649 
650 //==============================================================================
651 template <typename BV>
652 int BVHModel<BV>::updateTriangle(const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3)
653 {
655  {
656  std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. updateTriangle() was ignored. Must do a beginUpdateModel() for initialization." << std::endl;
658  }
659 
660  vertices[num_vertex_updated] = p1; num_vertex_updated++;
661  vertices[num_vertex_updated] = p2; num_vertex_updated++;
662  vertices[num_vertex_updated] = p3; num_vertex_updated++;
663  return BVH_OK;
664 }
665 
666 //==============================================================================
667 template <typename BV>
668 int BVHModel<BV>::updateSubModel(const std::vector<Vector3<S>>& ps)
669 {
671  {
672  std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. updateSubModel() was ignored. Must do a beginUpdateModel() for initialization." << std::endl;
674  }
675 
676  for(unsigned int i = 0; i < ps.size(); ++i)
677  {
678  vertices[num_vertex_updated] = ps[i];
679  num_vertex_updated++;
680  }
681  return BVH_OK;
682 }
683 
684 //==============================================================================
685 template <typename BV>
686 int BVHModel<BV>::endUpdateModel(bool refit, bool bottomup)
687 {
689  {
690  std::cerr << "BVH Warning! Call endUpdateModel() in a wrong order. endUpdateModel() was ignored. " << std::endl;
692  }
693 
694  if(num_vertex_updated != num_vertices)
695  {
696  std::cerr << "BVH Error! The updated model should have the same number of vertices as the old model." << std::endl;
697  return BVH_ERR_INCORRECT_DATA;
698  }
699 
700  if(refit) // refit, do not change BVH structure
701  {
702  refitTree(bottomup);
703  }
704  else // reconstruct bvh tree based on current frame data
705  {
706  buildTree();
707 
708  // then refit
709 
710  refitTree(bottomup);
711  }
712 
713 
715 
716  return BVH_OK;
717 }
718 
719 //==============================================================================
720 template <typename BV>
721 int BVHModel<BV>::memUsage(int msg) const
722 {
723  int mem_bv_list = sizeof(BV) * num_bvs;
724  int mem_tri_list = sizeof(Triangle) * num_tris;
725  int mem_vertex_list = sizeof(Vector3<S>) * num_vertices;
726 
727  int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list + sizeof(BVHModel<BV>);
728  if(msg)
729  {
730  std::cerr << "Total for model " << total_mem << " bytes." << std::endl;
731  std::cerr << "BVs: " << num_bvs << " allocated." << std::endl;
732  std::cerr << "Tris: " << num_tris << " allocated." << std::endl;
733  std::cerr << "Vertices: " << num_vertices << " allocated." << std::endl;
734  }
735 
736  return BVH_OK;
737 }
738 
739 //==============================================================================
740 template <typename BV>
742 {
743  makeParentRelativeRecurse(
744  0, Matrix3<S>::Identity(), Vector3<S>::Zero());
745 }
746 
747 //==============================================================================
748 template <typename BV>
749 Vector3<typename BV::S> BVHModel<BV>::computeCOM() const
750 {
751  S vol = 0;
752  Vector3<S> com = Vector3<S>::Zero();
753  for(int i = 0; i < num_tris; ++i)
754  {
755  const Triangle& tri = tri_indices[i];
756  S d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
757  vol += d_six_vol;
758  com.noalias() += (vertices[tri[0]] + vertices[tri[1]] + vertices[tri[2]]) * d_six_vol;
759  }
760 
761  return com / (vol * 4);
762 }
763 
764 //==============================================================================
765 template <typename BV>
766 typename BV::S BVHModel<BV>::computeVolume() const
767 {
768  S vol = 0;
769  for(int i = 0; i < num_tris; ++i)
770  {
771  const Triangle& tri = tri_indices[i];
772  S d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
773  vol += d_six_vol;
774  }
775 
776  return vol / 6;
777 }
778 
779 //==============================================================================
780 template <typename BV>
781 Matrix3<typename BV::S> BVHModel<BV>::computeMomentofInertia() const
782 {
783  Matrix3<S> C = Matrix3<S>::Zero();
784 
785  Matrix3<S> C_canonical;
786  C_canonical << 1/ 60.0, 1/120.0, 1/120.0,
787  1/120.0, 1/ 60.0, 1/120.0,
788  1/120.0, 1/120.0, 1/ 60.0;
789 
790  for(int i = 0; i < num_tris; ++i)
791  {
792  const Triangle& tri = tri_indices[i];
793  const Vector3<S>& v1 = vertices[tri[0]];
794  const Vector3<S>& v2 = vertices[tri[1]];
795  const Vector3<S>& v3 = vertices[tri[2]];
796  S d_six_vol = (v1.cross(v2)).dot(v3);
797  Matrix3<S> A;
798  A.row(0) = v1;
799  A.row(1) = v2;
800  A.row(2) = v3;
801  C.noalias() += A.transpose() * C_canonical * A * d_six_vol;
802  }
803 
804  S trace_C = C(0, 0) + C(1, 1) + C(2, 2);
805 
806  Matrix3<S> m;
807  m << trace_C - C(0, 0), -C(0, 1), -C(0, 2),
808  -C(1, 0), trace_C - C(1, 1), -C(1, 2),
809  -C(2, 0), -C(2, 1), trace_C - C(2, 2);
810 
811  return m;
812 }
813 
814 //==============================================================================
815 template <typename BV>
817 {
818  // set BVFitter
820  // set SplitRule
822 
823  num_bvs = 1;
824 
825  int num_primitives = 0;
826  switch(getModelType())
827  {
828  case BVH_MODEL_TRIANGLES:
829  num_primitives = num_tris;
830  break;
832  num_primitives = num_vertices;
833  break;
834  default:
835  std::cerr << "BVH Error: Model type not supported!" << std::endl;
837  }
838 
839  for(int i = 0; i < num_primitives; ++i)
840  primitive_indices[i] = i;
841  recursiveBuildTree(0, 0, num_primitives);
842 
843  bv_fitter->clear();
844  bv_splitter->clear();
845 
846  return BVH_OK;
847 }
848 
849 //==============================================================================
850 template <typename BV>
851 int BVHModel<BV>::recursiveBuildTree(int bv_id, int first_primitive, int num_primitives)
852 {
853  BVHModelType type = getModelType();
854  BVNode<BV>* bvnode = bvs + bv_id;
855  unsigned int* cur_primitive_indices = primitive_indices + first_primitive;
856 
857  // constructing BV
858  BV bv = bv_fitter->fit(cur_primitive_indices, num_primitives);
859  bv_splitter->computeRule(bv, cur_primitive_indices, num_primitives);
860 
861  bvnode->bv = bv;
862  bvnode->first_primitive = first_primitive;
863  bvnode->num_primitives = num_primitives;
864 
865  if(num_primitives == 1)
866  {
867  bvnode->first_child = -((*cur_primitive_indices) + 1);
868  }
869  else
870  {
871  bvnode->first_child = num_bvs;
872  num_bvs += 2;
873 
874  int c1 = 0;
875  for(int i = 0; i < num_primitives; ++i)
876  {
877  Vector3<S> p;
878  if(type == BVH_MODEL_POINTCLOUD) p = vertices[cur_primitive_indices[i]];
879  else if(type == BVH_MODEL_TRIANGLES)
880  {
881  const Triangle& t = tri_indices[cur_primitive_indices[i]];
882  const Vector3<S>& p1 = vertices[t[0]];
883  const Vector3<S>& p2 = vertices[t[1]];
884  const Vector3<S>& p3 = vertices[t[2]];
885  p.noalias() = (p1 + p2 + p3) / 3.0;
886  }
887  else
888  {
889  std::cerr << "BVH Error: Model type not supported!" << std::endl;
891  }
892 
893 
894  // loop invariant: up to (but not including) index c1 in group 1,
895  // then up to (but not including) index i in group 2
896  //
897  // [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x]
898  // c1 i
899  //
900  if(bv_splitter->apply(p)) // in the right side
901  {
902  // do nothing
903  }
904  else
905  {
906  std::swap(cur_primitive_indices[i], cur_primitive_indices[c1]);
907  c1++;
908  }
909  }
910 
911 
912  if((c1 == 0) || (c1 == num_primitives)) c1 = num_primitives / 2;
913 
914  int num_first_half = c1;
915 
916  recursiveBuildTree(bvnode->leftChild(), first_primitive, num_first_half);
917  recursiveBuildTree(bvnode->rightChild(), first_primitive + num_first_half, num_primitives - num_first_half);
918  }
919 
920  return BVH_OK;
921 }
922 
923 //==============================================================================
924 template <typename BV>
925 int BVHModel<BV>::refitTree(bool bottomup)
926 {
927  if(bottomup)
928  return refitTree_bottomup();
929  else
930  return refitTree_topdown();
931 }
932 
933 //==============================================================================
934 template <typename BV>
936 {
937  int res = recursiveRefitTree_bottomup(0);
938 
939  return res;
940 }
941 
942 //==============================================================================
943 template <typename BV>
945 {
946  BVNode<BV>* bvnode = bvs + bv_id;
947  if(bvnode->isLeaf())
948  {
949  BVHModelType type = getModelType();
950  int primitive_id = -(bvnode->first_child + 1);
951  if(type == BVH_MODEL_POINTCLOUD)
952  {
953  BV bv;
954 
955  if(prev_vertices)
956  {
957  Vector3<S> v[2];
958  v[0] = prev_vertices[primitive_id];
959  v[1] = vertices[primitive_id];
960  fit(v, 2, bv);
961  }
962  else
963  fit(vertices + primitive_id, 1, bv);
964 
965  bvnode->bv = bv;
966  }
967  else if(type == BVH_MODEL_TRIANGLES)
968  {
969  BV bv;
970  const Triangle& triangle = tri_indices[primitive_id];
971 
972  if(prev_vertices)
973  {
974  Vector3<S> v[6];
975  for(int i = 0; i < 3; ++i)
976  {
977  v[i] = prev_vertices[triangle[i]];
978  v[i + 3] = vertices[triangle[i]];
979  }
980 
981  fit(v, 6, bv);
982  }
983  else
984  {
985  Vector3<S> v[3];
986  for(int i = 0; i < 3; ++i)
987  {
988  v[i] = vertices[triangle[i]];
989  }
990 
991  fit(v, 3, bv);
992  }
993 
994  bvnode->bv = bv;
995  }
996  else
997  {
998  std::cerr << "BVH Error: Model type not supported!" << std::endl;
1000  }
1001  }
1002  else
1003  {
1004  recursiveRefitTree_bottomup(bvnode->leftChild());
1005  recursiveRefitTree_bottomup(bvnode->rightChild());
1006  bvnode->bv = bvs[bvnode->leftChild()].bv + bvs[bvnode->rightChild()].bv;
1007  }
1008 
1009  return BVH_OK;
1010 }
1011 
1012 //==============================================================================
1013 template <typename S, typename BV>
1015 {
1016  static void run(BVHModel<BV>& model,
1017  int bv_id,
1018  const Matrix3<S>& parent_axis,
1019  const Vector3<S>& parent_c)
1020  {
1021  if(!model.bvs[bv_id].isLeaf())
1022  {
1024  tmp1(model, model.bvs[bv_id].first_child, parent_axis, model.bvs[bv_id].getCenter());
1025 
1027  tmp2(model, model.bvs[bv_id].first_child + 1, parent_axis, model.bvs[bv_id].getCenter());
1028  }
1029 
1030  model.bvs[bv_id].bv = translate(model.bvs[bv_id].bv, -parent_c);
1031  }
1032 };
1033 
1034 //==============================================================================
1035 template <typename BV>
1037  int bv_id,
1038  const Matrix3<S>& parent_axis,
1039  const Vector3<S>& parent_c)
1040 {
1042  *this, bv_id, parent_axis, parent_c);
1043 }
1044 
1045 //==============================================================================
1046 template <typename BV>
1048 {
1050  for(int i = 0; i < num_bvs; ++i)
1051  {
1052  BV bv = bv_fitter->fit(primitive_indices + bvs[i].first_primitive, bvs[i].num_primitives);
1053  bvs[i].bv = bv;
1054  }
1055 
1056  bv_fitter->clear();
1057 
1058  return BVH_OK;
1059 }
1060 
1061 //==============================================================================
1062 template <typename BV>
1064 {
1065  AABB<S> aabb_;
1066  for(int i = 0; i < num_vertices; ++i)
1067  {
1068  aabb_ += vertices[i];
1069  }
1070 
1071  this->aabb_center = aabb_.center();
1072 
1073  this->aabb_radius = 0;
1074  for(int i = 0; i < num_vertices; ++i)
1075  {
1076  S r = (this->aabb_center - vertices[i]).squaredNorm();
1077  if(r > this->aabb_radius) this->aabb_radius = r;
1078  }
1079 
1080  this->aabb_radius = sqrt(this->aabb_radius);
1081 
1082  this->aabb_local = aabb_;
1083 }
1084 
1085 //==============================================================================
1086 template <typename S>
1088 {
1089  static void run(BVHModel<OBB<S>>& model,
1090  int bv_id,
1091  const Matrix3<S>& parent_axis,
1092  const Vector3<S>& parent_c)
1093  {
1094  OBB<S>& obb = model.bvs[bv_id].bv;
1095  if(!model.bvs[bv_id].isLeaf())
1096  {
1098  tmp1(model, model.bvs[bv_id].first_child, obb.axis, obb.To);
1099 
1101  tmp2(model, model.bvs[bv_id].first_child + 1, obb.axis, obb.To);
1102  }
1103 
1104  // make self parent relative
1105  obb.axis = parent_axis.transpose() * obb.axis;
1106  obb.To = (obb.To - parent_c).transpose() * parent_axis;
1107  }
1108 };
1109 
1110 //==============================================================================
1111 template <typename S>
1113 {
1114  static void run(BVHModel<RSS<S>>& model,
1115  int bv_id,
1116  const Matrix3<S>& parent_axis,
1117  const Vector3<S>& parent_c)
1118  {
1119  RSS<S>& rss = model.bvs[bv_id].bv;
1120  if(!model.bvs[bv_id].isLeaf())
1121  {
1123  tmp1(model, model.bvs[bv_id].first_child, rss.axis, rss.To);
1124 
1126  tmp2(model, model.bvs[bv_id].first_child + 1, rss.axis, rss.To);
1127  }
1128 
1129  // make self parent relative
1130  rss.axis = parent_axis.transpose() * rss.axis;
1131  rss.To = (rss.To - parent_c).transpose() * parent_axis;
1132  }
1133 };
1134 
1135 //==============================================================================
1136 template <typename S>
1138 {
1139  static void run(BVHModel<OBBRSS<S>>& model,
1140  int bv_id,
1141  const Matrix3<S>& parent_axis,
1142  const Vector3<S>& parent_c)
1143  {
1144  OBB<S>& obb = model.bvs[bv_id].bv.obb;
1145  RSS<S>& rss = model.bvs[bv_id].bv.rss;
1146  if(!model.bvs[bv_id].isLeaf())
1147  {
1149  tmp1(model, model.bvs[bv_id].first_child, obb.axis, obb.To);
1150 
1152  tmp2(model, model.bvs[bv_id].first_child + 1, obb.axis, obb.To);
1153  }
1154 
1155  // make self parent relative
1156  obb.axis = parent_axis.transpose() * obb.axis;
1157  rss.axis = obb.axis;
1158 
1159  obb.To = (obb.To - parent_c).transpose() * parent_axis;
1160  rss.To = obb.To;
1161  }
1162 };
1163 
1164 //==============================================================================
1165 template <typename S>
1167 {
1168  static NODE_TYPE run()
1169  {
1170  return BV_AABB;
1171  }
1172 };
1173 
1174 //==============================================================================
1175 template <typename S>
1177 {
1178  static NODE_TYPE run()
1179  {
1180  return BV_OBB;
1181  }
1182 };
1183 
1184 //==============================================================================
1185 template <typename S>
1187 {
1188  static NODE_TYPE run()
1189  {
1190  return BV_RSS;
1191  }
1192 };
1193 
1194 //==============================================================================
1195 template <typename S>
1197 {
1198  static NODE_TYPE run()
1199  {
1200  return BV_kIOS;
1201  }
1202 };
1203 
1204 //==============================================================================
1205 template <typename S>
1207 {
1208  static NODE_TYPE run()
1209  {
1210  return BV_OBBRSS;
1211  }
1212 };
1213 
1214 //==============================================================================
1215 template <typename S>
1216 struct GetNodeTypeImpl<KDOP<S, 16>>
1217 {
1218  static NODE_TYPE run()
1219  {
1220  return BV_KDOP16;
1221  }
1222 };
1223 
1224 //==============================================================================
1225 template <typename S>
1226 struct GetNodeTypeImpl<KDOP<S, 18>>
1227 {
1228  static NODE_TYPE run()
1229  {
1230  return BV_KDOP18;
1231  }
1232 };
1233 
1234 //==============================================================================
1235 template <typename S>
1236 struct GetNodeTypeImpl<KDOP<S, 24>>
1237 {
1238  static NODE_TYPE run()
1239  {
1240  return BV_KDOP24;
1241  }
1242 };
1243 
1244 } // namespace fcl
1245 
1246 #endif
void computeLocalAABB() override
Compute the AABB for the BVH, used for broad-phase collision.
Definition: BVH_model-inl.h:1063
int beginModel(int num_tris=0, int num_vertices=0)
Begin a new BVH model.
Definition: BVH_model-inl.h:205
Vector3< BV::S > aabb_center
AABB center in local coordinate.
Definition: collision_geometry.h:91
int updateSubModel(const std::vector< Vector3< S >> &ps)
Update a set of points in the old BVH model.
Definition: BVH_model-inl.h:668
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, halfspace, triangle), and octree
Definition: collision_geometry.h:54
void set(std::size_t p1, std::size_t p2, std::size_t p3)
Set the vertex indices of the triangle.
Definition: triangle.cpp:56
Matrix3< S > computeMomentofInertia() const override
compute the inertia matrix, related to the origin
Definition: BVH_model-inl.h:781
int addVertex(const Vector3< S > &p)
Add one point in the new BVH model.
Definition: BVH_model-inl.h:252
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
int replaceSubModel(const std::vector< Vector3< S >> &ps)
Replace a set of points in the old BVH model.
Definition: BVH_model-inl.h:559
int rightChild() const
Return the index of the second child. The index is referred to the bounding volume array (i...
Definition: BV_node_base.cpp:62
triangle model
Definition: BVH_internal.h:79
int leftChild() const
Return the index of the first child. The index is referred to the bounding volume array (i...
Definition: BV_node_base.cpp:56
int replaceVertex(const Vector3< S > &p)
Replace one point in the old BVH model.
Definition: BVH_model-inl.h:527
Matrix3< S > axis
Orientation of RSS. The axes of the rotation matrix are the principle directions of the box...
Definition: RSS.h:60
Vector3< S > To
Origin of the rectangle in RSS.
Definition: RSS.h:63
A class for rectangle sphere-swept bounding volume.
Definition: RSS.h:49
BV::S aabb_radius
AABB radius.
Definition: collision_geometry.h:94
int getNumBVs() const
Get the number of bv in the BVH.
Definition: BVH_model-inl.h:174
int beginReplaceModel()
Replace the geometry information of current frame (i.e. should have the same mesh topology with the p...
Definition: BVH_model-inl.h:508
BVHModelType getModelType() const
Model type described by the instance.
Definition: BVH_model-inl.h:48
AABB< S > translate(const AABB< S > &aabb, const Eigen::MatrixBase< Derived > &t)
translate the center of AABB by t
Definition: AABB-inl.h:345
OBJECT_TYPE getObjectType() const override
Get the object type: it is a BVH.
Definition: BVH_model-inl.h:181
BVHModelType
BVH model type.
Definition: BVH_internal.h:75
BVHModel()
Constructing an empty BVH.
Definition: BVH_model-inl.h:60
const BVNode< BV > & getBV(int id) const
We provide getBV() and getNumBVs() because BVH may be compressed (in future), so we must provide some...
Definition: BVH_model-inl.h:160
Vector3< S > * prev_vertices
Geometry point data in previous frame.
Definition: BVH_model.h:168
BVH model update failed.
Definition: BVH_internal.h:70
Vector3< S > center() const
Center of the AABB.
Definition: AABB-inl.h:230
Vector3< S > * vertices
Geometry point data.
Definition: BVH_model.h:162
int num_vertices
Number of points.
Definition: BVH_model.h:174
void makeParentRelative()
This is a special acceleration: BVH_model default stores the BV&#39;s transform in world coordinate...
Definition: BVH_model-inl.h:741
int addSubModel(const std::vector< Vector3< S >> &ps, const std::vector< Triangle > &ts)
Add a set of triangles in the new BVH model.
Definition: BVH_model-inl.h:374
int addTriangle(const Vector3< S > &p1, const Vector3< S > &p2, const Vector3< S > &p3)
Add one triangle in the new BVH model.
Definition: BVH_model-inl.h:283
BVH geometry is not prepared.
Definition: BVH_internal.h:67
The geometry for the object for collision or distance computation.
Definition: collision_geometry.h:59
Cannot allocate memory for vertices and triangles.
Definition: BVH_internal.h:65
Triangle * tri_indices
Geometry triangle index data, will be nullptr for point clouds.
Definition: BVH_model.h:165
Triangle with 3 indices for points.
Definition: triangle.h:47
void fit(Vector3< typename BV::S > *ps, int n, BV &bv)
Compute a bounding volume that fits a set of n points.
Definition: utility-inl.h:647
BVH geometry in previous frame is not prepared.
Definition: BVH_internal.h:68
unknown model type
Definition: BVH_internal.h:78
int num_tris
Number of triangles.
Definition: BVH_model.h:171
std::shared_ptr< detail::BVFitterBase< BV > > bv_fitter
Fitting rule to fit a BV node to a set of geometry primitives.
Definition: BVH_model.h:183
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_node_base.h:56
KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24 The KDOP structure is defined by some pairs of parallel planes defined by some axes. For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 8 (0,-1,0) and (0,1,0) -> indices 1 and 9 (0,0,-1) and (0,0,1) -> indices 2 and 10 (-1,-1,0) and (1,1,0) -> indices 3 and 11 (-1,0,-1) and (1,0,1) -> indices 4 and 12 (0,-1,-1) and (0,1,1) -> indices 5 and 13 (-1,1,0) and (1,-1,0) -> indices 6 and 14 (-1,0,1) and (1,0,-1) -> indices 7 and 15 For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 9 (0,-1,0) and (0,1,0) -> indices 1 and 10 (0,0,-1) and (0,0,1) -> indices 2 and 11 (-1,-1,0) and (1,1,0) -> indices 3 and 12 (-1,0,-1) and (1,0,1) -> indices 4 and 13 (0,-1,-1) and (0,1,1) -> indices 5 and 14 (-1,1,0) and (1,-1,0) -> indices 6 and 15 (-1,0,1) and (1,0,-1) -> indices 7 and 16 (0,-1,1) and (0,1,-1) -> indices 8 and 17 For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 12 (0,-1,0) and (0,1,0) -> indices 1 and 13 (0,0,-1) and (0,0,1) -> indices 2 and 14 (-1,-1,0) and (1,1,0) -> indices 3 and 15 (-1,0,-1) and (1,0,1) -> indices 4 and 16 (0,-1,-1) and (0,1,1) -> indices 5 and 17 (-1,1,0) and (1,-1,0) -> indices 6 and 18 (-1,0,1) and (1,0,-1) -> indices 7 and 19 (0,-1,1) and (0,1,-1) -> indices 8 and 20 (-1, -1, 1) and (1, 1, -1) –> indices 9 and 21 (-1, 1, -1) and (1, -1, 1) –> indices 10 and 22 (1, -1, -1) and (-1, 1, 1) –> indices 11 and 23.
Definition: kDOP.h:84
int num_primitives
The number of primitives belonging to the current node.
Definition: BV_node_base.h:63
S computeVolume() const override
compute the volume
Definition: BVH_model-inl.h:766
int updateTriangle(const Vector3< S > &p1, const Vector3< S > &p2, const Vector3< S > &p3)
Update one triangle in the old BVH model.
Definition: BVH_model-inl.h:652
int memUsage(int msg) const
Check the number of memory used.
Definition: BVH_model-inl.h:721
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_geometry.h:51
after beginModel(), state for adding geometry primitives
Definition: BVH_internal.h:54
AABB< BV::S > aabb_local
AABB in local coordinate, used for tight AABB when only translation transform.
Definition: collision_geometry.h:97
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
int replaceTriangle(const Vector3< S > &p1, const Vector3< S > &p2, const Vector3< S > &p3)
Replace one triangle in the old BVH model.
Definition: BVH_model-inl.h:543
int updateVertex(const Vector3< S > &p)
Update one point in the old BVH model.
Definition: BVH_model-inl.h:636
BV bv
bounding volume storing the geometry
Definition: BV_node.h:57
Matrix3< S > axis
Orientation of OBB. The axes of the rotation matrix are the principle directions of the box...
Definition: OBB.h:62
after tree has been build, ready for cd use
Definition: BVH_internal.h:55
A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and...
Definition: BV_node.h:52
BVH is valid.
Definition: BVH_internal.h:64
Definition: BVH_model-inl.h:1014
int endModel()
End BVH model construction, will build the bounding volume hierarchy.
Definition: BVH_model-inl.h:437
NODE_TYPE getNodeType() const override
Get the BV type: default is unknown.
Definition: BVH_model-inl.h:198
int endUpdateModel(bool refit=true, bool bottomup=true)
End BVH model update, will also refit or rebuild the bounding volume hierarchy.
Definition: BVH_model-inl.h:686
BVH construction does not follow correct sequence.
Definition: BVH_internal.h:66
after tree has been build for updated geometry, ready for ccd use
Definition: BVH_internal.h:57
Definition: BVH_model-inl.h:188
empty state, immediately after constructor
Definition: BVH_internal.h:53
Vector3< S > computeCOM() const override
compute center of mass
Definition: BVH_model-inl.h:749
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:50
bool isLeaf() const
Whether current node is a leaf node (i.e. contains a primitive index.
Definition: BV_node_base.cpp:44
int endReplaceModel(bool refit=true, bool bottomup=true)
End BVH model replacement, will also refit or rebuild the bounding volume hierarchy.
Definition: BVH_model-inl.h:577
std::shared_ptr< detail::BVSplitterBase< BV > > bv_splitter
Split rule to split one BV node into two children.
Definition: BVH_model.h:180
Vector3< S > To
Center of OBB.
Definition: OBB.h:65
BVHBuildState build_state
The state of BVH building process.
Definition: BVH_model.h:177
after beginUpdateModel(), state for updating geometry primitives
Definition: BVH_internal.h:56
~BVHModel()
deconstruction, delete mesh data related.
Definition: BVH_model-inl.h:148
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:48
int first_primitive
The start id the primitive belonging to the current node. The index is referred to the primitive_indi...
Definition: BV_node_base.h:60
int beginUpdateModel()
Replace the geometry information of current frame (i.e. should have the same mesh topology with the p...
Definition: BVH_model-inl.h:607
Oriented bounding box class.
Definition: OBB.h:51