From 7a47e04b9cf1e525b9bf9580ef392dfc38e3a7ea Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 14 May 2015 14:57:18 -0700
Subject: [PATCH] adjust margin for hulls with small dimensions

---
 libraries/physics/src/ShapeInfoUtil.cpp | 80 ++++++++++++-------------
 libraries/physics/src/ShapeInfoUtil.h   |  2 +
 2 files changed, 40 insertions(+), 42 deletions(-)

diff --git a/libraries/physics/src/ShapeInfoUtil.cpp b/libraries/physics/src/ShapeInfoUtil.cpp
index 768546ddeb..1cccd691c2 100644
--- a/libraries/physics/src/ShapeInfoUtil.cpp
+++ b/libraries/physics/src/ShapeInfoUtil.cpp
@@ -15,36 +15,48 @@
 #include "BulletUtil.h"
 
 
-// find the average point on a convex shape
-glm::vec3 findCenter(const QVector<glm::vec3>& points) {
-    glm::vec3 result = glm::vec3(0);
-    for (int i = 0; i < points.size(); i++) {
-        result += points[i];
+
+btConvexHullShape* ShapeInfoUtil::createConvexHull(const QVector<glm::vec3>& points) {
+    assert(points.size() > 0);
+
+    btConvexHullShape* hull = new btConvexHullShape();
+    glm::vec3 center = points[0];
+    glm::vec3 maxCorner = center;
+    glm::vec3 minCorner = center;
+    for (int i = 1; i < points.size(); i++) {
+        center += points[i];
+        maxCorner = glm::max(maxCorner, points[i]);
+        minCorner = glm::min(minCorner, points[i]);
     }
-    return result * (1.0f / points.size());
-}
+    center /= (float)(points.size());
 
+    float margin = hull->getMargin();
 
-// bullet puts "margins" around all the collision shapes.  This can cause shapes will hulls
-// to float a bit above what they are sitting on, etc.  One option is to call:
-//
-//    compound->setMargin(0.01);
-//
-// to reduce the size of the margin, but this has some consequences for the
-// performance and stability of the simulation.  Instead, we clench in all the points of
-// the hull by the margin.  These clenched points + bullets margin will but the actual
-// collision hull fairly close to the visual edge of the object.
-QVector<glm::vec3> shrinkByMargin(const QVector<glm::vec3>& points, const glm::vec3 center, float margin) {
-    QVector<glm::vec3> result(points.size());
-    for (int i = 0; i < points.size(); i++) {
-        glm::vec3 pVec = points[ i ] - center;
-        glm::vec3 pVecNorm = glm::normalize(pVec);
-        result[ i ] = center + pVec - (pVecNorm * margin);
+    // Bullet puts "margins" around all the collision shapes.  This can cause objects that use ConvexHull shapes
+    // to have visible gaps between them and the surface they touch.  One option is to reduce the size of the margin
+    // but this can reduce the performance and stability of the simulation (e.g. the GJK algorithm will fail to provide
+    // nearest contact points and narrow-phase collisions will fall into more expensive code paths).  Alternatively
+    // one can shift the geometry of the shape to make the margin surface approximately close to the visible surface.
+    // This is the strategy we try, but if the object is too small then we start to reduce the margin down to some minimum.
+
+    const float MIN_MARGIN = 0.01f;
+    glm::vec3 diagonal = maxCorner - minCorner;
+    float minDimension = glm::min(diagonal[0], diagonal[1]);
+    minDimension = glm::min(minDimension, diagonal[2]);
+    margin = glm::min(glm::max(0.5f * minDimension, MIN_MARGIN), margin);
+    hull->setMargin(margin);
+
+    // add the points, correcting for margin
+    glm::vec3 relativeScale = (diagonal - glm::vec3(2.0f * margin)) / diagonal;
+    glm::vec3 correctedPoint;
+    for (int i = 0; i < points.size(); ++i) {
+        correctedPoint = (points[i] - center) * relativeScale + center;
+        hull->addPoint(btVector3(correctedPoint[0], correctedPoint[1], correctedPoint[2]), false);
     }
-    return result;
+    hull->recalcLocalAabb();
+    return hull;
 }
 
-
 btCollisionShape* ShapeInfoUtil::createShapeFromInfo(const ShapeInfo& info) {
     btCollisionShape* shape = NULL;
     switch(info.getType()) {
@@ -68,30 +80,14 @@ btCollisionShape* ShapeInfoUtil::createShapeFromInfo(const ShapeInfo& info) {
             const QVector<QVector<glm::vec3>>& points = info.getPoints();
             uint32_t numSubShapes = info.getNumSubShapes();
             if (numSubShapes == 1) {
-                auto hull = new btConvexHullShape();
-                const QVector<QVector<glm::vec3>>& points = info.getPoints();
-                glm::vec3 center = findCenter(points[0]);
-                QVector<glm::vec3> shrunken = shrinkByMargin(points[0], center, hull->getMargin());
-                foreach (glm::vec3 point, shrunken) {
-                    btVector3 btPoint(point[0], point[1], point[2]);
-                    hull->addPoint(btPoint, false);
-                }
-                hull->recalcLocalAabb();
-                shape = hull;
+                shape = createConvexHull(info.getPoints()[0]);
             } else {
                 assert(numSubShapes > 1);
                 auto compound = new btCompoundShape();
                 btTransform trans;
                 trans.setIdentity();
                 foreach (QVector<glm::vec3> hullPoints, points) {
-                    auto hull = new btConvexHullShape();
-                    glm::vec3 center = findCenter(points[0]);
-                    QVector<glm::vec3> shrunken = shrinkByMargin(hullPoints, center, hull->getMargin());
-                    foreach (glm::vec3 point, shrunken) {
-                        btVector3 btPoint(point[0], point[1], point[2]);
-                        hull->addPoint(btPoint, false);
-                    }
-                    hull->recalcLocalAabb();
+                    btConvexHullShape* hull = createConvexHull(hullPoints);
                     compound->addChildShape (trans, hull);
                 }
                 shape = compound;
diff --git a/libraries/physics/src/ShapeInfoUtil.h b/libraries/physics/src/ShapeInfoUtil.h
index 39a897019c..7284cd3550 100644
--- a/libraries/physics/src/ShapeInfoUtil.h
+++ b/libraries/physics/src/ShapeInfoUtil.h
@@ -22,6 +22,8 @@
 // TODO: rename this to ShapeFactory
 namespace ShapeInfoUtil {
 
+    btConvexHullShape* createConvexHull(const QVector<glm::vec3>& points);
+
     btCollisionShape* createShapeFromInfo(const ShapeInfo& info);
 };