From d425c69e86b91e204ebf5d22a072b5e7681f7e61 Mon Sep 17 00:00:00 2001 From: Tomas Lorente Date: Fri, 11 Dec 2020 15:46:27 -0300 Subject: [PATCH 1/3] Collision working with plane Signed-off-by: Tomas Lorente --- bullet/src/SDFFeatures.cc | 14 +---- bullet/src/ShapeFeatures.cc | 102 ++++++++++++++++++++++++++++++++++++ bullet/src/ShapeFeatures.hh | 37 +++++++++++++ bullet/src/plugin.cc | 7 ++- 4 files changed, 145 insertions(+), 15 deletions(-) create mode 100644 bullet/src/ShapeFeatures.cc create mode 100644 bullet/src/ShapeFeatures.hh diff --git a/bullet/src/SDFFeatures.cc b/bullet/src/SDFFeatures.cc index 74c752c0d..42478af55 100644 --- a/bullet/src/SDFFeatures.cc +++ b/bullet/src/SDFFeatures.cc @@ -39,12 +39,6 @@ Identity SDFFeatures::ConstructSdfModel( // const auto &world = this->worlds.at(_worldID)->world; const auto modelIdentity = this->AddModel({name, _worldID, isStatic, pose}); - // Build links - for (std::size_t i = 0; i < _sdfModel.LinkCount(); ++i) - { - this->ConstructSdfLink(modelIdentity, *_sdfModel.LinkByIndex(i)); - } - return modelIdentity; } @@ -88,12 +82,6 @@ Identity SDFFeatures::ConstructSdfLink( // Generate an identity for it const auto linkIdentity = this->AddLink({name, body, _modelID}); - - for (std::size_t i = 0; i < _sdfLink.CollisionCount(); ++i) - { - this->ConstructSdfCollision(linkIdentity, *_sdfLink.CollisionByIndex(i)); - } - return linkIdentity; } @@ -158,7 +146,7 @@ Identity SDFFeatures::ConstructSdfCollision( const auto &modelInfo = this->models.at(modelID); const auto &link = linkInfo->link; const auto &world = this->worlds.at(modelInfo->world)->world; - // btTransform transform; + delete link->getCollisionShape(); link->setCollisionShape(shape); diff --git a/bullet/src/ShapeFeatures.cc b/bullet/src/ShapeFeatures.cc new file mode 100644 index 000000000..42413d41c --- /dev/null +++ b/bullet/src/ShapeFeatures.cc @@ -0,0 +1,102 @@ +#include "ShapeFeatures.hh" +#include +#include + +namespace ignition { +namespace physics { +namespace bullet { + +///////////////////////////////////////////////// +Identity ShapeFeatures::AttachMeshShape( + const Identity &_linkID, + const std::string &_name, + const ignition::common::Mesh &_mesh, + const Pose3d &_pose, + const LinearVector3d &_scale) +{ + (void) _pose; + + for (auto collision_it : this->collisions) + { + auto &collisionInfo = collision_it.second; + if (collisionInfo->link.id == _linkID.id) + { + return this->GenerateInvalidId(); + } + } + + double *vertices = nullptr; + int *indices = nullptr; + _mesh.FillArrays(&vertices, &indices); + + unsigned int numVertices = _mesh.VertexCount(); + unsigned int numIndices = _mesh.IndexCount(); + + btTriangleMesh *mTriMesh = new btTriangleMesh(); + + // Scale the vertex data + for (unsigned int j = 0; j < numVertices; ++j) + { + vertices[j*3+0] = vertices[j*3+0] * _scale[0]; + vertices[j*3+1] = vertices[j*3+1] * _scale[1]; + vertices[j*3+2] = vertices[j*3+2] * _scale[2]; + } + + // Create the Bullet trimesh + for (unsigned int j = 0; j < numIndices; j += 3) + { + btVector3 bv0(vertices[indices[j]*3+0], + vertices[indices[j]*3+1], + vertices[indices[j]*3+2]); + + btVector3 bv1(vertices[indices[j+1]*3+0], + vertices[indices[j+1]*3+1], + vertices[indices[j+1]*3+2]); + + btVector3 bv2(vertices[indices[j+2]*3+0], + vertices[indices[j+2]*3+1], + vertices[indices[j+2]*3+2]); + + mTriMesh->addTriangle(bv0, bv1, bv2); + } + + btGImpactMeshShape *gimpactMeshShape = + new btGImpactMeshShape(mTriMesh); + gimpactMeshShape->updateBound(); + + // collision->shape = gimpactMeshShape; + + delete [] vertices; + delete [] indices; + + const auto &linkInfo = this->links.at(_linkID); + const auto &modelID = linkInfo->model; + const auto &modelInfo = this->models.at(modelID); + const auto &link = linkInfo->link; + const auto &worldInfo = this->worlds.at(modelInfo->world); + const auto &world = worldInfo->world; + delete link->getCollisionShape(); + link->setCollisionShape(gimpactMeshShape); + btGImpactCollisionAlgorithm::registerAlgorithm(worldInfo->dispatcher); + + // We add the rigidbody to the world after it has collision, as + // non collision bodies don't get simulated on a dynamics world + world->addRigidBody(link); + + return this->AddCollision({_name, gimpactMeshShape, _linkID, + modelID}); + +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToMeshShape( + const Identity &_shapeID) const +{ + ignerr << "CASTTOMESHSHAPE" << std::endl; + (void) _shapeID; + return this->GenerateInvalidId(); +} + +} +} +} diff --git a/bullet/src/ShapeFeatures.hh b/bullet/src/ShapeFeatures.hh new file mode 100644 index 000000000..8a3b5fa05 --- /dev/null +++ b/bullet/src/ShapeFeatures.hh @@ -0,0 +1,37 @@ + +#ifndef IGNITION_PHYSICS_BULLET_SRC_SHAPEFEATURES_HH_ +#define IGNITION_PHYSICS_BULLET_SRC_SHAPEFEATURES_HH_ + +#include +#include + +#include "Base.hh" + +namespace ignition { +namespace physics { +namespace bullet { + +using ShapeFeatureList = FeatureList< + mesh::AttachMeshShapeFeature +>; + +class ShapeFeatures : + public virtual Base, + public virtual Implements3d +{ + public: Identity AttachMeshShape( + const Identity &_linkID, + const std::string &_name, + const ignition::common::Mesh &_mesh, + const Pose3d &_pose, + const LinearVector3d &_scale) override; + + public: Identity CastToMeshShape( + const Identity &_shapeID) const override; +}; + +} +} +} + +#endif diff --git a/bullet/src/plugin.cc b/bullet/src/plugin.cc index cee38d926..ff04beada 100644 --- a/bullet/src/plugin.cc +++ b/bullet/src/plugin.cc @@ -26,6 +26,7 @@ #include "SDFFeatures.hh" #include "KinematicsFeatures.hh" #include "FreeGroupFeatures.hh" +#include "ShapeFeatures.hh" namespace ignition { namespace physics { @@ -36,7 +37,8 @@ struct BulletFeatures : FeatureList < SimulationFeatureList, FreeGroupFeatureList, KinematicsFeatureList, - SDFFeatureList + SDFFeatureList, + ShapeFeatureList > { }; class Plugin : @@ -46,7 +48,8 @@ class Plugin : public virtual SimulationFeatures, public virtual FreeGroupFeatures, public virtual KinematicsFeatures, - public virtual SDFFeatures + public virtual SDFFeatures, + public virtual ShapeFeatures {}; IGN_PHYSICS_ADD_PLUGIN(Plugin, FeaturePolicy3d, BulletFeatures) From 9c9e15bf034526d8e0eaef4d38fe622e8b700d89 Mon Sep 17 00:00:00 2001 From: Tomas Lorente Date: Mon, 14 Dec 2020 15:54:27 -0300 Subject: [PATCH 2/3] fixed pose Signed-off-by: Tomas Lorente --- bullet/src/ShapeFeatures.cc | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/bullet/src/ShapeFeatures.cc b/bullet/src/ShapeFeatures.cc index 42413d41c..d3efa7978 100644 --- a/bullet/src/ShapeFeatures.cc +++ b/bullet/src/ShapeFeatures.cc @@ -34,12 +34,20 @@ Identity ShapeFeatures::AttachMeshShape( btTriangleMesh *mTriMesh = new btTriangleMesh(); - // Scale the vertex data for (unsigned int j = 0; j < numVertices; ++j) { + // Scale the vertex data vertices[j*3+0] = vertices[j*3+0] * _scale[0]; vertices[j*3+1] = vertices[j*3+1] * _scale[1]; vertices[j*3+2] = vertices[j*3+2] * _scale[2]; + + // Apply pose transformation to data + Eigen::Vector3d vect; + vect << vertices[j*3+0], vertices[j*3+1], vertices[j*3+2]; + vect = _pose * vect; + vertices[j*3+0] = vect(0); + vertices[j*3+1] = vect(1); + vertices[j*3+2] = vect(2); } // Create the Bullet trimesh @@ -64,6 +72,7 @@ Identity ShapeFeatures::AttachMeshShape( new btGImpactMeshShape(mTriMesh); gimpactMeshShape->updateBound(); + // TODO(lobotuerk) Save collision if needed // collision->shape = gimpactMeshShape; delete [] vertices; @@ -77,10 +86,9 @@ Identity ShapeFeatures::AttachMeshShape( const auto &world = worldInfo->world; delete link->getCollisionShape(); link->setCollisionShape(gimpactMeshShape); + btGImpactCollisionAlgorithm::registerAlgorithm(worldInfo->dispatcher); - // We add the rigidbody to the world after it has collision, as - // non collision bodies don't get simulated on a dynamics world world->addRigidBody(link); return this->AddCollision({_name, gimpactMeshShape, _linkID, From 9c00f1ff6d4688211c4aa7a5f1dfdb13a292cfc8 Mon Sep 17 00:00:00 2001 From: Jose Tomas Lorente Date: Tue, 15 Dec 2020 10:48:13 -0300 Subject: [PATCH 3/3] Update bullet/src/ShapeFeatures.cc Co-authored-by: Jorge Perez --- bullet/src/ShapeFeatures.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bullet/src/ShapeFeatures.cc b/bullet/src/ShapeFeatures.cc index d3efa7978..f702d5557 100644 --- a/bullet/src/ShapeFeatures.cc +++ b/bullet/src/ShapeFeatures.cc @@ -100,7 +100,7 @@ Identity ShapeFeatures::AttachMeshShape( Identity ShapeFeatures::CastToMeshShape( const Identity &_shapeID) const { - ignerr << "CASTTOMESHSHAPE" << std::endl; + ignwarn << "Dummy implementation CastToMeshShape" << std::endl; (void) _shapeID; return this->GenerateInvalidId(); }