@@ -424,32 +424,30 @@ void testSphereSphere(
424
424
425
425
collision::CollisionResult result;
426
426
427
- simpleFrame1->setTranslation (Eigen::Vector3d::Zero ());
428
- simpleFrame2->setTranslation (Eigen::Vector3d (2.0 , 0.0 , 0.0 ));
429
-
430
427
// ----------------------------------------------------------------------------
431
428
// Test 1: No contact
432
429
// ----------------------------------------------------------------------------
433
430
431
+ simpleFrame1->setTranslation (Eigen::Vector3d::Zero ());
432
+ simpleFrame2->setTranslation (Eigen::Vector3d (2.0 , 0.0 , 0.0 ));
434
433
result.clear ();
435
434
EXPECT_FALSE (group->collide (option, &result));
436
435
EXPECT_TRUE (result.getNumContacts () == 0u );
437
436
438
- simpleFrame1->setTranslation (Eigen::Vector3d::Zero ());
439
- simpleFrame2->setTranslation (Eigen::Vector3d (1.5 , 0.0 , 0.0 ));
440
-
441
437
// ----------------------------------------------------------------------------
442
438
// Test 2: Point contact
443
439
// ----------------------------------------------------------------------------
444
440
441
+ simpleFrame1->setTranslation (Eigen::Vector3d::Zero ());
442
+ simpleFrame2->setTranslation (Eigen::Vector3d (1.5 - tol, 0.0 , 0.0 ));
445
443
result.clear ();
446
444
EXPECT_TRUE (group->collide (option, &result));
447
445
EXPECT_TRUE (result.getNumContacts () == 1u );
448
446
449
447
const auto & contact = result.getContact (0 );
450
448
451
449
// Test contact location
452
- EXPECT_TRUE (contact.point .isApprox (Eigen::Vector3d::UnitX (), tol));
450
+ EXPECT_TRUE (contact.point .isApprox (Eigen::Vector3d::UnitX (), 2.0 * tol));
453
451
454
452
// Test normal
455
453
Eigen::Vector3d expectedNormal;
@@ -479,10 +477,10 @@ void testSphereSphere(
479
477
if (cd->getType () == FCLCollisionDetector::getStaticType ()) {
480
478
EXPECT_FALSE (group->collide (option, &result));
481
479
// FCL is not able to detect collisions when an object completely (strictly)
482
- // contanins the other object (no collisions between the hulls)
480
+ // contains the other object (no collisions between the hulls)
483
481
} else {
484
482
EXPECT_TRUE (group->collide (option, &result));
485
- // TODO(JS): BulletCollsionDetector includes a bug related to this.
483
+ // TODO(JS): BulletCollisionDetector includes a bug related to this.
486
484
// (see #876)
487
485
#if HAVE_BULLET
488
486
if (cd->getType () != BulletCollisionDetector::getStaticType ())
@@ -502,10 +500,13 @@ void testSphereSphere(
502
500
// ==============================================================================
503
501
TEST_F (Collision, SphereSphere)
504
502
{
505
- auto fcl_mesh_dart = FCLCollisionDetector::create ();
506
- fcl_mesh_dart->setPrimitiveShapeType (FCLCollisionDetector::MESH);
507
- fcl_mesh_dart->setContactPointComputationMethod (FCLCollisionDetector::DART);
508
- testSphereSphere (fcl_mesh_dart);
503
+ {
504
+ SCOPED_TRACE (" FCLCollisionDetector" );
505
+ auto fcl_mesh_dart = FCLCollisionDetector::create ();
506
+ fcl_mesh_dart->setPrimitiveShapeType (FCLCollisionDetector::MESH);
507
+ fcl_mesh_dart->setContactPointComputationMethod (FCLCollisionDetector::DART);
508
+ testSphereSphere (fcl_mesh_dart);
509
+ }
509
510
510
511
// auto fcl_prim_fcl = FCLCollisionDetector::create();
511
512
// fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH);
@@ -523,17 +524,26 @@ TEST_F(Collision, SphereSphere)
523
524
// testSphereSphere(fcl_mesh_fcl);
524
525
525
526
#if HAVE_ODE
526
- auto ode = OdeCollisionDetector::create ();
527
- testSphereSphere (ode);
527
+ {
528
+ SCOPED_TRACE (" OdeCollisionDetector" );
529
+ auto ode = OdeCollisionDetector::create ();
530
+ testSphereSphere (ode);
531
+ }
528
532
#endif
529
533
530
534
#if HAVE_BULLET
531
- auto bullet = BulletCollisionDetector::create ();
532
- testSphereSphere (bullet);
535
+ {
536
+ SCOPED_TRACE (" BulletCollisionDetector" );
537
+ auto bullet = BulletCollisionDetector::create ();
538
+ testSphereSphere (bullet);
539
+ }
533
540
#endif
534
541
535
- auto dart = DARTCollisionDetector::create ();
536
- testSphereSphere (dart);
542
+ {
543
+ SCOPED_TRACE (" BulletCollisionDetector" );
544
+ auto dart = DARTCollisionDetector::create ();
545
+ testSphereSphere (dart);
546
+ }
537
547
}
538
548
539
549
// ==============================================================================
0 commit comments