BV_splitter-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_BV_SPLITTER_INL_H
39 #define FCL_BV_SPLITTER_INL_H
40 
42 
43 #include "fcl/common/unused.h"
44 
45 namespace fcl
46 {
47 
48 namespace detail
49 {
50 
51 //==============================================================================
52 template <typename BV>
54  : split_method(method)
55 {
56  // Do nothing
57 }
58 
59 //==============================================================================
60 template <typename BV>
62 {
63  // Do nothing
64 }
65 
66 //==============================================================================
67 template <typename BV>
69  Vector3<S>* vertices_, Triangle* tri_indices_, BVHModelType type_)
70 {
71  vertices = vertices_;
72  tri_indices = tri_indices_;
73  type = type_;
74 }
75 
76 //==============================================================================
77 template <typename BV>
79  const BV& bv, unsigned int* primitive_indices, int num_primitives)
80 {
81  switch(split_method)
82  {
83  case SPLIT_METHOD_MEAN:
84  computeRule_mean(bv, primitive_indices, num_primitives);
85  break;
87  computeRule_median(bv, primitive_indices, num_primitives);
88  break;
90  computeRule_bvcenter(bv, primitive_indices, num_primitives);
91  break;
92  default:
93  std::cerr << "Split method not supported\n";
94  }
95 }
96 
97 //==============================================================================
98 template <typename S, typename BV>
99 struct ApplyImpl
100 {
101  static bool run(
102  const BVSplitter<BV>& splitter, const Vector3<S>& q)
103  {
104  return q[splitter.split_axis] > splitter.split_value;
105  }
106 };
107 
108 //==============================================================================
109 template <typename BV>
110 bool BVSplitter<BV>::apply(const Vector3<S>& q) const
111 {
112  return ApplyImpl<S, BV>::run(*this, q);
113 }
114 
115 //==============================================================================
116 template <typename S, typename BV>
118 {
119  static void run(
120  BVSplitter<BV>& splitter,
121  const BV& bv,
122  unsigned int* /*primitive_indices*/,
123  int /*num_primitives*/)
124  {
125  Vector3<S> center = bv.center();
126  int axis = 2;
127 
128  if(bv.width() >= bv.height() && bv.width() >= bv.depth())
129  axis = 0;
130  else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
131  axis = 1;
132 
133  splitter.split_axis = axis;
134  splitter.split_value = center[axis];
135  }
136 };
137 
138 //==============================================================================
139 template <typename BV>
141  const BV& bv, unsigned int* primitive_indices, int num_primitives)
142 {
144  *this, bv, primitive_indices, num_primitives);
145 }
146 
147 //==============================================================================
148 template <typename S, typename BV>
150 {
151  static void run(
152  BVSplitter<BV>& splitter,
153  const BV& bv,
154  unsigned int* primitive_indices,
155  int num_primitives)
156  {
157  int axis = 2;
158 
159  if(bv.width() >= bv.height() && bv.width() >= bv.depth())
160  axis = 0;
161  else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
162  axis = 1;
163 
164  splitter.split_axis = axis;
165  S sum = 0;
166 
167  if(splitter.type == BVH_MODEL_TRIANGLES)
168  {
169  for(int i = 0; i < num_primitives; ++i)
170  {
171  const Triangle& t = splitter.tri_indices[primitive_indices[i]];
172  sum += splitter.vertices[t[0]][splitter.split_axis]
173  + splitter.vertices[t[1]][splitter.split_axis]
174  + splitter.vertices[t[2]][splitter.split_axis];
175  }
176 
177  sum /= 3;
178  }
179  else if(splitter.type == BVH_MODEL_POINTCLOUD)
180  {
181  for(int i = 0; i < num_primitives; ++i)
182  {
183  sum += splitter.vertices[primitive_indices[i]][splitter.split_axis];
184  }
185  }
186 
187  splitter.split_value = sum / num_primitives;
188  }
189 };
190 
191 //==============================================================================
192 template <typename BV>
194  const BV& bv, unsigned int* primitive_indices, int num_primitives)
195 {
197  *this, bv, primitive_indices, num_primitives);
198 }
199 
200 //==============================================================================
201 template <typename S, typename BV>
203 {
204  static void run(
205  BVSplitter<BV>& splitter,
206  const BV& bv,
207  unsigned int* primitive_indices,
208  int num_primitives)
209  {
210  int axis = 2;
211 
212  if(bv.width() >= bv.height() && bv.width() >= bv.depth())
213  axis = 0;
214  else if(bv.height() >= bv.width() && bv.height() >= bv.depth())
215  axis = 1;
216 
217  splitter.split_axis = axis;
218  std::vector<S> proj(num_primitives);
219 
220  if(splitter.type == BVH_MODEL_TRIANGLES)
221  {
222  for(int i = 0; i < num_primitives; ++i)
223  {
224  const Triangle& t = splitter.tri_indices[primitive_indices[i]];
225  proj[i] = (splitter.vertices[t[0]][splitter.split_axis]
226  + splitter.vertices[t[1]][splitter.split_axis]
227  + splitter.vertices[t[2]][splitter.split_axis]) / 3;
228  }
229  }
230  else if(splitter.type == BVH_MODEL_POINTCLOUD)
231  {
232  for(int i = 0; i < num_primitives; ++i)
233  proj[i] = splitter.vertices[primitive_indices[i]][splitter.split_axis];
234  }
235 
236  std::sort(proj.begin(), proj.end());
237 
238  if(num_primitives % 2 == 1)
239  {
240  splitter.split_value = proj[(num_primitives - 1) / 2];
241  }
242  else
243  {
244  splitter.split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2;
245  }
246  }
247 };
248 
249 //==============================================================================
250 template <typename BV>
252  const BV& bv, unsigned int* primitive_indices, int num_primitives)
253 {
255  *this, bv, primitive_indices, num_primitives);
256 }
257 
258 //==============================================================================
259 template <typename S>
261 {
262  static void run(
263  BVSplitter<OBB<S>>& splitter,
264  const OBB<S>& bv,
265  unsigned int* /*primitive_indices*/,
266  int /*num_primitives*/)
267  {
268  computeSplitVector<S, OBB<S>>(bv, splitter.split_vector);
269  computeSplitValue_bvcenter<S, OBB<S>>(bv, splitter.split_value);
270  }
271 };
272 
273 //==============================================================================
274 template <typename S>
276 {
277  static void run(
278  BVSplitter<OBB<S>>& splitter,
279  const OBB<S>& bv,
280  unsigned int* primitive_indices,
281  int num_primitives)
282  {
283  computeSplitVector<S, OBB<S>>(bv, splitter.split_vector);
284  computeSplitValue_mean<S, OBB<S>>(
285  bv, splitter.vertices, splitter.tri_indices, primitive_indices,
286  num_primitives, splitter.type, splitter.split_vector, splitter.split_value);
287  }
288 };
289 
290 //==============================================================================
291 template <typename S>
293 {
294  static void run(
295  BVSplitter<OBB<S>>& splitter,
296  const OBB<S>& bv,
297  unsigned int* primitive_indices,
298  int num_primitives)
299  {
300  computeSplitVector<S, OBB<S>>(bv, splitter.split_vector);
301  computeSplitValue_median<S, OBB<S>>(
302  bv, splitter.vertices, splitter.tri_indices, primitive_indices,
303  num_primitives, splitter.type, splitter.split_vector, splitter.split_value);
304  }
305 };
306 
307 //==============================================================================
308 template <typename S>
310 {
311  static void run(
312  BVSplitter<RSS<S>>& splitter,
313  const RSS<S>& bv,
314  unsigned int* /*primitive_indices*/,
315  int /*num_primitives*/)
316  {
317  computeSplitVector<S, RSS<S>>(bv, splitter.split_vector);
318  computeSplitValue_bvcenter<S, RSS<S>>(bv, splitter.split_value);
319  }
320 };
321 
322 //==============================================================================
323 template <typename S>
325 {
326  static void run(
327  BVSplitter<RSS<S>>& splitter,
328  const RSS<S>& bv,
329  unsigned int* primitive_indices,
330  int num_primitives)
331  {
332  computeSplitVector<S, RSS<S>>(bv, splitter.split_vector);
333  computeSplitValue_mean<S, RSS<S>>(
334  bv, splitter.vertices, splitter.tri_indices, primitive_indices,
335  num_primitives, splitter.type, splitter.split_vector, splitter.split_value);
336  }
337 };
338 
339 //==============================================================================
340 template <typename S>
342 {
343  static void run(
344  BVSplitter<RSS<S>>& splitter,
345  const RSS<S>& bv,
346  unsigned int* primitive_indices,
347  int num_primitives)
348  {
349  computeSplitVector<S, RSS<S>>(bv, splitter.split_vector);
350  computeSplitValue_median<S, RSS<S>>(
351  bv, splitter.vertices, splitter.tri_indices, primitive_indices,
352  num_primitives, splitter.type, splitter.split_vector, splitter.split_value);
353  }
354 };
355 
356 //==============================================================================
357 template <typename S>
359 {
360  static void run(
361  BVSplitter<kIOS<S>>& splitter,
362  const kIOS<S>& bv,
363  unsigned int* /*primitive_indices*/,
364  int /*num_primitives*/)
365  {
366  computeSplitVector<S, kIOS<S>>(bv, splitter.split_vector);
367  computeSplitValue_bvcenter<S, kIOS<S>>(bv, splitter.split_value);
368  }
369 };
370 
371 //==============================================================================
372 template <typename S>
374 {
375  static void run(
376  BVSplitter<kIOS<S>>& splitter,
377  const kIOS<S>& bv,
378  unsigned int* primitive_indices,
379  int num_primitives)
380  {
381  computeSplitVector<S, kIOS<S>>(bv, splitter.split_vector);
382  computeSplitValue_mean<S, kIOS<S>>(
383  bv, splitter.vertices, splitter.tri_indices, primitive_indices,
384  num_primitives, splitter.type, splitter.split_vector, splitter.split_value);
385  }
386 };
387 
388 //==============================================================================
389 template <typename S>
391 {
392  static void run(
393  BVSplitter<kIOS<S>>& splitter,
394  const kIOS<S>& bv,
395  unsigned int* primitive_indices,
396  int num_primitives)
397  {
398  computeSplitVector<S, kIOS<S>>(bv, splitter.split_vector);
399  computeSplitValue_median<S, kIOS<S>>(
400  bv, splitter.vertices, splitter.tri_indices, primitive_indices,
401  num_primitives, splitter.type, splitter.split_vector, splitter.split_value);
402  }
403 };
404 
405 //==============================================================================
406 template <typename S>
408 {
409  static void run(
410  BVSplitter<OBBRSS<S>>& splitter,
411  const OBBRSS<S>& bv,
412  unsigned int* /*primitive_indices*/,
413  int /*num_primitives*/)
414  {
415  computeSplitVector<S, OBBRSS<S>>(bv, splitter.split_vector);
416  computeSplitValue_bvcenter<S, OBBRSS<S>>(bv, splitter.split_value);
417  }
418 };
419 
420 //==============================================================================
421 template <typename S>
423 {
424  static void run(
425  BVSplitter<OBBRSS<S>>& splitter,
426  const OBBRSS<S>& bv,
427  unsigned int* primitive_indices,
428  int num_primitives)
429  {
430  computeSplitVector<S, OBBRSS<S>>(bv, splitter.split_vector);
431  computeSplitValue_mean<S, OBBRSS<S>>(
432  bv, splitter.vertices, splitter.tri_indices, primitive_indices,
433  num_primitives, splitter.type, splitter.split_vector, splitter.split_value);
434  }
435 };
436 
437 //==============================================================================
438 template <typename S>
440 {
441  static void run(
442  BVSplitter<OBBRSS<S>>& splitter,
443  const OBBRSS<S>& bv,
444  unsigned int* primitive_indices,
445  int num_primitives)
446  {
447  computeSplitVector<S, OBBRSS<S>>(bv, splitter.split_vector);
448  computeSplitValue_median<S, OBBRSS<S>>(
449  bv, splitter.vertices, splitter.tri_indices, primitive_indices,
450  num_primitives, splitter.type, splitter.split_vector, splitter.split_value);
451  }
452 };
453 
454 //==============================================================================
455 template <typename S>
456 struct ApplyImpl<S, OBB<S>>
457 {
458  static bool run(
459  const BVSplitter<OBB<S>>& splitter,
460  const Vector3<S>& q)
461  {
462  return splitter.split_vector.dot(q) > splitter.split_value;
463  }
464 };
465 
466 //==============================================================================
467 template <typename S>
468 struct ApplyImpl<S, RSS<S>>
469 {
470  static bool run(
471  const BVSplitter<RSS<S>>& splitter,
472  const Vector3<S>& q)
473  {
474  return splitter.split_vector.dot(q) > splitter.split_value;
475  }
476 };
477 
478 //==============================================================================
479 template <typename S>
480 struct ApplyImpl<S, kIOS<S>>
481 {
482  static bool run(
483  const BVSplitter<kIOS<S>>& splitter,
484  const Vector3<S>& q)
485  {
486  return splitter.split_vector.dot(q) > splitter.split_value;
487  }
488 };
489 
490 //==============================================================================
491 template <typename S>
492 struct ApplyImpl<S, OBBRSS<S>>
493 {
494  static bool run(
495  const BVSplitter<OBBRSS<S>>& splitter,
496  const Vector3<S>& q)
497  {
498  return splitter.split_vector.dot(q) > splitter.split_value;
499  }
500 };
501 
502 //==============================================================================
503 template <typename BV>
505 {
506  vertices = nullptr;
507  tri_indices = nullptr;
508  type = BVH_MODEL_UNKNOWN;
509 }
510 
511 //==============================================================================
512 template <typename S, typename BV>
514 {
515  static void run(const BV& bv, Vector3<S>& split_vector)
516  {
517  split_vector = bv.axis.col(0);
518  }
519 };
520 
521 //==============================================================================
522 template <typename S, typename BV>
523 void computeSplitVector(const BV& bv, Vector3<S>& split_vector)
524 {
525  ComputeSplitVectorImpl<S, BV>::run(bv, split_vector);
526 }
527 
528 //==============================================================================
529 template <typename S>
531 {
532  static void run(const kIOS<S>& bv, Vector3<S>& split_vector)
533  {
534  split_vector = bv.obb.axis.col(0);
535  }
536 };
537 
538 //==============================================================================
539 template <typename S>
541 {
542  static void run(const OBBRSS<S>& bv, Vector3<S>& split_vector)
543  {
544  split_vector = bv.obb.axis.col(0);
545  }
546 };
547 
548 //==============================================================================
549 template <typename S, typename BV>
550 void computeSplitValue_bvcenter(const BV& bv, S& split_value)
551 {
552  Vector3<S> center = bv.center();
553  split_value = center[0];
554 }
555 
556 //==============================================================================
557 template <typename S, typename BV>
559  const BV& bv,
560  Vector3<S>* vertices,
561  Triangle* triangles,
562  unsigned int* primitive_indices,
563  int num_primitives,
564  BVHModelType type,
565  const Vector3<S>& split_vector,
566  S& split_value)
567 {
568  FCL_UNUSED(bv);
569 
570  S sum = 0.0;
571  if(type == BVH_MODEL_TRIANGLES)
572  {
573  S c[3] = {0.0, 0.0, 0.0};
574 
575  for(int i = 0; i < num_primitives; ++i)
576  {
577  const Triangle& t = triangles[primitive_indices[i]];
578  const Vector3<S>& p1 = vertices[t[0]];
579  const Vector3<S>& p2 = vertices[t[1]];
580  const Vector3<S>& p3 = vertices[t[2]];
581 
582  c[0] += (p1[0] + p2[0] + p3[0]);
583  c[1] += (p1[1] + p2[1] + p3[1]);
584  c[2] += (p1[2] + p2[2] + p3[2]);
585  }
586  split_value = (c[0] * split_vector[0] + c[1] * split_vector[1] + c[2] * split_vector[2]) / (3 * num_primitives);
587  }
588  else if(type == BVH_MODEL_POINTCLOUD)
589  {
590  for(int i = 0; i < num_primitives; ++i)
591  {
592  const Vector3<S>& p = vertices[primitive_indices[i]];
593  Vector3<S> v(p[0], p[1], p[2]);
594  sum += v.dot(split_vector);
595  }
596 
597  split_value = sum / num_primitives;
598  }
599 }
600 
601 //==============================================================================
602 template <typename S, typename BV>
604  const BV& bv,
605  Vector3<S>* vertices,
606  Triangle* triangles,
607  unsigned int* primitive_indices,
608  int num_primitives,
609  BVHModelType type,
610  const Vector3<S>& split_vector,
611  S& split_value)
612 {
613  FCL_UNUSED(bv);
614 
615  std::vector<S> proj(num_primitives);
616 
617  if(type == BVH_MODEL_TRIANGLES)
618  {
619  for(int i = 0; i < num_primitives; ++i)
620  {
621  const Triangle& t = triangles[primitive_indices[i]];
622  const Vector3<S>& p1 = vertices[t[0]];
623  const Vector3<S>& p2 = vertices[t[1]];
624  const Vector3<S>& p3 = vertices[t[2]];
625  Vector3<S> centroid3(p1[0] + p2[0] + p3[0],
626  p1[1] + p2[1] + p3[1],
627  p1[2] + p2[2] + p3[2]);
628 
629  proj[i] = centroid3.dot(split_vector) / 3;
630  }
631  }
632  else if(type == BVH_MODEL_POINTCLOUD)
633  {
634  for(int i = 0; i < num_primitives; ++i)
635  {
636  const Vector3<S>& p = vertices[primitive_indices[i]];
637  Vector3<S> v(p[0], p[1], p[2]);
638  proj[i] = v.dot(split_vector);
639  }
640  }
641 
642  std::sort(proj.begin(), proj.end());
643 
644  if(num_primitives % 2 == 1)
645  {
646  split_value = proj[(num_primitives - 1) / 2];
647  }
648  else
649  {
650  split_value = (proj[num_primitives / 2] + proj[num_primitives / 2 - 1]) / 2;
651  }
652 }
653 
654 } // namespace detail
655 } // namespace fcl
656 
657 #endif
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::detail::SPLIT_METHOD_MEDIAN
@ SPLIT_METHOD_MEDIAN
Definition: BV_splitter.h:59
fcl::detail::ComputeRuleMeanImpl< S, OBBRSS< S > >::run
static void run(BVSplitter< OBBRSS< S >> &splitter, const OBBRSS< S > &bv, unsigned int *primitive_indices, int num_primitives)
Definition: BV_splitter-inl.h:424
fcl::detail::BVSplitter::~BVSplitter
virtual ~BVSplitter()
Default deconstructor.
Definition: BV_splitter-inl.h:61
fcl::kIOS
A class describing the kIOS collision structure, which is a set of spheres.
Definition: kIOS.h:48
fcl::detail::BVSplitter::computeRule_mean
void computeRule_mean(const BV &bv, unsigned int *primitive_indices, int num_primitives)
Split algorithm 2: Split the node according to the mean of the data contained.
Definition: BV_splitter-inl.h:193
fcl::BVH_MODEL_UNKNOWN
@ BVH_MODEL_UNKNOWN
Definition: BVH_internal.h:77
fcl::detail::ComputeRuleMedianImpl< S, RSS< S > >::run
static void run(BVSplitter< RSS< S >> &splitter, const RSS< S > &bv, unsigned int *primitive_indices, int num_primitives)
Definition: BV_splitter-inl.h:343
fcl::detail::ComputeRuleMeanImpl< S, OBB< S > >::run
static void run(BVSplitter< OBB< S >> &splitter, const OBB< S > &bv, unsigned int *primitive_indices, int num_primitives)
Definition: BV_splitter-inl.h:277
unused.h
fcl::detail::computeSplitValue_bvcenter
void computeSplitValue_bvcenter(const BV &bv, S &split_value)
Definition: BV_splitter-inl.h:550
fcl::detail::computeSplitValue_mean
void computeSplitValue_mean(const BV &bv, Vector3< S > *vertices, Triangle *triangles, unsigned int *primitive_indices, int num_primitives, BVHModelType type, const Vector3< S > &split_vector, S &split_value)
Definition: BV_splitter-inl.h:558
fcl::detail::ComputeSplitVectorImpl< S, OBBRSS< S > >::run
static void run(const OBBRSS< S > &bv, Vector3< S > &split_vector)
Definition: BV_splitter-inl.h:542
fcl::detail::BVSplitter::computeRule_bvcenter
void computeRule_bvcenter(const BV &bv, unsigned int *primitive_indices, int num_primitives)
Split algorithm 1: Split the node from center.
Definition: BV_splitter-inl.h:140
fcl::detail::SplitMethodType
SplitMethodType
Three types of split algorithms are provided in FCL as default.
Definition: BV_splitter.h:56
fcl::detail::ComputeRuleMedianImpl< S, kIOS< S > >::run
static void run(BVSplitter< kIOS< S >> &splitter, const kIOS< S > &bv, unsigned int *primitive_indices, int num_primitives)
Definition: BV_splitter-inl.h:392
fcl::detail::ComputeRuleMedianImpl::run
static void run(BVSplitter< BV > &splitter, const BV &bv, unsigned int *primitive_indices, int num_primitives)
Definition: BV_splitter-inl.h:204
fcl::detail::ComputeSplitVectorImpl::run
static void run(const BV &bv, Vector3< S > &split_vector)
Definition: BV_splitter-inl.h:515
fcl::detail::BVSplitter::computeRule_median
void computeRule_median(const BV &bv, unsigned int *primitive_indices, int num_primitives)
Split algorithm 3: Split the node according to the median of the data contained.
Definition: BV_splitter-inl.h:251
fcl::detail::BVSplitter::set
void set(Vector3< S > *vertices_, Triangle *tri_indices_, BVHModelType type_)
Set the geometry data needed by the split rule.
Definition: BV_splitter-inl.h:68
fcl::BVHModelType
BVHModelType
BVH model type.
Definition: BVH_internal.h:75
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::detail::ApplyImpl< S, OBB< S > >::run
static bool run(const BVSplitter< OBB< S >> &splitter, const Vector3< S > &q)
Definition: BV_splitter-inl.h:458
fcl::detail::ComputeRuleMeanImpl
Definition: BV_splitter-inl.h:149
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::detail::BVSplitter::type
BVHModelType type
Whether the geometry is mesh or point cloud.
Definition: BV_splitter.h:112
fcl::OBB
Oriented bounding box class.
Definition: OBB.h:51
fcl::detail::ComputeRuleCenterImpl< S, kIOS< S > >::run
static void run(BVSplitter< kIOS< S >> &splitter, const kIOS< S > &bv, unsigned int *, int)
Definition: BV_splitter-inl.h:360
fcl::detail::BVSplitter::computeRule
void computeRule(const BV &bv, unsigned int *primitive_indices, int num_primitives)
Compute the split rule according to a subset of geometry and the corresponding BV node.
Definition: BV_splitter-inl.h:78
fcl::OBBRSS::obb
OBB< S > obb
OBB member, for rotation.
Definition: OBBRSS.h:57
fcl::detail::computeSplitVector
void computeSplitVector(const BV &bv, Vector3< S > &split_vector)
Definition: BV_splitter-inl.h:523
fcl::detail::BVSplitter::split_axis
int split_axis
The axis based on which the split decision is made. For most BV, the axis is aligned with one of the ...
Definition: BV_splitter.h:97
fcl::detail::ComputeRuleMedianImpl
Definition: BV_splitter-inl.h:202
fcl::kIOS::obb
OBB< S > obb
OBB related with kIOS.
Definition: kIOS.h:72
fcl::detail::ApplyImpl
Definition: BV_splitter-inl.h:99
fcl::detail::ComputeRuleCenterImpl< S, OBB< S > >::run
static void run(BVSplitter< OBB< S >> &splitter, const OBB< S > &bv, unsigned int *, int)
Definition: BV_splitter-inl.h:262
fcl::detail::ComputeRuleMedianImpl< S, OBB< S > >::run
static void run(BVSplitter< OBB< S >> &splitter, const OBB< S > &bv, unsigned int *primitive_indices, int num_primitives)
Definition: BV_splitter-inl.h:294
fcl::BVH_MODEL_POINTCLOUD
@ BVH_MODEL_POINTCLOUD
triangle model
Definition: BVH_internal.h:79
fcl::detail::ApplyImpl< S, OBBRSS< S > >::run
static bool run(const BVSplitter< OBBRSS< S >> &splitter, const Vector3< S > &q)
Definition: BV_splitter-inl.h:494
fcl::detail::ComputeSplitVectorImpl< S, kIOS< S > >::run
static void run(const kIOS< S > &bv, Vector3< S > &split_vector)
Definition: BV_splitter-inl.h:532
fcl::detail::ComputeRuleCenterImpl< S, RSS< S > >::run
static void run(BVSplitter< RSS< S >> &splitter, const RSS< S > &bv, unsigned int *, int)
Definition: BV_splitter-inl.h:311
fcl::detail::ComputeRuleCenterImpl< S, OBBRSS< S > >::run
static void run(BVSplitter< OBBRSS< S >> &splitter, const OBBRSS< S > &bv, unsigned int *, int)
Definition: BV_splitter-inl.h:409
fcl::detail::BVSplitter::clear
void clear()
Clear the geometry data set before.
Definition: BV_splitter-inl.h:504
fcl::detail::SPLIT_METHOD_MEAN
@ SPLIT_METHOD_MEAN
Definition: BV_splitter.h:58
fcl::detail::SPLIT_METHOD_BV_CENTER
@ SPLIT_METHOD_BV_CENTER
Definition: BV_splitter.h:60
fcl::detail::ComputeRuleMeanImpl::run
static void run(BVSplitter< BV > &splitter, const BV &bv, unsigned int *primitive_indices, int num_primitives)
Definition: BV_splitter-inl.h:151
fcl::OBBRSS
Class merging the OBB and RSS, can handle collision and distance simultaneously.
Definition: OBBRSS.h:50
fcl::detail::computeSplitValue_median
void computeSplitValue_median(const BV &bv, Vector3< S > *vertices, Triangle *triangles, unsigned int *primitive_indices, int num_primitives, BVHModelType type, const Vector3< S > &split_vector, S &split_value)
Definition: BV_splitter-inl.h:603
fcl::detail::ComputeRuleCenterImpl::run
static void run(BVSplitter< BV > &splitter, const BV &bv, unsigned int *, int)
Definition: BV_splitter-inl.h:119
fcl::detail::ApplyImpl::run
static bool run(const BVSplitter< BV > &splitter, const Vector3< S > &q)
Definition: BV_splitter-inl.h:101
BV_splitter.h
fcl::detail::ComputeRuleMeanImpl< S, kIOS< S > >::run
static void run(BVSplitter< kIOS< S >> &splitter, const kIOS< S > &bv, unsigned int *primitive_indices, int num_primitives)
Definition: BV_splitter-inl.h:375
FCL_UNUSED
#define FCL_UNUSED(x)
Definition: unused.h:39
fcl::detail::ApplyImpl< S, RSS< S > >::run
static bool run(const BVSplitter< RSS< S >> &splitter, const Vector3< S > &q)
Definition: BV_splitter-inl.h:470
fcl::detail::ComputeSplitVectorImpl
Definition: BV_splitter-inl.h:513
fcl::detail::ComputeRuleMedianImpl< S, OBBRSS< S > >::run
static void run(BVSplitter< OBBRSS< S >> &splitter, const OBBRSS< S > &bv, unsigned int *primitive_indices, int num_primitives)
Definition: BV_splitter-inl.h:441
fcl::detail::BVSplitter::vertices
Vector3< S > * vertices
The mesh vertices or points handled by the splitter.
Definition: BV_splitter.h:106
fcl::detail::BVSplitter::tri_indices
Triangle * tri_indices
The triangles handled by the splitter.
Definition: BV_splitter.h:109
fcl::detail::BVSplitter::BVSplitter
BVSplitter(SplitMethodType method)
Definition: BV_splitter-inl.h:53
fcl::detail::BVSplitter
A class describing the split rule that splits each BV node.
Definition: BV_splitter.h:65
fcl::detail::BVSplitter::apply
bool apply(const Vector3< S > &q) const
Apply the split rule on a given point.
Definition: BV_splitter-inl.h:110
fcl::detail::BVSplitter::split_value
S split_value
The split threshold, different primitives are splitted according whether their projection on the spli...
Definition: BV_splitter.h:103
fcl::detail::ComputeRuleCenterImpl
Definition: BV_splitter-inl.h:117
fcl::BVH_MODEL_TRIANGLES
@ BVH_MODEL_TRIANGLES
unknown model type
Definition: BVH_internal.h:78
fcl::detail::ComputeRuleMeanImpl< S, RSS< S > >::run
static void run(BVSplitter< RSS< S >> &splitter, const RSS< S > &bv, unsigned int *primitive_indices, int num_primitives)
Definition: BV_splitter-inl.h:326
fcl::detail::ApplyImpl< S, kIOS< S > >::run
static bool run(const BVSplitter< kIOS< S >> &splitter, const Vector3< S > &q)
Definition: BV_splitter-inl.h:482
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:48