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
36 /** @author Jia Pan */
37
38 #ifndef FCL_BV_SPLITTER_INL_H
39 #define FCL_BV_SPLITTER_INL_H
40
41 #include "fcl/geometry/bvh/detail/BV_splitter.h"
42
43 #include "fcl/common/unused.h"
44
45 namespace fcl
46 {
47
48 namespace detail
49 {
50
51 //==============================================================================
52 template <typename BV>
BVSplitter(SplitMethodType method)53 BVSplitter<BV>::BVSplitter(SplitMethodType method)
54 : split_method(method)
55 {
56 // Do nothing
57 }
58
59 //==============================================================================
60 template <typename BV>
~BVSplitter()61 BVSplitter<BV>::~BVSplitter()
62 {
63 // Do nothing
64 }
65
66 //==============================================================================
67 template <typename BV>
set(Vector3<S> * vertices_,Triangle * tri_indices_,BVHModelType type_)68 void BVSplitter<BV>::set(
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>
computeRule(const BV & bv,unsigned int * primitive_indices,int num_primitives)78 void BVSplitter<BV>::computeRule(
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;
86 case SPLIT_METHOD_MEDIAN:
87 computeRule_median(bv, primitive_indices, num_primitives);
88 break;
89 case SPLIT_METHOD_BV_CENTER:
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 {
runApplyImpl101 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>
apply(const Vector3<S> & q)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>
117 struct ComputeRuleCenterImpl
118 {
runComputeRuleCenterImpl119 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>
computeRule_bvcenter(const BV & bv,unsigned int * primitive_indices,int num_primitives)140 void BVSplitter<BV>::computeRule_bvcenter(
141 const BV& bv, unsigned int* primitive_indices, int num_primitives)
142 {
143 ComputeRuleCenterImpl<S, BV>::run(
144 *this, bv, primitive_indices, num_primitives);
145 }
146
147 //==============================================================================
148 template <typename S, typename BV>
149 struct ComputeRuleMeanImpl
150 {
runComputeRuleMeanImpl151 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>
computeRule_mean(const BV & bv,unsigned int * primitive_indices,int num_primitives)193 void BVSplitter<BV>::computeRule_mean(
194 const BV& bv, unsigned int* primitive_indices, int num_primitives)
195 {
196 ComputeRuleMeanImpl<S, BV>::run(
197 *this, bv, primitive_indices, num_primitives);
198 }
199
200 //==============================================================================
201 template <typename S, typename BV>
202 struct ComputeRuleMedianImpl
203 {
runComputeRuleMedianImpl204 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>
computeRule_median(const BV & bv,unsigned int * primitive_indices,int num_primitives)251 void BVSplitter<BV>::computeRule_median(
252 const BV& bv, unsigned int* primitive_indices, int num_primitives)
253 {
254 ComputeRuleMedianImpl<S, BV>::run(
255 *this, bv, primitive_indices, num_primitives);
256 }
257
258 //==============================================================================
259 template <typename S>
260 struct ComputeRuleCenterImpl<S, OBB<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>
275 struct ComputeRuleMeanImpl<S, OBB<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>
292 struct ComputeRuleMedianImpl<S, OBB<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>
309 struct ComputeRuleCenterImpl<S, RSS<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>
324 struct ComputeRuleMeanImpl<S, RSS<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>
341 struct ComputeRuleMedianImpl<S, RSS<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>
358 struct ComputeRuleCenterImpl<S, kIOS<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>
373 struct ComputeRuleMeanImpl<S, kIOS<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>
390 struct ComputeRuleMedianImpl<S, kIOS<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>
407 struct ComputeRuleCenterImpl<S, OBBRSS<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>
422 struct ComputeRuleMeanImpl<S, OBBRSS<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>
439 struct ComputeRuleMedianImpl<S, OBBRSS<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>
504 void BVSplitter<BV>::clear()
505 {
506 vertices = nullptr;
507 tri_indices = nullptr;
508 type = BVH_MODEL_UNKNOWN;
509 }
510
511 //==============================================================================
512 template <typename S, typename BV>
513 struct ComputeSplitVectorImpl
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>
530 struct ComputeSplitVectorImpl<S, kIOS<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>
540 struct ComputeSplitVectorImpl<S, OBBRSS<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>
558 void computeSplitValue_mean(
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>
603 void computeSplitValue_median(
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
658