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 #include "fcl/BVH/BV_fitter.h"
39 #include "fcl/BVH/BVH_utility.h"
40 #include <limits>
41
42 namespace fcl
43 {
44
45
46 static const double kIOS_RATIO = 1.5;
47 static const double invSinA = 2;
48 static const double invCosA = 2.0 / sqrt(3.0);
49 // static const double sinA = 0.5;
50 static const double cosA = sqrt(3.0) / 2.0;
51
axisFromEigen(Vec3f eigenV[3],Matrix3f::U eigenS[3],Vec3f axis[3])52 static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::U eigenS[3], Vec3f axis[3])
53 {
54 int min, mid, max;
55 if(eigenS[0] > eigenS[1]) { max = 0; min = 1; }
56 else { min = 0; max = 1; }
57 if(eigenS[2] < eigenS[min]) { mid = min; min = 2; }
58 else if(eigenS[2] > eigenS[max]) { mid = max; max = 2; }
59 else { mid = 2; }
60
61 axis[0].setValue(eigenV[0][max], eigenV[1][max], eigenV[2][max]);
62 axis[1].setValue(eigenV[0][mid], eigenV[1][mid], eigenV[2][mid]);
63 axis[2].setValue(eigenV[1][max]*eigenV[2][mid] - eigenV[1][mid]*eigenV[2][max],
64 eigenV[0][mid]*eigenV[2][max] - eigenV[0][max]*eigenV[2][mid],
65 eigenV[0][max]*eigenV[1][mid] - eigenV[0][mid]*eigenV[1][max]);
66 }
67
68 namespace OBB_fit_functions
69 {
70
fit1(Vec3f * ps,OBB & bv)71 void fit1(Vec3f* ps, OBB& bv)
72 {
73 bv.To = ps[0];
74 bv.axis[0].setValue(1, 0, 0);
75 bv.axis[1].setValue(0, 1, 0);
76 bv.axis[2].setValue(0, 0, 1);
77 bv.extent.setValue(0);
78 }
79
fit2(Vec3f * ps,OBB & bv)80 void fit2(Vec3f* ps, OBB& bv)
81 {
82 const Vec3f& p1 = ps[0];
83 const Vec3f& p2 = ps[1];
84 Vec3f p1p2 = p1 - p2;
85 FCL_REAL len_p1p2 = p1p2.length();
86 p1p2.normalize();
87
88 bv.axis[0] = p1p2;
89 generateCoordinateSystem(bv.axis[0], bv.axis[1], bv.axis[2]);
90
91 bv.extent.setValue(len_p1p2 * 0.5, 0, 0);
92 bv.To.setValue(0.5 * (p1[0] + p2[0]),
93 0.5 * (p1[1] + p2[1]),
94 0.5 * (p1[2] + p2[2]));
95 }
96
fit3(Vec3f * ps,OBB & bv)97 void fit3(Vec3f* ps, OBB& bv)
98 {
99 const Vec3f& p1 = ps[0];
100 const Vec3f& p2 = ps[1];
101 const Vec3f& p3 = ps[2];
102 Vec3f e[3];
103 e[0] = p1 - p2;
104 e[1] = p2 - p3;
105 e[2] = p3 - p1;
106 FCL_REAL len[3];
107 len[0] = e[0].sqrLength();
108 len[1] = e[1].sqrLength();
109 len[2] = e[2].sqrLength();
110
111 int imax = 0;
112 if(len[1] > len[0]) imax = 1;
113 if(len[2] > len[imax]) imax = 2;
114
115 Vec3f& u = bv.axis[0];
116 Vec3f& v = bv.axis[1];
117 Vec3f& w = bv.axis[2];
118
119 w = e[0].cross(e[1]);
120 w.normalize();
121 u = e[imax];
122 u.normalize();
123 v = w.cross(u);
124
125 getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.axis, bv.To, bv.extent);
126 }
127
fit6(Vec3f * ps,OBB & bv)128 void fit6(Vec3f* ps, OBB& bv)
129 {
130 OBB bv1, bv2;
131 fit3(ps, bv1);
132 fit3(ps + 3, bv2);
133 bv = bv1 + bv2;
134 }
135
136
fitn(Vec3f * ps,int n,OBB & bv)137 void fitn(Vec3f* ps, int n, OBB& bv)
138 {
139 Matrix3f M;
140 Vec3f E[3];
141 Matrix3f::U s[3] = {0, 0, 0}; // three eigen values
142
143 getCovariance(ps, NULL, NULL, NULL, n, M);
144 eigen(M, s, E);
145 axisFromEigen(E, s, bv.axis);
146
147 // set obb centers and extensions
148 getExtentAndCenter(ps, NULL, NULL, NULL, n, bv.axis, bv.To, bv.extent);
149 }
150
151 }
152
153
154 namespace RSS_fit_functions
155 {
fit1(Vec3f * ps,RSS & bv)156 void fit1(Vec3f* ps, RSS& bv)
157 {
158 bv.Tr = ps[0];
159 bv.axis[0].setValue(1, 0, 0);
160 bv.axis[1].setValue(0, 1, 0);
161 bv.axis[2].setValue(0, 0, 1);
162 bv.l[0] = 0;
163 bv.l[1] = 0;
164 bv.r = 0;
165 }
166
fit2(Vec3f * ps,RSS & bv)167 void fit2(Vec3f* ps, RSS& bv)
168 {
169 const Vec3f& p1 = ps[0];
170 const Vec3f& p2 = ps[1];
171 Vec3f p1p2 = p1 - p2;
172 FCL_REAL len_p1p2 = p1p2.length();
173 p1p2.normalize();
174
175 bv.axis[0] = p1p2;
176 generateCoordinateSystem(bv.axis[0], bv.axis[1], bv.axis[2]);
177 bv.l[0] = len_p1p2;
178 bv.l[1] = 0;
179
180 bv.Tr = p2;
181 bv.r = 0;
182 }
183
fit3(Vec3f * ps,RSS & bv)184 void fit3(Vec3f* ps, RSS& bv)
185 {
186 const Vec3f& p1 = ps[0];
187 const Vec3f& p2 = ps[1];
188 const Vec3f& p3 = ps[2];
189 Vec3f e[3];
190 e[0] = p1 - p2;
191 e[1] = p2 - p3;
192 e[2] = p3 - p1;
193 FCL_REAL len[3];
194 len[0] = e[0].sqrLength();
195 len[1] = e[1].sqrLength();
196 len[2] = e[2].sqrLength();
197
198 int imax = 0;
199 if(len[1] > len[0]) imax = 1;
200 if(len[2] > len[imax]) imax = 2;
201
202 Vec3f& u = bv.axis[0];
203 Vec3f& v = bv.axis[1];
204 Vec3f& w = bv.axis[2];
205
206 w = e[0].cross(e[1]);
207 w.normalize();
208 u = e[imax];
209 u.normalize();
210 v = w.cross(u);
211
212 getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axis, bv.Tr, bv.l, bv.r);
213 }
214
fit6(Vec3f * ps,RSS & bv)215 void fit6(Vec3f* ps, RSS& bv)
216 {
217 RSS bv1, bv2;
218 fit3(ps, bv1);
219 fit3(ps + 3, bv2);
220 bv = bv1 + bv2;
221 }
222
fitn(Vec3f * ps,int n,RSS & bv)223 void fitn(Vec3f* ps, int n, RSS& bv)
224 {
225 Matrix3f M; // row first matrix
226 Vec3f E[3]; // row first eigen-vectors
227 Matrix3f::U s[3] = {0, 0, 0};
228
229 getCovariance(ps, NULL, NULL, NULL, n, M);
230 eigen(M, s, E);
231 axisFromEigen(E, s, bv.axis);
232
233 // set rss origin, rectangle size and radius
234 getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axis, bv.Tr, bv.l, bv.r);
235 }
236
237 }
238
239 namespace kIOS_fit_functions
240 {
241
fit1(Vec3f * ps,kIOS & bv)242 void fit1(Vec3f* ps, kIOS& bv)
243 {
244 bv.num_spheres = 1;
245 bv.spheres[0].o = ps[0];
246 bv.spheres[0].r = 0;
247
248 bv.obb.axis[0].setValue(1, 0, 0);
249 bv.obb.axis[1].setValue(0, 1, 0);
250 bv.obb.axis[2].setValue(0, 0, 1);
251 bv.obb.extent.setValue(0);
252 bv.obb.To = ps[0];
253 }
254
fit2(Vec3f * ps,kIOS & bv)255 void fit2(Vec3f* ps, kIOS& bv)
256 {
257 bv.num_spheres = 5;
258
259 const Vec3f& p1 = ps[0];
260 const Vec3f& p2 = ps[1];
261 Vec3f p1p2 = p1 - p2;
262 FCL_REAL len_p1p2 = p1p2.length();
263 p1p2.normalize();
264
265 Vec3f* axis = bv.obb.axis;
266 axis[0] = p1p2;
267 generateCoordinateSystem(axis[0], axis[1], axis[2]);
268
269 FCL_REAL r0 = len_p1p2 * 0.5;
270 bv.obb.extent.setValue(r0, 0, 0);
271 bv.obb.To = (p1 + p2) * 0.5;
272
273 bv.spheres[0].o = bv.obb.To;
274 bv.spheres[0].r = r0;
275
276 FCL_REAL r1 = r0 * invSinA;
277 FCL_REAL r1cosA = r1 * cosA;
278 bv.spheres[1].r = r1;
279 bv.spheres[2].r = r1;
280 Vec3f delta = axis[1] * r1cosA;
281 bv.spheres[1].o = bv.spheres[0].o - delta;
282 bv.spheres[2].o = bv.spheres[0].o + delta;
283
284 bv.spheres[3].r = r1;
285 bv.spheres[4].r = r1;
286 delta = axis[2] * r1cosA;
287 bv.spheres[3].o = bv.spheres[0].o - delta;
288 bv.spheres[4].o = bv.spheres[0].o + delta;
289 }
290
fit3(Vec3f * ps,kIOS & bv)291 void fit3(Vec3f* ps, kIOS& bv)
292 {
293 bv.num_spheres = 3;
294
295 const Vec3f& p1 = ps[0];
296 const Vec3f& p2 = ps[1];
297 const Vec3f& p3 = ps[2];
298 Vec3f e[3];
299 e[0] = p1 - p2;
300 e[1] = p2 - p3;
301 e[2] = p3 - p1;
302 FCL_REAL len[3];
303 len[0] = e[0].sqrLength();
304 len[1] = e[1].sqrLength();
305 len[2] = e[2].sqrLength();
306
307 int imax = 0;
308 if(len[1] > len[0]) imax = 1;
309 if(len[2] > len[imax]) imax = 2;
310
311 Vec3f& u = bv.obb.axis[0];
312 Vec3f& v = bv.obb.axis[1];
313 Vec3f& w = bv.obb.axis[2];
314
315 w = e[0].cross(e[1]);
316 w.normalize();
317 u = e[imax];
318 u.normalize();
319 v = w.cross(u);
320
321 getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.obb.axis, bv.obb.To, bv.obb.extent);
322
323 // compute radius and center
324 FCL_REAL r0;
325 Vec3f center;
326 circumCircleComputation(p1, p2, p3, center, r0);
327
328 bv.spheres[0].o = center;
329 bv.spheres[0].r = r0;
330
331 FCL_REAL r1 = r0 * invSinA;
332 Vec3f delta = bv.obb.axis[2] * (r1 * cosA);
333
334 bv.spheres[1].r = r1;
335 bv.spheres[1].o = center - delta;
336 bv.spheres[2].r = r1;
337 bv.spheres[2].o = center + delta;
338 }
339
fitn(Vec3f * ps,int n,kIOS & bv)340 void fitn(Vec3f* ps, int n, kIOS& bv)
341 {
342 Matrix3f M;
343 Vec3f E[3];
344 Matrix3f::U s[3] = {0, 0, 0}; // three eigen values;
345
346 getCovariance(ps, NULL, NULL, NULL, n, M);
347 eigen(M, s, E);
348
349 Vec3f* axis = bv.obb.axis;
350 axisFromEigen(E, s, axis);
351
352 getExtentAndCenter(ps, NULL, NULL, NULL, n, axis, bv.obb.To, bv.obb.extent);
353
354 // get center and extension
355 const Vec3f& center = bv.obb.To;
356 const Vec3f& extent = bv.obb.extent;
357 FCL_REAL r0 = maximumDistance(ps, NULL, NULL, NULL, n, center);
358
359 // decide the k in kIOS
360 if(extent[0] > kIOS_RATIO * extent[2])
361 {
362 if(extent[0] > kIOS_RATIO * extent[1]) bv.num_spheres = 5;
363 else bv.num_spheres = 3;
364 }
365 else bv.num_spheres = 1;
366
367
368 bv.spheres[0].o = center;
369 bv.spheres[0].r = r0;
370
371 if(bv.num_spheres >= 3)
372 {
373 FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
374 Vec3f delta = axis[2] * (r10 * cosA - extent[2]);
375 bv.spheres[1].o = center - delta;
376 bv.spheres[2].o = center + delta;
377
378 FCL_REAL r11 = 0, r12 = 0;
379 r11 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[1].o);
380 r12 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[2].o);
381 bv.spheres[1].o += axis[2] * (-r10 + r11);
382 bv.spheres[2].o += axis[2] * (r10 - r12);
383
384 bv.spheres[1].r = r10;
385 bv.spheres[2].r = r10;
386 }
387
388 if(bv.num_spheres >= 5)
389 {
390 FCL_REAL r10 = bv.spheres[1].r;
391 Vec3f delta = axis[1] * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]);
392 bv.spheres[3].o = bv.spheres[0].o - delta;
393 bv.spheres[4].o = bv.spheres[0].o + delta;
394
395 FCL_REAL r21 = 0, r22 = 0;
396 r21 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[3].o);
397 r22 = maximumDistance(ps, NULL, NULL, NULL, n, bv.spheres[4].o);
398
399 bv.spheres[3].o += axis[1] * (-r10 + r21);
400 bv.spheres[4].o += axis[1] * (r10 - r22);
401
402 bv.spheres[3].r = r10;
403 bv.spheres[4].r = r10;
404 }
405 }
406
407 }
408
409 namespace OBBRSS_fit_functions
410 {
fit1(Vec3f * ps,OBBRSS & bv)411 void fit1(Vec3f* ps, OBBRSS& bv)
412 {
413 OBB_fit_functions::fit1(ps, bv.obb);
414 RSS_fit_functions::fit1(ps, bv.rss);
415 }
416
fit2(Vec3f * ps,OBBRSS & bv)417 void fit2(Vec3f* ps, OBBRSS& bv)
418 {
419 OBB_fit_functions::fit2(ps, bv.obb);
420 RSS_fit_functions::fit2(ps, bv.rss);
421 }
422
fit3(Vec3f * ps,OBBRSS & bv)423 void fit3(Vec3f* ps, OBBRSS& bv)
424 {
425 OBB_fit_functions::fit3(ps, bv.obb);
426 RSS_fit_functions::fit3(ps, bv.rss);
427 }
428
fitn(Vec3f * ps,int n,OBBRSS & bv)429 void fitn(Vec3f* ps, int n, OBBRSS& bv)
430 {
431 OBB_fit_functions::fitn(ps, n, bv.obb);
432 RSS_fit_functions::fitn(ps, n, bv.rss);
433 }
434
435 }
436
437
438
439 template<>
fit(Vec3f * ps,int n,OBB & bv)440 void fit(Vec3f* ps, int n, OBB& bv)
441 {
442 switch(n)
443 {
444 case 1:
445 OBB_fit_functions::fit1(ps, bv);
446 break;
447 case 2:
448 OBB_fit_functions::fit2(ps, bv);
449 break;
450 case 3:
451 OBB_fit_functions::fit3(ps, bv);
452 break;
453 case 6:
454 OBB_fit_functions::fit6(ps, bv);
455 break;
456 default:
457 OBB_fit_functions::fitn(ps, n, bv);
458 }
459 }
460
461
462 template<>
fit(Vec3f * ps,int n,RSS & bv)463 void fit(Vec3f* ps, int n, RSS& bv)
464 {
465 switch(n)
466 {
467 case 1:
468 RSS_fit_functions::fit1(ps, bv);
469 break;
470 case 2:
471 RSS_fit_functions::fit2(ps, bv);
472 break;
473 case 3:
474 RSS_fit_functions::fit3(ps, bv);
475 break;
476 default:
477 RSS_fit_functions::fitn(ps, n, bv);
478 }
479 }
480
481 template<>
fit(Vec3f * ps,int n,kIOS & bv)482 void fit(Vec3f* ps, int n, kIOS& bv)
483 {
484 switch(n)
485 {
486 case 1:
487 kIOS_fit_functions::fit1(ps, bv);
488 break;
489 case 2:
490 kIOS_fit_functions::fit2(ps, bv);
491 break;
492 case 3:
493 kIOS_fit_functions::fit3(ps, bv);
494 break;
495 default:
496 kIOS_fit_functions::fitn(ps, n, bv);
497 }
498 }
499
500 template<>
fit(Vec3f * ps,int n,OBBRSS & bv)501 void fit(Vec3f* ps, int n, OBBRSS& bv)
502 {
503 switch(n)
504 {
505 case 1:
506 OBBRSS_fit_functions::fit1(ps, bv);
507 break;
508 case 2:
509 OBBRSS_fit_functions::fit2(ps, bv);
510 break;
511 case 3:
512 OBBRSS_fit_functions::fit3(ps, bv);
513 break;
514 default:
515 OBBRSS_fit_functions::fitn(ps, n, bv);
516 }
517 }
518
519
fit(unsigned int * primitive_indices,int num_primitives)520 OBB BVFitter<OBB>::fit(unsigned int* primitive_indices, int num_primitives)
521 {
522 OBB bv;
523
524 Matrix3f M; // row first matrix
525 Vec3f E[3]; // row first eigen-vectors
526 Matrix3f::U s[3]; // three eigen values
527
528 getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
529 eigen(M, s, E);
530
531 axisFromEigen(E, s, bv.axis);
532
533 // set obb centers and extensions
534 getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axis, bv.To, bv.extent);
535
536 return bv;
537 }
538
fit(unsigned int * primitive_indices,int num_primitives)539 OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices, int num_primitives)
540 {
541 OBBRSS bv;
542 Matrix3f M;
543 Vec3f E[3];
544 Matrix3f::U s[3];
545
546 getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
547 eigen(M, s, E);
548
549 axisFromEigen(E, s, bv.obb.axis);
550 bv.rss.axis[0] = bv.obb.axis[0];
551 bv.rss.axis[1] = bv.obb.axis[1];
552 bv.rss.axis[2] = bv.obb.axis[2];
553
554 getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.obb.axis, bv.obb.To, bv.obb.extent);
555
556 Vec3f origin;
557 FCL_REAL l[2];
558 FCL_REAL r;
559 getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.rss.axis, origin, l, r);
560
561 bv.rss.Tr = origin;
562 bv.rss.l[0] = l[0];
563 bv.rss.l[1] = l[1];
564 bv.rss.r = r;
565
566 return bv;
567 }
568
fit(unsigned int * primitive_indices,int num_primitives)569 RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
570 {
571 RSS bv;
572
573 Matrix3f M; // row first matrix
574 Vec3f E[3]; // row first eigen-vectors
575 Matrix3f::U s[3]; // three eigen values
576 getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
577 eigen(M, s, E);
578 axisFromEigen(E, s, bv.axis);
579
580 // set rss origin, rectangle size and radius
581
582 Vec3f origin;
583 FCL_REAL l[2];
584 FCL_REAL r;
585 getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axis, origin, l, r);
586
587 bv.Tr = origin;
588 bv.l[0] = l[0];
589 bv.l[1] = l[1];
590 bv.r = r;
591
592
593 return bv;
594 }
595
596
fit(unsigned int * primitive_indices,int num_primitives)597 kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, int num_primitives)
598 {
599 kIOS bv;
600
601 Matrix3f M; // row first matrix
602 Vec3f E[3]; // row first eigen-vectors
603 Matrix3f::U s[3];
604
605 getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
606 eigen(M, s, E);
607
608 Vec3f* axis = bv.obb.axis;
609 axisFromEigen(E, s, axis);
610
611 // get centers and extensions
612 getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, axis, bv.obb.To, bv.obb.extent);
613
614 const Vec3f& center = bv.obb.To;
615 const Vec3f& extent = bv.obb.extent;
616 FCL_REAL r0 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, center);
617
618 // decide k in kIOS
619 if(extent[0] > kIOS_RATIO * extent[2])
620 {
621 if(extent[0] > kIOS_RATIO * extent[1]) bv.num_spheres = 5;
622 else bv.num_spheres = 3;
623 }
624 else bv.num_spheres = 1;
625
626 bv.spheres[0].o = center;
627 bv.spheres[0].r = r0;
628
629 if(bv.num_spheres >= 3)
630 {
631 FCL_REAL r10 = sqrt(r0 * r0 - extent[2] * extent[2]) * invSinA;
632 Vec3f delta = axis[2] * (r10 * cosA - extent[2]);
633 bv.spheres[1].o = center - delta;
634 bv.spheres[2].o = center + delta;
635
636 FCL_REAL r11 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[1].o);
637 FCL_REAL r12 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[2].o);
638
639 bv.spheres[1].o += axis[2] * (-r10 + r11);
640 bv.spheres[2].o += axis[2] * (r10 - r12);
641
642 bv.spheres[1].r = r10;
643 bv.spheres[2].r = r10;
644 }
645
646 if(bv.num_spheres >= 5)
647 {
648 FCL_REAL r10 = bv.spheres[1].r;
649 Vec3f delta = axis[1] * (sqrt(r10 * r10 - extent[0] * extent[0] - extent[2] * extent[2]) - extent[1]);
650 bv.spheres[3].o = bv.spheres[0].o - delta;
651 bv.spheres[4].o = bv.spheres[0].o + delta;
652
653 FCL_REAL r21 = 0, r22 = 0;
654 r21 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[3].o);
655 r22 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.spheres[4].o);
656
657 bv.spheres[3].o += axis[1] * (-r10 + r21);
658 bv.spheres[4].o += axis[1] * (r10 - r22);
659
660 bv.spheres[3].r = r10;
661 bv.spheres[4].r = r10;
662 }
663
664 return bv;
665 }
666
667
668 }
669