Function testBoxBox

tests/integration/test_Collision.cpp:566–620

==============================================================================

//==============================================================================
void testBoxBox(
    const std::shared_ptr<CollisionDetector>& cd, double tol = 1e-12)
{
  auto simpleFrame1 = SimpleFrame::createShared(Frame::World());
  auto simpleFrame2 = SimpleFrame::createShared(Frame::World());

  ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0)));
  ShapePtr shape2(new BoxShape(Eigen::Vector3d(0.5, 0.5, 0.5)));
  simpleFrame1->setShape(shape1);
  simpleFrame2->setShape(shape2);

  Eigen::Vector3d pos1 = Eigen::Vector3d(0.0, 0.0, -0.5);
  Eigen::Vector3d pos2 = Eigen::Vector3d(0.0, 0.5, 0.25);
  simpleFrame1->setTranslation(pos1);
  simpleFrame2->setTranslation(pos2);

  auto group1 = cd->createCollisionGroup(simpleFrame1.get());
  auto group2 = cd->createCollisionGroup(simpleFrame2.get());
  auto groupAll = cd->createCollisionGroup(group1.get(), group2.get());

  EXPECT_EQ(group1->getNumShapeFrames(), 1u);
  EXPECT_EQ(group2->getNumShapeFrames(), 1u);
  EXPECT_EQ(
      groupAll->getNumShapeFrames(),
      group1->getNumShapeFrames() + group2->getNumShapeFrames());

  collision::CollisionOption option;
  collision::CollisionResult result;

  result.clear();
  EXPECT_TRUE(group1->collide(group2.get(), option, &result));

  result.clear();
  EXPECT_TRUE(groupAll->collide(option, &result));

  Eigen::Vector3d min = Eigen::Vector3d(-0.25, 0.25, 0.0);
  Eigen::Vector3d max = Eigen::Vector3d(0.25, 0.5, 0.0);

  const auto numContacts = result.getNumContacts();

  const auto checkNumContacts = (numContacts <= 4u);
  EXPECT_TRUE(checkNumContacts);
  if (!checkNumContacts)
    std::cout << "# of contants: " << numContacts << "\n";

  for (const auto& contact : result.getContacts()) {
    const auto& point = contact.point;

    const auto result = checkBoundingBox(min, max, point, tol);
    EXPECT_TRUE(result);

    if (!result)
      std::cout << "point: " << point.transpose() << "\n";
  }
}

//==============================================================================
TEST_F(Collision, BoxBox)