BVH_model-inl.h
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-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 
42 #include <new>
43 #include <algorithm>
44 
45 namespace fcl
46 {
47 
48 //==============================================================================
49 template <typename BV>
51 {
52  if(num_tris && num_vertices)
53  return BVH_MODEL_TRIANGLES;
54  else if(num_vertices)
55  return BVH_MODEL_POINTCLOUD;
56  else
57  return BVH_MODEL_UNKNOWN;
58 }
59 
60 //==============================================================================
61 template <typename BV>
62 BVHModel<BV>::BVHModel() : vertices(nullptr),
63  tri_indices(nullptr),
64  prev_vertices(nullptr),
65  num_tris(0),
66  num_vertices(0),
67  build_state(BVH_BUILD_STATE_EMPTY),
68  bv_splitter(new detail::BVSplitter<BV>(detail::SPLIT_METHOD_MEAN)),
69  bv_fitter(new detail::BVFitter<BV>()),
70  num_tris_allocated(0),
71  num_vertices_allocated(0),
72  num_bvs_allocated(0),
73  num_vertex_updated(0),
74  primitive_indices(nullptr),
75  bvs(nullptr),
76  num_bvs(0)
77 {
78  // Do nothing
79 }
80 
81 //==============================================================================
82 template <typename BV>
84  : CollisionGeometry<S>(other),
85  num_tris(other.num_tris),
86  num_vertices(other.num_vertices),
87  build_state(other.build_state),
88  bv_splitter(other.bv_splitter),
89  bv_fitter(other.bv_fitter),
90  num_tris_allocated(other.num_tris),
91  num_vertices_allocated(other.num_vertices)
92 {
93  if(other.vertices)
94  {
95  vertices = new Vector3<S>[num_vertices];
96  std::copy(other.vertices, other.vertices + num_vertices, vertices);
97  }
98  else
99  vertices = nullptr;
100 
101  if(other.tri_indices)
102  {
103  tri_indices = new Triangle[num_tris];
104  std::copy(other.tri_indices, other.tri_indices + num_tris, tri_indices);
105  }
106  else
107  tri_indices = nullptr;
108 
109  if(other.prev_vertices)
110  {
111  prev_vertices = new Vector3<S>[num_vertices];
112  std::copy(other.prev_vertices, other.prev_vertices + num_vertices, prev_vertices);
113  }
114  else
115  prev_vertices = nullptr;
116 
117  if(other.primitive_indices)
118  {
119  int num_primitives = 0;
120  switch(other.getModelType())
121  {
122  case BVH_MODEL_TRIANGLES:
123  num_primitives = num_tris;
124  break;
126  num_primitives = num_vertices;
127  break;
128  default:
129  ;
130  }
131 
132  primitive_indices = new unsigned int[num_primitives];
133  std::copy(other.primitive_indices, other.primitive_indices + num_primitives, primitive_indices);
134  }
135  else
136  primitive_indices = nullptr;
137 
138  num_bvs = num_bvs_allocated = other.num_bvs;
139  if(other.bvs)
140  {
141  bvs = new BVNode<BV>[num_bvs];
142  std::copy(other.bvs, other.bvs + num_bvs, bvs);
143  }
144  else
145  bvs = nullptr;
146 }
147 
148 //==============================================================================
149 template <typename BV>
151 {
152  delete [] vertices;
153  delete [] tri_indices;
154  delete [] bvs;
155 
156  delete [] prev_vertices;
157  delete [] primitive_indices;
158 }
159 
160 //==============================================================================
161 template <typename BV>
162 const BVNode<BV>& BVHModel<BV>::getBV(int id) const
163 {
164  return bvs[id];
165 }
166 
167 //==============================================================================
168 template <typename BV>
170 {
171  return bvs[id];
172 }
173 
174 //==============================================================================
175 template <typename BV>
177 {
178  return num_bvs;
179 }
180 
181 //==============================================================================
182 template <typename BV>
184 {
185  return OT_BVH;
186 }
187 
188 //==============================================================================
189 template <typename BV>
191 {
192  static NODE_TYPE run()
193  {
194  return BV_UNKNOWN;
195  }
196 };
197 
198 //==============================================================================
199 template <typename BV>
201 {
202  return GetNodeTypeImpl<BV>::run();
203 }
204 
205 //==============================================================================
206 template <typename BV>
207 int BVHModel<BV>::beginModel(int num_tris_, int num_vertices_)
208 {
209  if(build_state != BVH_BUILD_STATE_EMPTY)
210  {
211  delete [] vertices; vertices = nullptr;
212  delete [] tri_indices; tri_indices = nullptr;
213  delete [] bvs; bvs = nullptr;
214  delete [] prev_vertices; prev_vertices = nullptr;
215  delete [] primitive_indices; primitive_indices = nullptr;
216 
217  num_vertices_allocated = num_vertices = num_tris_allocated = num_tris = num_bvs_allocated = num_bvs = 0;
218  }
219 
220  if(num_tris_ < 0) num_tris_ = 8;
221  if(num_vertices_ <= 0) num_vertices_ = 8;
222 
223  num_vertices_allocated = num_vertices_;
224  num_tris_allocated = num_tris_;
225 
226  if(num_tris_ > 0)
227  {
228  tri_indices = new(std::nothrow) Triangle[num_tris_allocated];
229  if(!tri_indices)
230  {
231  std::cerr << "BVH Error! Out of memory for tri_indices array on BeginModel() call!" << std::endl;
233  }
234  }
235 
236  vertices = new Vector3<S>[num_vertices_allocated];
237  if(!vertices)
238  {
239  std::cerr << "BVH Error! Out of memory for vertices array on BeginModel() call!" << std::endl;
241  }
242 
243  if(build_state != BVH_BUILD_STATE_EMPTY)
244  {
245  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;
246  build_state = BVH_BUILD_STATE_EMPTY;
248  }
249 
250  build_state = BVH_BUILD_STATE_BEGUN;
251 
252  return BVH_OK;
253 }
254 
255 //==============================================================================
256 template <typename BV>
258 {
259  if(build_state != BVH_BUILD_STATE_BEGUN)
260  {
261  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;
263  }
264 
265  if(num_vertices >= num_vertices_allocated)
266  {
267  Vector3<S>* temp = new Vector3<S>[num_vertices_allocated * 2];
268  if(!temp)
269  {
270  std::cerr << "BVH Error! Out of memory for vertices array on addVertex() call!" << std::endl;
272  }
273 
274  std::copy(vertices, vertices + num_vertices, temp);
275  delete [] vertices;
276  vertices = temp;
277  num_vertices_allocated *= 2;
278  }
279 
280  vertices[num_vertices] = p;
281  num_vertices += 1;
282 
283  return BVH_OK;
284 }
285 
286 //==============================================================================
287 template <typename BV>
288 int BVHModel<BV>::addTriangle(const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3)
289 {
290  if(build_state == BVH_BUILD_STATE_PROCESSED)
291  {
292  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;
294  }
295 
296  if(num_vertices + 2 >= num_vertices_allocated)
297  {
298  Vector3<S>* temp = new Vector3<S>[num_vertices_allocated * 2 + 2];
299  if(!temp)
300  {
301  std::cerr << "BVH Error! Out of memory for vertices array on addTriangle() call!" << std::endl;
303  }
304 
305  std::copy(vertices, vertices + num_vertices, temp);
306  delete [] vertices;
307  vertices = temp;
308  num_vertices_allocated = num_vertices_allocated * 2 + 2;
309  }
310 
311  int offset = num_vertices;
312 
313  vertices[num_vertices] = p1;
314  num_vertices++;
315  vertices[num_vertices] = p2;
316  num_vertices++;
317  vertices[num_vertices] = p3;
318  num_vertices++;
319 
320  if(num_tris >= num_tris_allocated)
321  {
322  if(num_tris_allocated == 0)
323  {
324  num_tris_allocated = 1;
325  }
326  Triangle* temp = new Triangle[num_tris_allocated * 2];
327  if(!temp)
328  {
329  std::cerr << "BVH Error! Out of memory for tri_indices array on addTriangle() call!" << std::endl;
331  }
332 
333  std::copy(tri_indices, tri_indices + num_tris, temp);
334  delete [] tri_indices;
335  tri_indices = temp;
336  num_tris_allocated *= 2;
337  }
338 
339  tri_indices[num_tris].set(offset, offset + 1, offset + 2);
340  num_tris++;
341 
342  return BVH_OK;
343 }
344 
345 //==============================================================================
346 template <typename BV>
347 int BVHModel<BV>::addSubModel(const std::vector<Vector3<S>>& ps)
348 {
349  if(build_state == BVH_BUILD_STATE_PROCESSED)
350  {
351  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;
353  }
354 
355  int num_vertices_to_add = ps.size();
356 
357  if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated)
358  {
359  Vector3<S>* temp = new Vector3<S>[num_vertices_allocated * 2 + num_vertices_to_add - 1];
360  if(!temp)
361  {
362  std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl;
364  }
365 
366  std::copy(vertices, vertices + num_vertices, temp);
367  delete [] vertices;
368  vertices = temp;
369  num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1;
370  }
371 
372  for(int i = 0; i < num_vertices_to_add; ++i)
373  {
374  vertices[num_vertices] = ps[i];
375  num_vertices++;
376  }
377 
378  return BVH_OK;
379 }
380 
381 //==============================================================================
382 template <typename BV>
383 int BVHModel<BV>::addSubModel(const std::vector<Vector3<S>>& ps, const std::vector<Triangle>& ts)
384 {
385  if(build_state == BVH_BUILD_STATE_PROCESSED)
386  {
387  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;
389  }
390 
391  int num_vertices_to_add = ps.size();
392 
393  if(num_vertices + num_vertices_to_add - 1 >= num_vertices_allocated)
394  {
395  Vector3<S>* temp = new Vector3<S>[num_vertices_allocated * 2 + num_vertices_to_add - 1];
396  if(!temp)
397  {
398  std::cerr << "BVH Error! Out of memory for vertices array on addSubModel() call!" << std::endl;
400  }
401 
402  std::copy(vertices, vertices + num_vertices, temp);
403  delete [] vertices;
404  vertices = temp;
405  num_vertices_allocated = num_vertices_allocated * 2 + num_vertices_to_add - 1;
406  }
407 
408  int offset = num_vertices;
409 
410  for(int i = 0; i < num_vertices_to_add; ++i)
411  {
412  vertices[num_vertices] = ps[i];
413  num_vertices++;
414  }
415 
416 
417  int num_tris_to_add = ts.size();
418 
419  if(num_tris + num_tris_to_add - 1 >= num_tris_allocated)
420  {
421  if(num_tris_allocated == 0)
422  {
423  num_tris_allocated = 1;
424  }
425  Triangle* temp = new(std::nothrow) Triangle[num_tris_allocated * 2 + num_tris_to_add - 1];
426  if(!temp)
427  {
428  std::cerr << "BVH Error! Out of memory for tri_indices array on addSubModel() call!" << std::endl;
430  }
431 
432  std::copy(tri_indices, tri_indices + num_tris, temp);
433  delete [] tri_indices;
434  tri_indices = temp;
435  num_tris_allocated = num_tris_allocated * 2 + num_tris_to_add - 1;
436  }
437 
438  for(int i = 0; i < num_tris_to_add; ++i)
439  {
440  const Triangle& t = ts[i];
441  tri_indices[num_tris].set(t[0] + offset, t[1] + offset, t[2] + offset);
442  num_tris++;
443  }
444 
445  return BVH_OK;
446 }
447 
448 //==============================================================================
449 template <typename BV>
451 {
452  if(build_state != BVH_BUILD_STATE_BEGUN)
453  {
454  std::cerr << "BVH Warning! Call endModel() in wrong order. endModel() was ignored." << std::endl;
456  }
457 
458  if(num_tris == 0 && num_vertices == 0)
459  {
460  std::cerr << "BVH Error! endModel() called on model with no triangles and vertices." << std::endl;
462  }
463 
464  if(num_tris_allocated > num_tris)
465  {
466  Triangle* new_tris = new(std::nothrow) Triangle[num_tris];
467  if(!new_tris)
468  {
469  std::cerr << "BVH Error! Out of memory for tri_indices array in endModel() call!" << std::endl;
471  }
472  std::copy(tri_indices, tri_indices + num_tris, new_tris);
473  delete [] tri_indices;
474  tri_indices = new_tris;
475  num_tris_allocated = num_tris;
476  }
477 
478  if(num_vertices_allocated > num_vertices)
479  {
480  Vector3<S>* new_vertices = new Vector3<S>[num_vertices];
481  if(!new_vertices)
482  {
483  std::cerr << "BVH Error! Out of memory for vertices array in endModel() call!" << std::endl;
485  }
486  std::copy(vertices, vertices + num_vertices, new_vertices);
487  delete [] vertices;
488  vertices = new_vertices;
489  num_vertices_allocated = num_vertices;
490  }
491 
492 
493  // construct BVH tree
494  int num_bvs_to_be_allocated = 0;
495  if(num_tris == 0)
496  num_bvs_to_be_allocated = 2 * num_vertices - 1;
497  else
498  num_bvs_to_be_allocated = 2 * num_tris - 1;
499 
500 
501  bvs = new(std::nothrow) BVNode<BV> [num_bvs_to_be_allocated];
502  primitive_indices = new(std::nothrow) unsigned int [num_bvs_to_be_allocated];
503  if(!bvs || !primitive_indices)
504  {
505  std::cerr << "BVH Error! Out of memory for BV array in endModel()!" << std::endl;
507  }
508  num_bvs_allocated = num_bvs_to_be_allocated;
509  num_bvs = 0;
510 
511  buildTree();
512 
513  // finish constructing
514  build_state = BVH_BUILD_STATE_PROCESSED;
515 
516  return BVH_OK;
517 }
518 
519 //==============================================================================
520 template <typename BV>
522 {
523  if(build_state != BVH_BUILD_STATE_PROCESSED)
524  {
525  std::cerr << "BVH Error! Call beginReplaceModel() on a BVHModel that has no previous frame." << std::endl;
527  }
528 
529  if(prev_vertices)
530  {
531  delete [] prev_vertices;
532  prev_vertices = nullptr;
533  }
534 
535  num_vertex_updated = 0;
536 
537  build_state = BVH_BUILD_STATE_REPLACE_BEGUN;
538 
539  return BVH_OK;
540 }
541 
542 //==============================================================================
543 template <typename BV>
545 {
546  if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
547  {
548  std::cerr << "BVH Warning! Call replaceVertex() in a wrong order. replaceVertex() was ignored. Must do a beginReplaceModel() for initialization." << std::endl;
550  }
551 
552  vertices[num_vertex_updated] = p;
553  num_vertex_updated++;
554 
555  return BVH_OK;
556 }
557 
558 //==============================================================================
559 template <typename BV>
560 int BVHModel<BV>::replaceTriangle(const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3)
561 {
562  if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
563  {
564  std::cerr << "BVH Warning! Call replaceTriangle() in a wrong order. replaceTriangle() was ignored. Must do a beginReplaceModel() for initialization." << std::endl;
566  }
567 
568  vertices[num_vertex_updated] = p1; num_vertex_updated++;
569  vertices[num_vertex_updated] = p2; num_vertex_updated++;
570  vertices[num_vertex_updated] = p3; num_vertex_updated++;
571  return BVH_OK;
572 }
573 
574 //==============================================================================
575 template <typename BV>
576 int BVHModel<BV>::replaceSubModel(const std::vector<Vector3<S>>& ps)
577 {
578  if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
579  {
580  std::cerr << "BVH Warning! Call replaceSubModel() in a wrong order. replaceSubModel() was ignored. Must do a beginReplaceModel() for initialization." << std::endl;
582  }
583 
584  for(unsigned int i = 0; i < ps.size(); ++i)
585  {
586  vertices[num_vertex_updated] = ps[i];
587  num_vertex_updated++;
588  }
589  return BVH_OK;
590 }
591 
592 //==============================================================================
593 template <typename BV>
594 int BVHModel<BV>::endReplaceModel(bool refit, bool bottomup)
595 {
596  if(build_state != BVH_BUILD_STATE_REPLACE_BEGUN)
597  {
598  std::cerr << "BVH Warning! Call endReplaceModel() in a wrong order. endReplaceModel() was ignored. " << std::endl;
600  }
601 
602  if(num_vertex_updated != num_vertices)
603  {
604  std::cerr << "BVH Error! The replaced model should have the same number of vertices as the old model." << std::endl;
605  return BVH_ERR_INCORRECT_DATA;
606  }
607 
608  if(refit) // refit, do not change BVH structure
609  {
610  refitTree(bottomup);
611  }
612  else // reconstruct bvh tree based on current frame data
613  {
614  buildTree();
615  }
616 
617  build_state = BVH_BUILD_STATE_PROCESSED;
618 
619  return BVH_OK;
620 }
621 
622 //==============================================================================
623 template <typename BV>
625 {
626  if(build_state != BVH_BUILD_STATE_PROCESSED && build_state != BVH_BUILD_STATE_UPDATED)
627  {
628  std::cerr << "BVH Error! Call beginUpdatemodel() on a BVHModel that has no previous frame." << std::endl;
630  }
631 
632  if(prev_vertices)
633  {
634  Vector3<S>* temp = prev_vertices;
635  prev_vertices = vertices;
636  vertices = temp;
637  }
638  else
639  {
640  prev_vertices = vertices;
641  vertices = new Vector3<S>[num_vertices];
642  }
643 
644  num_vertex_updated = 0;
645 
646  build_state = BVH_BUILD_STATE_UPDATE_BEGUN;
647 
648  return BVH_OK;
649 }
650 
651 //==============================================================================
652 template <typename BV>
654 {
655  if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
656  {
657  std::cerr << "BVH Warning! Call updateVertex() in a wrong order. updateVertex() was ignored. Must do a beginUpdateModel() for initialization." << std::endl;
659  }
660 
661  vertices[num_vertex_updated] = p;
662  num_vertex_updated++;
663 
664  return BVH_OK;
665 }
666 
667 //==============================================================================
668 template <typename BV>
669 int BVHModel<BV>::updateTriangle(const Vector3<S>& p1, const Vector3<S>& p2, const Vector3<S>& p3)
670 {
671  if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
672  {
673  std::cerr << "BVH Warning! Call updateTriangle() in a wrong order. updateTriangle() was ignored. Must do a beginUpdateModel() for initialization." << std::endl;
675  }
676 
677  vertices[num_vertex_updated] = p1; num_vertex_updated++;
678  vertices[num_vertex_updated] = p2; num_vertex_updated++;
679  vertices[num_vertex_updated] = p3; num_vertex_updated++;
680  return BVH_OK;
681 }
682 
683 //==============================================================================
684 template <typename BV>
685 int BVHModel<BV>::updateSubModel(const std::vector<Vector3<S>>& ps)
686 {
687  if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
688  {
689  std::cerr << "BVH Warning! Call updateSubModel() in a wrong order. updateSubModel() was ignored. Must do a beginUpdateModel() for initialization." << std::endl;
691  }
692 
693  for(unsigned int i = 0; i < ps.size(); ++i)
694  {
695  vertices[num_vertex_updated] = ps[i];
696  num_vertex_updated++;
697  }
698  return BVH_OK;
699 }
700 
701 //==============================================================================
702 template <typename BV>
703 int BVHModel<BV>::endUpdateModel(bool refit, bool bottomup)
704 {
705  if(build_state != BVH_BUILD_STATE_UPDATE_BEGUN)
706  {
707  std::cerr << "BVH Warning! Call endUpdateModel() in a wrong order. endUpdateModel() was ignored. " << std::endl;
709  }
710 
711  if(num_vertex_updated != num_vertices)
712  {
713  std::cerr << "BVH Error! The updated model should have the same number of vertices as the old model." << std::endl;
714  return BVH_ERR_INCORRECT_DATA;
715  }
716 
717  if(refit) // refit, do not change BVH structure
718  {
719  refitTree(bottomup);
720  }
721  else // reconstruct bvh tree based on current frame data
722  {
723  buildTree();
724 
725  // then refit
726 
727  refitTree(bottomup);
728  }
729 
730 
731  build_state = BVH_BUILD_STATE_UPDATED;
732 
733  return BVH_OK;
734 }
735 
736 //==============================================================================
737 template <typename BV>
738 int BVHModel<BV>::memUsage(int msg) const
739 {
740  int mem_bv_list = sizeof(BV) * num_bvs;
741  int mem_tri_list = sizeof(Triangle) * num_tris;
742  int mem_vertex_list = sizeof(Vector3<S>) * num_vertices;
743 
744  int total_mem = mem_bv_list + mem_tri_list + mem_vertex_list + sizeof(BVHModel<BV>);
745  if(msg)
746  {
747  std::cerr << "Total for model " << total_mem << " bytes." << std::endl;
748  std::cerr << "BVs: " << num_bvs << " allocated." << std::endl;
749  std::cerr << "Tris: " << num_tris << " allocated." << std::endl;
750  std::cerr << "Vertices: " << num_vertices << " allocated." << std::endl;
751  }
752 
753  return BVH_OK;
754 }
755 
756 //==============================================================================
757 template <typename BV>
759 {
760  makeParentRelativeRecurse(
762 }
763 
764 //==============================================================================
765 template <typename BV>
767 {
768  S vol = 0;
770  for(int i = 0; i < num_tris; ++i)
771  {
772  const Triangle& tri = tri_indices[i];
773  S d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
774  vol += d_six_vol;
775  com.noalias() += (vertices[tri[0]] + vertices[tri[1]] + vertices[tri[2]]) * d_six_vol;
776  }
777 
778  return com / (vol * 4);
779 }
780 
781 //==============================================================================
782 template <typename BV>
783 typename BV::S BVHModel<BV>::computeVolume() const
784 {
785  S vol = 0;
786  for(int i = 0; i < num_tris; ++i)
787  {
788  const Triangle& tri = tri_indices[i];
789  S d_six_vol = (vertices[tri[0]].cross(vertices[tri[1]])).dot(vertices[tri[2]]);
790  vol += d_six_vol;
791  }
792 
793  return vol / 6;
794 }
795 
796 //==============================================================================
797 template <typename BV>
799 {
801 
802  Matrix3<S> C_canonical;
803  C_canonical << 1/ 60.0, 1/120.0, 1/120.0,
804  1/120.0, 1/ 60.0, 1/120.0,
805  1/120.0, 1/120.0, 1/ 60.0;
806 
807  for(int i = 0; i < num_tris; ++i)
808  {
809  const Triangle& tri = tri_indices[i];
810  const Vector3<S>& v1 = vertices[tri[0]];
811  const Vector3<S>& v2 = vertices[tri[1]];
812  const Vector3<S>& v3 = vertices[tri[2]];
813  S d_six_vol = (v1.cross(v2)).dot(v3);
814  Matrix3<S> A;
815  A.row(0) = v1;
816  A.row(1) = v2;
817  A.row(2) = v3;
818  C.noalias() += A.transpose() * C_canonical * A * d_six_vol;
819  }
820 
821  S trace_C = C(0, 0) + C(1, 1) + C(2, 2);
822 
823  Matrix3<S> m;
824  m << trace_C - C(0, 0), -C(0, 1), -C(0, 2),
825  -C(1, 0), trace_C - C(1, 1), -C(1, 2),
826  -C(2, 0), -C(2, 1), trace_C - C(2, 2);
827 
828  return m;
829 }
830 
831 //==============================================================================
832 template <typename BV>
834 {
835  // set BVFitter
836  bv_fitter->set(vertices, tri_indices, getModelType());
837  // set SplitRule
838  bv_splitter->set(vertices, tri_indices, getModelType());
839 
840  num_bvs = 1;
841 
842  int num_primitives = 0;
843  switch(getModelType())
844  {
845  case BVH_MODEL_TRIANGLES:
846  num_primitives = num_tris;
847  break;
849  num_primitives = num_vertices;
850  break;
851  default:
852  std::cerr << "BVH Error: Model type not supported!" << std::endl;
854  }
855 
856  for(int i = 0; i < num_primitives; ++i)
857  primitive_indices[i] = i;
858  recursiveBuildTree(0, 0, num_primitives);
859 
860  bv_fitter->clear();
861  bv_splitter->clear();
862 
863  return BVH_OK;
864 }
865 
866 //==============================================================================
867 template <typename BV>
868 int BVHModel<BV>::recursiveBuildTree(int bv_id, int first_primitive, int num_primitives)
869 {
870  BVHModelType type = getModelType();
871  BVNode<BV>* bvnode = bvs + bv_id;
872  unsigned int* cur_primitive_indices = primitive_indices + first_primitive;
873 
874  // constructing BV
875  BV bv = bv_fitter->fit(cur_primitive_indices, num_primitives);
876  bv_splitter->computeRule(bv, cur_primitive_indices, num_primitives);
877 
878  bvnode->bv = bv;
879  bvnode->first_primitive = first_primitive;
880  bvnode->num_primitives = num_primitives;
881 
882  if(num_primitives == 1)
883  {
884  bvnode->first_child = -((*cur_primitive_indices) + 1);
885  }
886  else
887  {
888  bvnode->first_child = num_bvs;
889  num_bvs += 2;
890 
891  int c1 = 0;
892  for(int i = 0; i < num_primitives; ++i)
893  {
894  Vector3<S> p;
895  if(type == BVH_MODEL_POINTCLOUD) p = vertices[cur_primitive_indices[i]];
896  else if(type == BVH_MODEL_TRIANGLES)
897  {
898  const Triangle& t = tri_indices[cur_primitive_indices[i]];
899  const Vector3<S>& p1 = vertices[t[0]];
900  const Vector3<S>& p2 = vertices[t[1]];
901  const Vector3<S>& p3 = vertices[t[2]];
902  p.noalias() = (p1 + p2 + p3) / 3.0;
903  }
904  else
905  {
906  std::cerr << "BVH Error: Model type not supported!" << std::endl;
908  }
909 
910 
911  // loop invariant: up to (but not including) index c1 in group 1,
912  // then up to (but not including) index i in group 2
913  //
914  // [1] [1] [1] [1] [2] [2] [2] [x] [x] ... [x]
915  // c1 i
916  //
917  if(bv_splitter->apply(p)) // in the right side
918  {
919  // do nothing
920  }
921  else
922  {
923  std::swap(cur_primitive_indices[i], cur_primitive_indices[c1]);
924  c1++;
925  }
926  }
927 
928 
929  if((c1 == 0) || (c1 == num_primitives)) c1 = num_primitives / 2;
930 
931  int num_first_half = c1;
932 
933  recursiveBuildTree(bvnode->leftChild(), first_primitive, num_first_half);
934  recursiveBuildTree(bvnode->rightChild(), first_primitive + num_first_half, num_primitives - num_first_half);
935  }
936 
937  return BVH_OK;
938 }
939 
940 //==============================================================================
941 template <typename BV>
942 int BVHModel<BV>::refitTree(bool bottomup)
943 {
944  if(bottomup)
945  return refitTree_bottomup();
946  else
947  return refitTree_topdown();
948 }
949 
950 //==============================================================================
951 template <typename BV>
953 {
954  int res = recursiveRefitTree_bottomup(0);
955 
956  return res;
957 }
958 
959 //==============================================================================
960 template <typename BV>
962 {
963  BVNode<BV>* bvnode = bvs + bv_id;
964  if(bvnode->isLeaf())
965  {
966  BVHModelType type = getModelType();
967  int primitive_id = -(bvnode->first_child + 1);
968  if(type == BVH_MODEL_POINTCLOUD)
969  {
970  BV bv;
971 
972  if(prev_vertices)
973  {
974  Vector3<S> v[2];
975  v[0] = prev_vertices[primitive_id];
976  v[1] = vertices[primitive_id];
977  fit(v, 2, bv);
978  }
979  else
980  fit(vertices + primitive_id, 1, bv);
981 
982  bvnode->bv = bv;
983  }
984  else if(type == BVH_MODEL_TRIANGLES)
985  {
986  BV bv;
987  const Triangle& triangle = tri_indices[primitive_id];
988 
989  if(prev_vertices)
990  {
991  Vector3<S> v[6];
992  for(int i = 0; i < 3; ++i)
993  {
994  v[i] = prev_vertices[triangle[i]];
995  v[i + 3] = vertices[triangle[i]];
996  }
997 
998  fit(v, 6, bv);
999  }
1000  else
1001  {
1002  Vector3<S> v[3];
1003  for(int i = 0; i < 3; ++i)
1004  {
1005  v[i] = vertices[triangle[i]];
1006  }
1007 
1008  fit(v, 3, bv);
1009  }
1010 
1011  bvnode->bv = bv;
1012  }
1013  else
1014  {
1015  std::cerr << "BVH Error: Model type not supported!" << std::endl;
1017  }
1018  }
1019  else
1020  {
1021  recursiveRefitTree_bottomup(bvnode->leftChild());
1022  recursiveRefitTree_bottomup(bvnode->rightChild());
1023  bvnode->bv = bvs[bvnode->leftChild()].bv + bvs[bvnode->rightChild()].bv;
1024  }
1025 
1026  return BVH_OK;
1027 }
1028 
1029 //==============================================================================
1030 template <typename S, typename BV>
1032 {
1033  static void run(BVHModel<BV>& model,
1034  int bv_id,
1035  const Matrix3<S>& parent_axis,
1036  const Vector3<S>& parent_c)
1037  {
1038  if(!model.bvs[bv_id].isLeaf())
1039  {
1041  tmp1(model, model.bvs[bv_id].first_child, parent_axis, model.bvs[bv_id].getCenter());
1042 
1044  tmp2(model, model.bvs[bv_id].first_child + 1, parent_axis, model.bvs[bv_id].getCenter());
1045  }
1046 
1047  model.bvs[bv_id].bv = translate(model.bvs[bv_id].bv, -parent_c);
1048  }
1049 };
1050 
1051 //==============================================================================
1052 template <typename BV>
1054  int bv_id,
1055  const Matrix3<S>& parent_axis,
1056  const Vector3<S>& parent_c)
1057 {
1059  *this, bv_id, parent_axis, parent_c);
1060 }
1061 
1062 //==============================================================================
1063 template <typename BV>
1065 {
1066  bv_fitter->set(vertices, prev_vertices, tri_indices, getModelType());
1067  for(int i = 0; i < num_bvs; ++i)
1068  {
1069  BV bv = bv_fitter->fit(primitive_indices + bvs[i].first_primitive, bvs[i].num_primitives);
1070  bvs[i].bv = bv;
1071  }
1072 
1073  bv_fitter->clear();
1074 
1075  return BVH_OK;
1076 }
1077 
1078 //==============================================================================
1079 template <typename BV>
1081 {
1082  AABB<S> aabb_;
1083  for(int i = 0; i < num_vertices; ++i)
1084  {
1085  aabb_ += vertices[i];
1086  }
1087 
1088  this->aabb_center = aabb_.center();
1089 
1090  this->aabb_radius = 0;
1091  for(int i = 0; i < num_vertices; ++i)
1092  {
1093  S r = (this->aabb_center - vertices[i]).squaredNorm();
1094  if(r > this->aabb_radius) this->aabb_radius = r;
1095  }
1096 
1097  this->aabb_radius = sqrt(this->aabb_radius);
1098 
1099  this->aabb_local = aabb_;
1100 }
1101 
1102 //==============================================================================
1103 template <typename S>
1105 {
1106  static void run(BVHModel<OBB<S>>& model,
1107  int bv_id,
1108  const Matrix3<S>& parent_axis,
1109  const Vector3<S>& parent_c)
1110  {
1111  OBB<S>& obb = model.bvs[bv_id].bv;
1112  if(!model.bvs[bv_id].isLeaf())
1113  {
1115  tmp1(model, model.bvs[bv_id].first_child, obb.axis, obb.To);
1116 
1118  tmp2(model, model.bvs[bv_id].first_child + 1, obb.axis, obb.To);
1119  }
1120 
1121  // make self parent relative
1122  obb.axis = parent_axis.transpose() * obb.axis;
1123  obb.To = (obb.To - parent_c).transpose() * parent_axis;
1124  }
1125 };
1126 
1127 //==============================================================================
1128 template <typename S>
1130 {
1131  static void run(BVHModel<RSS<S>>& model,
1132  int bv_id,
1133  const Matrix3<S>& parent_axis,
1134  const Vector3<S>& parent_c)
1135  {
1136  RSS<S>& rss = model.bvs[bv_id].bv;
1137  if(!model.bvs[bv_id].isLeaf())
1138  {
1140  tmp1(model, model.bvs[bv_id].first_child, rss.axis, rss.To);
1141 
1143  tmp2(model, model.bvs[bv_id].first_child + 1, rss.axis, rss.To);
1144  }
1145 
1146  // make self parent relative
1147  rss.axis = parent_axis.transpose() * rss.axis;
1148  rss.To = (rss.To - parent_c).transpose() * parent_axis;
1149  }
1150 };
1151 
1152 //==============================================================================
1153 template <typename S>
1155 {
1156  static void run(BVHModel<OBBRSS<S>>& model,
1157  int bv_id,
1158  const Matrix3<S>& parent_axis,
1159  const Vector3<S>& parent_c)
1160  {
1161  OBB<S>& obb = model.bvs[bv_id].bv.obb;
1162  RSS<S>& rss = model.bvs[bv_id].bv.rss;
1163  if(!model.bvs[bv_id].isLeaf())
1164  {
1166  tmp1(model, model.bvs[bv_id].first_child, obb.axis, obb.To);
1167 
1169  tmp2(model, model.bvs[bv_id].first_child + 1, obb.axis, obb.To);
1170  }
1171 
1172  // make self parent relative
1173  obb.axis = parent_axis.transpose() * obb.axis;
1174  rss.axis = obb.axis;
1175 
1176  obb.To = (obb.To - parent_c).transpose() * parent_axis;
1177  rss.To = obb.To;
1178  }
1179 };
1180 
1181 //==============================================================================
1182 template <typename S>
1184 {
1185  static NODE_TYPE run()
1186  {
1187  return BV_AABB;
1188  }
1189 };
1190 
1191 //==============================================================================
1192 template <typename S>
1194 {
1195  static NODE_TYPE run()
1196  {
1197  return BV_OBB;
1198  }
1199 };
1200 
1201 //==============================================================================
1202 template <typename S>
1204 {
1205  static NODE_TYPE run()
1206  {
1207  return BV_RSS;
1208  }
1209 };
1210 
1211 //==============================================================================
1212 template <typename S>
1214 {
1215  static NODE_TYPE run()
1216  {
1217  return BV_kIOS;
1218  }
1219 };
1220 
1221 //==============================================================================
1222 template <typename S>
1224 {
1225  static NODE_TYPE run()
1226  {
1227  return BV_OBBRSS;
1228  }
1229 };
1230 
1231 //==============================================================================
1232 template <typename S>
1233 struct GetNodeTypeImpl<KDOP<S, 16>>
1234 {
1235  static NODE_TYPE run()
1236  {
1237  return BV_KDOP16;
1238  }
1239 };
1240 
1241 //==============================================================================
1242 template <typename S>
1243 struct GetNodeTypeImpl<KDOP<S, 18>>
1244 {
1245  static NODE_TYPE run()
1246  {
1247  return BV_KDOP18;
1248  }
1249 };
1250 
1251 //==============================================================================
1252 template <typename S>
1253 struct GetNodeTypeImpl<KDOP<S, 24>>
1254 {
1255  static NODE_TYPE run()
1256  {
1257  return BV_KDOP24;
1258  }
1259 };
1260 
1261 } // namespace fcl
1262 
1263 #endif
fcl::BVNodeBase::first_child
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
fcl::OBB::axis
Matrix3< S > axis
Orientation of OBB. The axes of the rotation matrix are the principle directions of the box....
Definition: OBB.h:62
fcl::BVNodeBase::rightChild
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
fcl::BVNodeBase::isLeaf
bool isLeaf() const
Whether current node is a leaf node (i.e. contains a primitive index.
Definition: BV_node_base.cpp:44
fcl::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:48
fcl::GetNodeTypeImpl
Definition: BVH_model-inl.h:190
fcl::BVH_BUILD_STATE_PROCESSED
@ BVH_BUILD_STATE_PROCESSED
after beginModel(), state for adding geometry primitives
Definition: BVH_internal.h:54
fcl::BVHModel::vertices
Vector3< S > * vertices
Geometry point data.
Definition: BVH_model.h:162
fcl::BVNode::bv
BV bv
bounding volume storing the geometry
Definition: BV_node.h:57
fcl::MakeParentRelativeRecurseImpl::run
static void run(BVHModel< BV > &model, int bv_id, const Matrix3< S > &parent_axis, const Vector3< S > &parent_c)
Definition: BVH_model-inl.h:1033
fcl::MakeParentRelativeRecurseImpl< S, RSS< S > >::run
static void run(BVHModel< RSS< S >> &model, int bv_id, const Matrix3< S > &parent_axis, const Vector3< S > &parent_c)
Definition: BVH_model-inl.h:1131
fcl::GetNodeTypeImpl< OBB< S > >::run
static NODE_TYPE run()
Definition: BVH_model-inl.h:1195
fcl::Triangle::set
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
fcl::BVH_MODEL_UNKNOWN
@ BVH_MODEL_UNKNOWN
Definition: BVH_internal.h:77
fcl::BVH_BUILD_STATE_UPDATE_BEGUN
@ BVH_BUILD_STATE_UPDATE_BEGUN
after tree has been build, ready for cd use
Definition: BVH_internal.h:55
fcl::MakeParentRelativeRecurseImpl< S, OBB< S > >::run
static void run(BVHModel< OBB< S >> &model, int bv_id, const Matrix3< S > &parent_axis, const Vector3< S > &parent_c)
Definition: BVH_model-inl.h:1106
fcl::BVHModel< OBB< S > >::S
typename OBB< S > ::S S
Definition: BVH_model.h:61
fcl::translate
AABB< S > translate(const AABB< S > &aabb, const Eigen::MatrixBase< Derived > &t)
translate the center of AABB by t
Definition: AABB-inl.h:345
fcl::BVNodeBase::leftChild
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
swap
static void swap(T &x, T &y)
Definition: svm.cpp:54
fcl::GetNodeTypeImpl::run
static NODE_TYPE run()
Definition: BVH_model-inl.h:192
fcl::BV_KDOP18
@ BV_KDOP18
Definition: collision_geometry.h:53
fcl::BVH_ERR_BUILD_EMPTY_MODEL
@ BVH_ERR_BUILD_EMPTY_MODEL
BVH construction does not follow correct sequence.
Definition: BVH_internal.h:66
fcl::AABB::center
Vector3< S > center() const
Center of the AABB.
Definition: AABB-inl.h:230
fcl::RSS::To
Vector3< S > To
Origin of frame T in frame F.
Definition: RSS.h:76
fcl::GetNodeTypeImpl< KDOP< S, 18 > >::run
static NODE_TYPE run()
Definition: BVH_model-inl.h:1245
fcl::BVH_BUILD_STATE_BEGUN
@ BVH_BUILD_STATE_BEGUN
empty state, immediately after constructor
Definition: BVH_internal.h:53
fcl::AABB< S >
BVH_model.h
fcl::BVHModelType
BVHModelType
BVH model type.
Definition: BVH_internal.h:75
fcl::BVH_ERR_MODEL_OUT_OF_MEMORY
@ BVH_ERR_MODEL_OUT_OF_MEMORY
BVH is valid.
Definition: BVH_internal.h:64
fcl::RSS
A class for rectangle swept sphere bounding volume.
Definition: RSS.h:58
fcl::Triangle
Triangle with 3 indices for points.
Definition: triangle.h:48
fcl::BVH_BUILD_STATE_REPLACE_BEGUN
@ BVH_BUILD_STATE_REPLACE_BEGUN
after tree has been build for updated geometry, ready for ccd use
Definition: BVH_internal.h:57
fcl::GetNodeTypeImpl< AABB< S > >::run
static NODE_TYPE run()
Definition: BVH_model-inl.h:1185
fcl::BVNodeBase::num_primitives
int num_primitives
The number of primitives belonging to the current node.
Definition: BV_node_base.h:63
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::BVHModel::primitive_indices
unsigned int * primitive_indices
for ccd vertex update
Definition: BVH_model.h:191
fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_geometry.h:58
fcl::OBB
Oriented bounding box class.
Definition: OBB.h:51
fcl::BVHModel::num_bvs
int num_bvs
Number of BV nodes in bounding volume hierarchy.
Definition: BVH_model.h:197
fcl::BVH_BUILD_STATE_UPDATED
@ BVH_BUILD_STATE_UPDATED
after beginUpdateModel(), state for updating geometry primitives
Definition: BVH_internal.h:56
fcl::BVHModel::tri_indices
Triangle * tri_indices
Geometry triangle index data, will be nullptr for point clouds.
Definition: BVH_model.h:165
fcl::BV_KDOP24
@ BV_KDOP24
Definition: collision_geometry.h:53
fcl::BVHModel::BVHModel
BVHModel()
Constructing an empty BVH.
Definition: BVH_model-inl.h:62
fcl::BVNode
A class describing a bounding volume node. It includes the tree structure providing in BVNodeBase and...
Definition: BV_node.h:52
fcl::BV_UNKNOWN
@ BV_UNKNOWN
Definition: collision_geometry.h:53
fcl::BV_OBB
@ BV_OBB
Definition: collision_geometry.h:53
fcl::BVH_BUILD_STATE_EMPTY
@ BVH_BUILD_STATE_EMPTY
Definition: BVH_internal.h:52
fcl::BVH_MODEL_POINTCLOUD
@ BVH_MODEL_POINTCLOUD
triangle model
Definition: BVH_internal.h:79
fcl::BVHModel::prev_vertices
Vector3< S > * prev_vertices
Geometry point data in previous frame.
Definition: BVH_model.h:168
fcl::BV_KDOP16
@ BV_KDOP16
Definition: collision_geometry.h:53
fcl::KDOP
KDOP class describes the KDOP collision structures. K is set as the template parameter,...
Definition: kDOP.h:84
fcl::GetNodeTypeImpl< OBBRSS< S > >::run
static NODE_TYPE run()
Definition: BVH_model-inl.h:1225
r
S r
Definition: test_sphere_box.cpp:171
fcl::MakeParentRelativeRecurseImpl
Definition: BVH_model-inl.h:1031
fcl::detail::SPLIT_METHOD_MEAN
@ SPLIT_METHOD_MEAN
Definition: BV_splitter.h:58
fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:50
fcl::OBJECT_TYPE
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_geometry.h:50
fcl::BVHModel
A class describing the bounding hierarchy of a mesh model or a point cloud model (which is viewed as ...
Definition: BVH_model.h:57
fcl::GetNodeTypeImpl< KDOP< S, 16 > >::run
static NODE_TYPE run()
Definition: BVH_model-inl.h:1235
fcl::BV_kIOS
@ BV_kIOS
Definition: collision_geometry.h:53
fcl::BV_RSS
@ BV_RSS
Definition: collision_geometry.h:53
fcl::OT_BVH
@ OT_BVH
Definition: collision_geometry.h:50
fcl::BVH_ERR_INCORRECT_DATA
@ BVH_ERR_INCORRECT_DATA
BVH model update failed.
Definition: BVH_internal.h:70
fcl::BVH_ERR_UNSUPPORTED_FUNCTION
@ BVH_ERR_UNSUPPORTED_FUNCTION
BVH geometry in previous frame is not prepared.
Definition: BVH_internal.h:68
fcl::BV_OBBRSS
@ BV_OBBRSS
Definition: collision_geometry.h:53
fcl::BVHModel::bvs
BVNode< BV > * bvs
Bounding volume hierarchy.
Definition: BVH_model.h:194
fcl::RSS::axis
Matrix3< S > axis
Frame T's orientation in frame F.
Definition: RSS.h:70
fcl::NODE_TYPE
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_geometry.h:53
fcl::fit
FCL_EXPORT void fit(const Vector3< typename BV::S > *const ps, int n, BV &bv)
Compute a bounding volume that fits a set of n points.
Definition: math/bv/utility-inl.h:671
fcl::BVH_OK
@ BVH_OK
Definition: BVH_internal.h:63
fcl::BVH_MODEL_TRIANGLES
@ BVH_MODEL_TRIANGLES
unknown model type
Definition: BVH_internal.h:78
fcl::BV_AABB
@ BV_AABB
Definition: collision_geometry.h:53
fcl::BVHModel::getModelType
BVHModelType getModelType() const
Model type described by the instance.
Definition: BVH_model-inl.h:50
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::MakeParentRelativeRecurseImpl< S, OBBRSS< S > >::run
static void run(BVHModel< OBBRSS< S >> &model, int bv_id, const Matrix3< S > &parent_axis, const Vector3< S > &parent_c)
Definition: BVH_model-inl.h:1156
fcl::BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME
@ BVH_ERR_BUILD_EMPTY_PREVIOUS_FRAME
BVH geometry is not prepared.
Definition: BVH_internal.h:67
fcl::BVH_ERR_BUILD_OUT_OF_SEQUENCE
@ BVH_ERR_BUILD_OUT_OF_SEQUENCE
Cannot allocate memory for vertices and triangles.
Definition: BVH_internal.h:65
fcl::OBB::To
Vector3< S > To
Center of OBB.
Definition: OBB.h:65
fcl::GetNodeTypeImpl< KDOP< S, 24 > >::run
static NODE_TYPE run()
Definition: BVH_model-inl.h:1255
fcl::BVNodeBase::first_primitive
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
fcl::GetNodeTypeImpl< kIOS< S > >::run
static NODE_TYPE run()
Definition: BVH_model-inl.h:1215
fcl::GetNodeTypeImpl< RSS< S > >::run
static NODE_TYPE run()
Definition: BVH_model-inl.h:1205


fcl
Author(s):
autogenerated on Fri Apr 2 2021 02:37:59