From fdd7a1098de9680a5013d03e0421dc6a1c1155e0 Mon Sep 17 00:00:00 2001 From: Jose Tomas Lorente Date: Tue, 15 Dec 2020 15:50:59 -0300 Subject: [PATCH] [Bullet] Mesh features (#178) * Collision working Signed-off-by: Tomas Lorente Co-authored-by: Jorge Perez Signed-off-by: Jorge Perez --- bullet/src/SDFFeatures.cc | 8 +-- bullet/src/ShapeFeatures.cc | 110 ++++++++++++++++++++++++++++++++++++ bullet/src/ShapeFeatures.hh | 37 ++++++++++++ bullet/src/plugin.cc | 7 ++- 4 files changed, 153 insertions(+), 9 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 6470a0c44..d207e7981 100644 --- a/bullet/src/SDFFeatures.cc +++ b/bullet/src/SDFFeatures.cc @@ -82,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; } @@ -152,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..f702d5557 --- /dev/null +++ b/bullet/src/ShapeFeatures.cc @@ -0,0 +1,110 @@ +#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(); + + 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 + 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(); + + // TODO(lobotuerk) Save collision if needed + // 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); + + world->addRigidBody(link); + + return this->AddCollision({_name, gimpactMeshShape, _linkID, + modelID}); + +} + +///////////////////////////////////////////////// +Identity ShapeFeatures::CastToMeshShape( + const Identity &_shapeID) const +{ + ignwarn << "Dummy implementation 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)