diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh index 9e764cc4d..4db28bb08 100644 --- a/bullet-featherstone/src/Base.hh +++ b/bullet-featherstone/src/Base.hh @@ -28,6 +28,7 @@ #include #include #include +#include "BulletCollision/Gimpact/btGImpactShape.h" #include @@ -340,8 +341,8 @@ class Base : public Implements3d> public: std::unordered_map collisions; public: std::unordered_map joints; - public: std::vector> meshes; - public: btAlignedObjectArray triangleMeshes; + public: std::vector> meshesGImpact; + public: std::vector> triangleMeshes; }; } // namespace bullet_featherstone diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc index 4dab2cb61..87273aa02 100644 --- a/bullet-featherstone/src/SDFFeatures.cc +++ b/bullet-featherstone/src/SDFFeatures.cc @@ -729,21 +729,22 @@ bool SDFFeatures::AddSdfCollision( s->Vertex(i).Z() * scale.Z())); } - auto *btTrimesh = new btTriangleMesh(); - this->triangleMeshes.push_back(btTrimesh); - + this->triangleMeshes.push_back(std::make_unique()); for (unsigned int i = 0; i < indexCount/3; i++) { const btVector3& v0 = convertedVerts[s->Index(i*3)]; const btVector3& v1 = convertedVerts[s->Index(i*3 + 1)]; const btVector3& v2 = convertedVerts[s->Index(i*3 + 2)]; - btTrimesh->addTriangle(v0, v1, v2); + this->triangleMeshes.back()->addTriangle(v0, v1, v2); } - this->meshes.push_back(std::make_unique( - btTrimesh, true, true)); - compoundShape->addChildShape( - btTransform::getIdentity(), this->meshes.back().get()); + this->meshesGImpact.push_back( + std::make_unique( + this->triangleMeshes.back().get())); + this->meshesGImpact.back()->updateBound(); + this->meshesGImpact.back()->setMargin(0.001); + compoundShape->addChildShape(btTransform::getIdentity(), + this->meshesGImpact.back().get()); } shape = std::move(compoundShape); }