[Bullet] Mesh features (#178)

* Collision working

Signed-off-by: Tomas Lorente <jtlorente@ekumenlabs.com>

Co-authored-by: Jorge Perez <j.j.perez13@hotmail.com>
Signed-off-by: Jorge Perez <jjperez@ekumenlabs.com>
Signed-off-by: Tomas Lorente <jtlorente@ekumenlabs.com>

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);
 

bullet/src/ShapeFeatures.cc

@@ -0,0 +1,110 @@
+#include "ShapeFeatures.hh"
+#include <BulletCollision/Gimpact/btGImpactShape.h>
+#include <BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h>
+
+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();
+}
+
+}
+}
+}

bullet/src/ShapeFeatures.hh

@@ -0,0 +1,37 @@
+
+#ifndef IGNITION_PHYSICS_BULLET_SRC_SHAPEFEATURES_HH_
+#define IGNITION_PHYSICS_BULLET_SRC_SHAPEFEATURES_HH_
+
+#include <ignition/physics/mesh/MeshShape.hh>
+#include <string>
+
+#include "Base.hh"
+
+namespace ignition {
+namespace physics {
+namespace bullet {
+
+using ShapeFeatureList = FeatureList<
+  mesh::AttachMeshShapeFeature
+>;
+
+class ShapeFeatures :
+    public virtual Base,
+    public virtual Implements3d<ShapeFeatureList>
+{
+  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

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)