From 711752bff13df04b71e457061af456b3caeedf37 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Wed, 13 May 2015 13:54:24 -0700
Subject: [PATCH] rather than call setMargin, clench in all the points of a
 collision-hull toward the center of the hull by the margin amount

---
 libraries/physics/src/ShapeInfoUtil.cpp | 41 ++++++++++++++++++++++---
 1 file changed, 36 insertions(+), 5 deletions(-)

diff --git a/libraries/physics/src/ShapeInfoUtil.cpp b/libraries/physics/src/ShapeInfoUtil.cpp
index 484f17f060..768546ddeb 100644
--- a/libraries/physics/src/ShapeInfoUtil.cpp
+++ b/libraries/physics/src/ShapeInfoUtil.cpp
@@ -14,7 +14,36 @@
 #include "ShapeInfoUtil.h"
 #include "BulletUtil.h"
 
-const float BULLET_COLLISION_MARGIN = 0.01;
+
+// 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];
+    }
+    return result * (1.0f / points.size());
+}
+
+
+// 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);
+    }
+    return result;
+}
+
 
 btCollisionShape* ShapeInfoUtil::createShapeFromInfo(const ShapeInfo& info) {
     btCollisionShape* shape = NULL;
@@ -41,11 +70,12 @@ btCollisionShape* ShapeInfoUtil::createShapeFromInfo(const ShapeInfo& info) {
             if (numSubShapes == 1) {
                 auto hull = new btConvexHullShape();
                 const QVector<QVector<glm::vec3>>& points = info.getPoints();
-                foreach (glm::vec3 point, points[0]) {
+                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->setMargin(BULLET_COLLISION_MARGIN);
                 hull->recalcLocalAabb();
                 shape = hull;
             } else {
@@ -55,14 +85,15 @@ btCollisionShape* ShapeInfoUtil::createShapeFromInfo(const ShapeInfo& info) {
                 trans.setIdentity();
                 foreach (QVector<glm::vec3> hullPoints, points) {
                     auto hull = new btConvexHullShape();
-                    foreach (glm::vec3 point, hullPoints) {
+                    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();
                     compound->addChildShape (trans, hull);
                 }
-                compound->setMargin(BULLET_COLLISION_MARGIN);
                 shape = compound;
             }
         }