@@ -315,34 +315,37 @@ void computeShapeBoundingSphere(const Shape *shape, Eigen::Vector3d& center, dou
315
315
else if (shape->type == BOX)
316
316
{
317
317
const double * sz = static_cast <const Box*>(shape)->size ;
318
- double x = sz[0 ] * 0.5 ;
319
- double y = sz[1 ] * 0.5 ;
320
- double z = sz[2 ] * 0.5 ;
321
- radius = std::sqrt (x*x + y*y + z*z);
318
+ double half_width = sz[0 ] * 0.5 ;
319
+ double half_height = sz[1 ] * 0.5 ;
320
+ double half_depth = sz[2 ] * 0.5 ;
321
+ radius = std::sqrt ( half_width * half_width +
322
+ half_height * half_height +
323
+ half_depth * half_depth);
322
324
}
323
325
else if (shape->type == CYLINDER)
324
326
{
325
- double r = static_cast <const Cylinder*>(shape)->radius ;
326
- double lo2 = static_cast <const Cylinder*>(shape)->length * 0.5 ;
327
- radius = std::sqrt (r * r + l * l);
327
+ double cyl_radius = static_cast <const Cylinder*>(shape)->radius ;
328
+ double half_len = static_cast <const Cylinder*>(shape)->length * 0.5 ;
329
+ radius = std::sqrt ( cyl_radius * cyl_radius +
330
+ half_len * half_len);
328
331
}
329
332
else if (shape->type == CONE)
330
333
{
331
- double r = static_cast <const Cone*>(shape)->radius ;
332
- double l = static_cast <const Cone*>(shape)->length ;
334
+ double cone_radius = static_cast <const Cone*>(shape)->radius ;
335
+ double cone_height = static_cast <const Cone*>(shape)->length ;
333
336
334
- if (l > r )
337
+ if (cone_height > cone_radius )
335
338
{
336
339
// center of sphere is intersection of perpendicular bisectors:
337
- double z = (l - (r * r / l )) * 0.5 ;
338
- center.z () = z - (l * 0.5 );
339
- radius = l - z;
340
+ double z = (cone_height - (cone_radius * cone_radius / cone_height )) * 0.5 ;
341
+ center.z () = z - (cone_height * 0.5 );
342
+ radius = cone_height - z;
340
343
}
341
344
else
342
345
{
343
346
// short cone. Bounding sphere touches base only.
344
- center.z () = - (l * 0.5 );
345
- radius = r ;
347
+ center.z () = - (cone_height * 0.5 );
348
+ radius = cone_radius ;
346
349
}
347
350
}
348
351
else if (shape->type == MESH)
0 commit comments