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