Normal and penetration depth in convex collision

[yanwenx020]

Hello,

Consider this scenario as shown below. There are two cubes in the scene, one big and one small. The center of the big cube lies at the origin of the world, while the center of the small one is on the diagonal of the big cube, e.g. the vertex of the big cube is at (0.5, 0.5, 0.5), and the center of the small cube is at (0.4, 0.4, 0.4). One of the cubes is represented as fcl::Convexf, the other one can be a fcl::Boxf, or convex as well.

The problem is, when I use fcl::collide function to detect the collision of these two cubes, and request the contact information, the returned normal direction starts from the blue point and is pointing to the yellow point, i.e. pointing to the vertex of the big cube. However, the minimum penetration depth direction should be from the blue point to the red point, i.e. orthogonal to one surface of the big cube, which is also the result when using two boxes (fcl::Boxf) as collision geometries. Accordingly, the penetration depth in this case is sqrt(3) times the actual minimum penetration depth, which is consistent to the normal direction.

As far as I know, this only happens when the deepest penetration point lies on the diagonal of one cube and at least one of the cube is represented as a convex. Could you please take a look at it? A code snippet is attached below.

      std::vector<fcl::Vector3f> vertices1{
          {-0.5f, -0.5f, -0.5f},
          {-0.5f, 0.5f, -0.5f},
          {-0.5f, 0.5f, 0.5f},
          {-0.5f, -0.5f, 0.5f},
          {0.5f, -0.5f, -0.5f},
          {0.5f, 0.5f, -0.5f},
          {0.5f, 0.5f, 0.5f},
          {0.5f, -0.5f, 0.5f}
      };
      std::vector<fcl::Vector3f> vertices2{
          {-0.01f, -0.01f, -0.01f},
          {-0.01f, 0.01f, -0.01f},
          {-0.01f, 0.01f, 0.01f},
          {-0.01f, -0.01f, 0.01f},
          {0.01f, -0.01f, -0.01f},
          {0.01f, 0.01f, -0.01f},
          {0.01f, 0.01f, 0.01f},
          {0.01f, -0.01f, 0.01f}
      };
      std::vector<int> faces{
          0, 3, 2, 1,
          4, 5, 6, 7,
          4, 7, 3, 0,
          5, 1, 2, 6,
          4, 0, 1, 5,
          7, 6, 2, 3
      };
      const int faceCount = 6;
      fcl::Convexf* convex1 = new fcl::Convexf(std::make_shared<const std::vector<fcl::Vector3f>>(vertices1), faceCount, std::make_shared<std::vector<int>>(faces));
      fcl::Convexf* convex2 = new fcl::Convexf(std::make_shared<const std::vector<fcl::Vector3f>>(vertices2), faceCount, std::make_shared<std::vector<int>>(faces));
      fcl::Transform3f trans1;
      trans1.linear() = Eigen::Quaternionf{ 1, 0, 0, 0 }.toRotationMatrix();
      trans1.translation() = fcl::Vector3f{ 0, 0, 0 };
      fcl::Transform3f trans2;
      trans2.linear() = Eigen::Quaternionf{ 1, 0, 0, 0 }.toRotationMatrix();
      trans2.translation() = fcl::Vector3f{ 0.4f, 0.4f, 0.4f };
      const fcl::CollisionRequestf req{ UINT_MAX, true };
      fcl::CollisionResultf res;
      fcl::collide(convex1, trans1, convex2, trans2, req, res);
      if (res.isCollision()) {
          std::vector<fcl::Contactf> contacts;
          res.getContacts(contacts);
          for (const auto& contact : contacts) {
              const auto normal = contact.normal;
              std::cout << "Contact normal: (" << normal.x() << ", " << normal.y() << ", " << normal.z() << "), penetration depth: " << contact.penetration_depth << std::endl;
          }
      }
      delete convex1;
      delete convex2;

Thanks!