From 45a2559a2ce6e4ff99d706e0a68ad76fc352c628 Mon Sep 17 00:00:00 2001
From: misha <quant87@mail.ru>
Date: Wed, 25 May 2022 22:22:42 +0300
Subject: [PATCH] create convex mesh when possible

---
 .../bullet/BulletCollisionDetector.cpp        | 82 ++++++++++++++++---
 1 file changed, 72 insertions(+), 10 deletions(-)

diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp
index 32dc9e51e0800..1a141a35cd491 100644
--- a/dart/collision/bullet/BulletCollisionDetector.cpp
+++ b/dart/collision/bullet/BulletCollisionDetector.cpp
@@ -95,6 +95,8 @@ template <typename HeightmapShapeT>
 std::unique_ptr<BulletCollisionShape> createBulletCollisionShapeFromHeightmap(
     const HeightmapShapeT* heightMap);
 
+bool isConvex(const aiMesh* mesh, float threshold = 0.001);
+
 } // anonymous namespace
 
 //==============================================================================
@@ -948,12 +950,22 @@ std::unique_ptr<btCollisionShape> createBulletCollisionShapeFromAssimpScene(
       triMesh->addTriangle(vertices[0], vertices[1], vertices[2]);
     }
   }
-
-  auto gimpactMeshShape = std::make_unique<btGImpactMeshShape>(triMesh);
-  gimpactMeshShape->updateBound();
-  gimpactMeshShape->setUserPointer(triMesh);
-
-  return gimpactMeshShape;
+  const bool makeConvexMesh
+      = scene->mNumMeshes == 1 && isConvex(scene->mMeshes[0]);
+  if (makeConvexMesh)
+  {
+    auto convexMeshShape = std::make_unique<btConvexTriangleMeshShape>(triMesh);
+    convexMeshShape->setMargin(0.0f);
+    convexMeshShape->setUserPointer(triMesh);
+    return convexMeshShape;
+  }
+  else
+  {
+    auto gimpactMeshShape = std::make_unique<btGImpactMeshShape>(triMesh);
+    gimpactMeshShape->updateBound();
+    gimpactMeshShape->setUserPointer(triMesh);
+    return gimpactMeshShape;
+  }
 }
 
 //==============================================================================
@@ -973,10 +985,19 @@ std::unique_ptr<btCollisionShape> createBulletCollisionShapeFromAssimpMesh(
     triMesh->addTriangle(vertices[0], vertices[1], vertices[2]);
   }
 
-  auto gimpactMeshShape = std::make_unique<btGImpactMeshShape>(triMesh);
-  gimpactMeshShape->updateBound();
-
-  return gimpactMeshShape;
+  const bool makeConvexMesh = isConvex(mesh);
+  if (makeConvexMesh)
+  {
+    auto convexMeshShape = std::make_unique<btConvexTriangleMeshShape>(triMesh);
+    convexMeshShape->setMargin(0.0f);
+    return convexMeshShape;
+  }
+  else
+  {
+    auto gimpactMeshShape = std::make_unique<btGImpactMeshShape>(triMesh);
+    gimpactMeshShape->updateBound();
+    return gimpactMeshShape;
+  }
 }
 
 //==============================================================================
@@ -1042,6 +1063,47 @@ std::unique_ptr<BulletCollisionShape> createBulletCollisionShapeFromHeightmap(
       std::move(heightFieldShape), relativeShapeTransform);
 }
 
+//==============================================================================
+bool isConvex(const aiMesh* mesh, float threshold)
+{
+  const auto points = mesh->mVertices;
+  for (auto i = 0u; i < mesh->mNumFaces; ++i)
+  {
+    btVector3 vertices[3];
+    for (auto j = 0u; j < 3; ++j)
+    {
+      const aiVector3D& vertex = mesh->mVertices[mesh->mFaces[i].mIndices[j]];
+      vertices[j] = btVector3(vertex.x, vertex.y, vertex.z);
+    }
+    btVector3 A = vertices[0];
+    btVector3 B = vertices[1];
+    btVector3 C = vertices[2];
+    B -= A;
+    C -= A;
+
+    const btVector3 BCNorm = B.cross(C).normalized();
+
+    const float checkPoint
+        = btVector3(
+              points[0].x - A.x(), points[0].y - A.y(), points[0].z - A.z())
+              .dot(BCNorm);
+
+    for (unsigned long j = 0; j < mesh->mNumVertices; j++)
+    {
+      float dist
+          = btVector3(
+                points[j].x - A.x(), points[j].y - A.y(), points[j].z - A.z())
+                .dot(BCNorm);
+      if ((std::abs(checkPoint) > threshold) && (std::abs(dist) > threshold)
+          && (checkPoint * dist < 0.0f))
+      {
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
 } // anonymous namespace
 
 } // namespace collision