BVHModel security_margin check error #755
Description
hi,I did a small test case, BVHModel security_margin, I set security_margin to 1, but when I set box2 pose to Transform3s(Vec3s(0.0, 0.0, 1.7)) there is no collision, Is this a bug?
void cteateBVH(coal::BVHModel<coal::kIOS>* model,double a, double b, double c) {
std::vector<coal::Vec3s> points(8);
std::vector<coal::Triangle> tri_indices(12);
points[0] << a, -b, c;
points[1] << a, b, c;
points[2] << -a, b, c;
points[3] << -a, -b, c;
points[4] << a, -b, -c;
points[5] << a, b, -c;
points[6] << -a, b, -c;
points[7] << -a, -b, -c;
tri_indices[0].set(0, 4, 1);
tri_indices[1].set(1, 4, 5);
tri_indices[2].set(2, 6, 3);
tri_indices[3].set(3, 6, 7);
tri_indices[4].set(3, 0, 2);
tri_indices[5].set(2, 0, 1);
tri_indices[6].set(6, 5, 7);
tri_indices[7].set(7, 5, 4);
tri_indices[8].set(1, 5, 2);
tri_indices[9].set(2, 5, 6);
tri_indices[10].set(3, 7, 0);
tri_indices[11].set(0, 7, 4);
int result;
result = model->beginModel();
for (std::size_t i = 0; i < tri_indices.size(); ++i) {
result =
model->addTriangle(points[tri_indices[i][0]], points[tri_indices[i][1]],
points[tri_indices[i][2]]);
}
result = model->endModel();
model->computeLocalAABB();
}
CollisionResult boxBVHTest(Transform3s T1, Transform3s T2, CollisionRequest col_req, CollisionResult col_res) {
coal::BVHModel<coal::kIOS>* shape1 = new coal::BVHModel<coal::kIOS>;
coal::BVHModel<coal::kIOS>* shape2 = new coal::BVHModel<coal::kIOS>;
cteateBVH(shape1,0.5, 0.5, 0.5);
cteateBVH(shape2, 0.5,0.5, 0.5);
col_req.security_margin = 1;
auto start = std::chrono::high_resolution_clock::now();
coal::collide(shape1, T1, shape2, T2, col_req, col_res);
auto end = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration<double, std::milli>(end - start).count();
std::cout << "boxBVH Time: " << duration << " ms" << std::endl;
return col_res;
}
void getCollisionMessage(CollisionResult& col_res, CollisionRequest col_req) {
std::cout << "Collision? " << col_res.isCollision() << "\n";
if (col_res.isCollision()) {
coal::Contact contact = col_res.getContact(0);
std::cout<<"ContactPointSize："<<col_res.numContacts()<<"\n";
std::cout << "Penetration depth: " << contact.penetration_depth << "\n";
std::cout << "Distance between the shapes including the security margin: " << contact.penetration_depth + col_req.security_margin << "\n";
std::cout << "Witness point on shape1: " << contact.nearest_points[0].transpose() << "\n";
std::cout << "Witness point on shape2: " << contact.nearest_points[1].transpose() << "\n";
std::cout << "Nearest point on shape1: " << col_res.nearest_points[0].transpose() << "\n";
std::cout << "Nearest point on shape2: " << col_res.nearest_points[1].transpose() << "\n";
std::cout << "Normal: " << contact.normal.transpose() << "\n";
std::cout << "Pos: " << contact.pos.transpose() << "\n";
}
}
int main() {
/*
* Test the collision
*/
CollisionResult col_res;
CollisionRequest col_req(CollisionRequestFlag::CONTACT, 100);
col_res = boxBVHTest(Transform3s(Vec3s(0, 0, 0)), Transform3s(Vec3s(0.0, 0.0, 1.7)), col_req, col_res);
getCollisionMessage(col_res, col_req);
col_res.clear();
return 0;
}
This one runs col_res = boxBVHTest(Transform3s(Vec3s(0, 0, 0)), Transform3s(Vec3s(0.0, 0.0, 1.7)), col_req, col_res);
--not collision
This one runs `col_res = boxBVHTest(Transform3s(Vec3s(0, 0, 0)), Transform3s(Vec3s(0.0, 0.0, 1.69)), col_req, col_res);`
--collision
