==============================================================================
| 407 | |
| 408 | //============================================================================== |
| 409 | void testSphereSphere( |
| 410 | const std::shared_ptr<CollisionDetector>& cd, double tol = 1e-12) |
| 411 | { |
| 412 | auto simpleFrame1 = SimpleFrame::createShared(Frame::World()); |
| 413 | auto simpleFrame2 = SimpleFrame::createShared(Frame::World()); |
| 414 | |
| 415 | ShapePtr shape1(new SphereShape(1.0)); |
| 416 | ShapePtr shape2(new SphereShape(0.5)); |
| 417 | simpleFrame1->setShape(shape1); |
| 418 | simpleFrame2->setShape(shape2); |
| 419 | |
| 420 | auto group = cd->createCollisionGroup(simpleFrame1.get(), simpleFrame2.get()); |
| 421 | |
| 422 | EXPECT_EQ(group->getNumShapeFrames(), 2u); |
| 423 | |
| 424 | collision::CollisionOption option; |
| 425 | option.enableContact = true; |
| 426 | |
| 427 | collision::CollisionResult result; |
| 428 | |
| 429 | //---------------------------------------------------------------------------- |
| 430 | // Test 1: No contact |
| 431 | //---------------------------------------------------------------------------- |
| 432 | |
| 433 | simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); |
| 434 | simpleFrame2->setTranslation(Eigen::Vector3d(2.0, 0.0, 0.0)); |
| 435 | result.clear(); |
| 436 | EXPECT_FALSE(group->collide(option, &result)); |
| 437 | EXPECT_TRUE(result.getNumContacts() == 0u); |
| 438 | |
| 439 | //---------------------------------------------------------------------------- |
| 440 | // Test 2: Point contact |
| 441 | //---------------------------------------------------------------------------- |
| 442 | |
| 443 | simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); |
| 444 | simpleFrame2->setTranslation(Eigen::Vector3d(1.5 - tol, 0.0, 0.0)); |
| 445 | result.clear(); |
| 446 | EXPECT_TRUE(group->collide(option, &result)); |
| 447 | EXPECT_TRUE(result.getNumContacts() == 1u); |
| 448 | |
| 449 | const auto& contact = result.getContact(0); |
| 450 | |
| 451 | // Test contact location |
| 452 | EXPECT_TRUE(contact.point.isApprox(Eigen::Vector3d::UnitX(), 2.0 * tol)); |
| 453 | |
| 454 | // Test normal |
| 455 | Eigen::Vector3d expectedNormal; |
| 456 | if (result.getContact(0).collisionObject1->getShapeFrame() |
| 457 | == simpleFrame1.get()) |
| 458 | expectedNormal << -1, 0, 0; |
| 459 | else |
| 460 | expectedNormal << 1, 0, 0; |
| 461 | double tol2 = tol; |
| 462 | if (cd->getType() == FCLCollisionDetector::getStaticType() |
| 463 | && static_cast<FCLCollisionDetector*>(cd.get())->getPrimitiveShapeType() |
| 464 | == FCLCollisionDetector::MESH) { |
| 465 | tol2 *= 1e+12; |
| 466 | // FCL returns less accurate contact normals for sphere-sphere since we're |
no test coverage detected