From 58e31abf60bc1eb022bf1c83de428df064faeae4 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 17 Jul 2014 14:31:16 -0700
Subject: [PATCH] improved collision shapes for fingers

---
 libraries/fbx/src/FBXReader.cpp | 49 ++++++++++++++++++++-------------
 1 file changed, 30 insertions(+), 19 deletions(-)

diff --git a/libraries/fbx/src/FBXReader.cpp b/libraries/fbx/src/FBXReader.cpp
index 0e1b1ae0c6..594ad7ebe5 100644
--- a/libraries/fbx/src/FBXReader.cpp
+++ b/libraries/fbx/src/FBXReader.cpp
@@ -967,19 +967,19 @@ QString getString(const QVariant& value) {
 
 class JointShapeInfo {
 public:
-    JointShapeInfo() : numVertices(0), numProjectedVertices(0), 
-            sumVertexWeights(0.0f), sumWeightedRadii(0.0f),
+    JointShapeInfo() : numVertices(0), 
+            sumVertexWeights(0.0f), sumWeightedRadii(0.0f), numVertexWeights(0), 
             averageVertex(0.f), boneBegin(0.f), averageRadius(0.f) {
     }
 
     // NOTE: the points here are in the "joint frame" which has the "jointEnd" at the origin
-    int numVertices;            // num vertices from contributing meshes
-    int numProjectedVertices;   // num vertices that successfully project onto bone axis
-    float sumVertexWeights;
-    float sumWeightedRadii;
-    glm::vec3 averageVertex;    // average of all mesh vertices (in joint frame)
-    glm::vec3 boneBegin;        // parent joint location (in joint frame)
-    float averageRadius;        // average distance from mesh points to averageVertex
+    int numVertices;        // num vertices from contributing meshes
+    float sumVertexWeights; // sum of all vertex weights
+    float sumWeightedRadii; // sum of weighted vertices
+    int numVertexWeights;   // num vertices that contributed to sums
+    glm::vec3 averageVertex;// average of all mesh vertices (in joint frame)
+    glm::vec3 boneBegin;    // parent joint location (in joint frame)
+    float averageRadius;    // average distance from mesh points to averageVertex
 };
 
 class AnimationCurve {
@@ -1743,11 +1743,14 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                         if (weight > EXPANSION_WEIGHT_THRESHOLD) {
                             const glm::vec3& vertex = extracted.mesh.vertices.at(it.value());
                             float proj = glm::dot(boneDirection, boneEnd - vertex);
-                            if (proj > 0.0f && proj < boneLength) {
-                                joint.boneRadius = glm::max(joint.boneRadius, 
-                                        radiusScale * glm::distance(vertex, boneEnd - boneDirection * proj));
-                                ++jointShapeInfo.numProjectedVertices;
+                            if (proj < 0.0f || proj > boneLength) {
+                                weight *= 0.5f;
                             }
+
+                            jointShapeInfo.sumVertexWeights += weight;
+                            jointShapeInfo.sumWeightedRadii += weight * radiusScale * glm::distance(vertex, boneEnd - boneDirection * proj);
+                            ++jointShapeInfo.numVertexWeights;
+
                             glm::vec3 vertexInJointFrame = rotateMeshToJoint * (radiusScale * (vertex - boneEnd));
                             jointShapeInfo.averageVertex += vertexInJointFrame;
                             ++jointShapeInfo.numVertices;
@@ -1793,11 +1796,15 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
 
             glm::vec3 averageVertex(0.f);
             foreach (const glm::vec3& vertex, extracted.mesh.vertices) {
+                float weight = 1.0f;
                 float proj = glm::dot(boneDirection, boneEnd - vertex);
-                if (proj > 0.0f && proj < boneLength) {
-                    joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance(vertex, boneEnd - boneDirection * proj));
-                    ++jointShapeInfo.numProjectedVertices;
+                if (proj < 0.0f || proj > boneLength) {
+                    weight *= 0.5f;
                 }
+                jointShapeInfo.sumVertexWeights += weight;
+                jointShapeInfo.sumWeightedRadii += weight * radiusScale * glm::distance(vertex, boneEnd - boneDirection * proj);
+                ++jointShapeInfo.numVertexWeights;
+
                 glm::vec3 vertexInJointFrame = rotateMeshToJoint * (radiusScale * (vertex - boneEnd));
                 jointShapeInfo.averageVertex += vertexInJointFrame;
                 averageVertex += vertex;
@@ -1832,9 +1839,13 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
             jointShapeInfo.boneBegin = inverseRotation * (extractTranslation(parentJoint.transform) - extractTranslation(joint.transform));
         }
 
-        // we use a capsule if the joint ANY mesh vertices successfully projected onto the bone
+        if (jointShapeInfo.sumVertexWeights > 0.0f) {
+            joint.boneRadius = jointShapeInfo.sumWeightedRadii / jointShapeInfo.sumVertexWeights;
+        }
+
+        // we use a capsule if the joint had ANY mesh vertices successfully projected onto the bone
         // AND its boneRadius is not too close to zero
-        bool collideLikeCapsule = jointShapeInfo.numProjectedVertices > 0
+        bool collideLikeCapsule = jointShapeInfo.numVertexWeights > 0
                 && glm::length(jointShapeInfo.boneBegin) > EPSILON;
 
         if (collideLikeCapsule) {
@@ -1850,7 +1861,7 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
             } else {
                 joint.shapePosition = glm::vec3(0.f);
             }
-            if (jointShapeInfo.numProjectedVertices == 0
+            if (jointShapeInfo.numVertexWeights == 0
                    && jointShapeInfo.numVertices > 0) {
                 // the bone projection algorithm was not able to compute the joint radius
                 // so we use an alternative measure