From 2633223f4eb97cd03fb684d614ba76e6135a823a Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Mon, 2 Dec 2013 18:07:29 -0800
Subject: [PATCH 01/14] Compute the bone radii from the vertices.  It's more
 error-prone than I expected, but it more or less works.

---
 interface/src/Util.cpp               |  9 +++++
 interface/src/Util.h                 |  4 +++
 interface/src/renderer/FBXReader.cpp | 50 +++++++++++++++++++++++++---
 interface/src/renderer/FBXReader.h   |  1 +
 4 files changed, 60 insertions(+), 4 deletions(-)

diff --git a/interface/src/Util.cpp b/interface/src/Util.cpp
index 779ae5736b..a5c12b3edc 100644
--- a/interface/src/Util.cpp
+++ b/interface/src/Util.cpp
@@ -224,6 +224,15 @@ glm::quat extractRotation(const glm::mat4& matrix, bool assumeOrthogonal) {
         0.5f * sqrtf(z2) * (upper[0][1] >= upper[1][0] ? 1.0f : -1.0f)));
 }
 
+glm::vec3 extractScale(const glm::mat4& matrix) {
+    return glm::vec3(glm::length(matrix[0]), glm::length(matrix[1]), glm::length(matrix[2]));
+}
+
+float extractUniformScale(const glm::mat4& matrix) {
+    glm::vec3 scale = extractScale(matrix);
+    return (scale.x + scale.y + scale.z) / 3.0f;
+}
+
 //  Draw a 3D vector floating in space
 void drawVector(glm::vec3 * vector) {
     glDisable(GL_LIGHTING);
diff --git a/interface/src/Util.h b/interface/src/Util.h
index ec410429db..b80731bd20 100644
--- a/interface/src/Util.h
+++ b/interface/src/Util.h
@@ -61,6 +61,10 @@ void setTranslation(glm::mat4& matrix, const glm::vec3& translation);
 
 glm::quat extractRotation(const glm::mat4& matrix, bool assumeOrthogonal = false);
 
+glm::vec3 extractScale(const glm::mat4& matrix);
+
+float extractUniformScale(const glm::mat4& matrix);
+
 double diffclock(timeval *clock1,timeval *clock2);
 
 void renderMouseVoxelGrid(const float& mouseVoxelX, const float& mouseVoxelY, const float& mouseVoxelZ, const float& mouseVoxelS);
diff --git a/interface/src/renderer/FBXReader.cpp b/interface/src/renderer/FBXReader.cpp
index e841780975..ad44bf2da6 100644
--- a/interface/src/renderer/FBXReader.cpp
+++ b/interface/src/renderer/FBXReader.cpp
@@ -20,6 +20,7 @@
 
 #include <OctalCode.h>
 
+#include <GeometryUtil.h>
 #include <VoxelTree.h>
 
 #include "FBXReader.h"
@@ -1142,6 +1143,7 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
             joint.distanceToParent = glm::distance(extractTranslation(parentJoint.transform),
                 extractTranslation(joint.transform));
         }
+        joint.boneRadius = 0.0f;
         joint.inverseBindRotation = joint.inverseDefaultRotation;
         geometry.joints.append(joint);
         geometry.jointIndices.insert(model.name, geometry.joints.size() - 1);  
@@ -1274,7 +1276,9 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
         }
         
         // whether we're skinned depends on how many clusters are attached
-        int maxJointIndex = extracted.mesh.clusters.at(0).jointIndex;
+        const FBXCluster& firstFBXCluster = extracted.mesh.clusters.at(0);
+        int maxJointIndex = firstFBXCluster.jointIndex;
+        glm::mat4 inverseModelTransform = glm::inverse(modelTransform);
         if (clusterIDs.size() > 1) {
             extracted.mesh.clusterIndices.resize(extracted.mesh.vertices.size());
             extracted.mesh.clusterWeights.resize(extracted.mesh.vertices.size());
@@ -1282,6 +1286,21 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
             for (int i = 0; i < clusterIDs.size(); i++) {
                 QString clusterID = clusterIDs.at(i);
                 const Cluster& cluster = clusters[clusterID];
+                const FBXCluster& fbxCluster = extracted.mesh.clusters.at(i);
+                int jointIndex = fbxCluster.jointIndex;
+                FBXJoint& joint = geometry.joints[jointIndex];
+                glm::vec3 boneEnd = extractTranslation(inverseModelTransform * joint.bindTransform);
+                glm::vec3 boneDirection;
+                float boneLength;
+                if (joint.parentIndex != -1) {
+                    boneDirection = boneEnd - extractTranslation(inverseModelTransform *
+                        geometry.joints[joint.parentIndex].bindTransform);
+                    boneLength = glm::length(boneDirection);
+                    if (boneLength > EPSILON) {
+                        boneDirection /= boneLength;
+                    }
+                }
+                float radiusScale = extractUniformScale(joint.transform * fbxCluster.inverseBindMatrix);
                 float totalWeight = 0.0f;
                 for (int j = 0; j < cluster.indices.size(); j++) {
                     int oldIndex = cluster.indices.at(j);
@@ -1289,9 +1308,18 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                     totalWeight += weight;
                     for (QMultiHash<int, int>::const_iterator it = extracted.newIndices.constFind(oldIndex);
                             it != extracted.newIndices.end() && it.key() == oldIndex; it++) {
-                        glm::vec4& weights = extracted.mesh.clusterWeights[it.value()];
-                    
+                        // expand the bone radius
+                        if (weight > 0.25f) {
+                            const glm::vec3& vertex = extracted.mesh.vertices.at(it.value());
+                            float proj = glm::dot(boneDirection, vertex - boneEnd);
+                            if (proj < 0.0f && proj > -boneLength) {
+                                joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance(
+                                    vertex, boneEnd + boneDirection * proj));
+                        }   }
+                        
+                        
                         // look for an unused slot in the weights vector
+                        glm::vec4& weights = extracted.mesh.clusterWeights[it.value()];
                         for (int k = 0; k < 4; k++) {
                             if (weights[k] == 0.0f) {
                                 extracted.mesh.clusterIndices[it.value()][k] = i;
@@ -1303,9 +1331,23 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                 }
                 if (totalWeight > maxWeight) {
                     maxWeight = totalWeight;
-                    maxJointIndex = extracted.mesh.clusters.at(i).jointIndex;
+                    maxJointIndex = jointIndex;
                 }
             }
+        } else {
+            int jointIndex = maxJointIndex;
+            FBXJoint& joint = geometry.joints[jointIndex];
+            glm::vec3 boneEnd = extractTranslation(inverseModelTransform * joint.bindTransform);
+            glm::vec3 boneStart = boneEnd;
+            if (joint.parentIndex != -1) {
+                boneStart = extractTranslation(inverseModelTransform * geometry.joints[joint.parentIndex].bindTransform);
+            }
+            float radiusScale = extractUniformScale(joint.transform * firstFBXCluster.inverseBindMatrix);
+            foreach (const glm::vec3& vertex, extracted.mesh.vertices) {
+                // expand the bone radius
+                joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::length(
+                    computeVectorFromPointToSegment(vertex, boneStart, boneEnd)));
+            }
         }
         extracted.mesh.isEye = (maxJointIndex == geometry.leftEyeJointIndex || maxJointIndex == geometry.rightEyeJointIndex);
         
diff --git a/interface/src/renderer/FBXReader.h b/interface/src/renderer/FBXReader.h
index eb454f7566..6cc08a1549 100644
--- a/interface/src/renderer/FBXReader.h
+++ b/interface/src/renderer/FBXReader.h
@@ -47,6 +47,7 @@ public:
     QVector<int> freeLineage;
     int parentIndex;
     float distanceToParent;
+    float boneRadius;
     glm::mat4 preTransform;
     glm::quat preRotation;
     glm::quat rotation;

From 817946bed5603ea9024c87fcb41447d5080dd141 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 3 Dec 2013 11:05:38 -0800
Subject: [PATCH 02/14] Attempting to rotate longitudinally at elbow, rather
 than wrist.

---
 interface/src/avatar/SkeletonModel.cpp | 18 ++++++++++++++++++
 interface/src/renderer/Model.cpp       |  5 +++--
 interface/src/renderer/Model.h         |  2 +-
 3 files changed, 22 insertions(+), 3 deletions(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 64288effc4..047a45206f 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -141,6 +141,24 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
     float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
     glm::quat palmRotation;
     getJointRotation(jointIndex, palmRotation, true);
+    
+    // start by rotating the elbow into place
+    int parentIndex = geometry.joints[jointIndex].parentIndex;
+    if (parentIndex != -1) {
+        glm::vec3 boneVector = extractTranslation(_jointStates[jointIndex].transform) -
+            extractTranslation(_jointStates[parentIndex].transform);
+        float boneLength = glm::length(boneVector);
+        if (boneLength > EPSILON) {
+            boneVector /= boneLength;
+            applyRotationDelta(parentIndex, rotationBetween(
+                glm::cross(boneVector, glm::cross(palmRotation * geometry.palmDirection, boneVector)),
+                glm::cross(boneVector, glm::cross(palm.getNormal(), boneVector))), false);
+            updateJointState(jointIndex);
+            getJointRotation(jointIndex, palmRotation, true);
+        }
+    }
+    
+    // continue with the wrist
     applyRotationDelta(jointIndex, rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()));
     getJointRotation(jointIndex, palmRotation, true);
     
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 3698f0682f..8be7a967f7 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -680,10 +680,11 @@ float Model::getLimbLength(int jointIndex) const {
     return length;
 }
 
-void Model::applyRotationDelta(int jointIndex, const glm::quat& delta) {
+void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain) {
     JointState& state = _jointStates[jointIndex];
     const FBXJoint& joint = _geometry->getFBXGeometry().joints[jointIndex];
-    if (joint.rotationMin == glm::vec3(-180.0f, -180.0f, -180.0f) && joint.rotationMax == glm::vec3(180.0f, 180.0f, 180.0f)) {
+    if (!constrain || (joint.rotationMin == glm::vec3(-180.0f, -180.0f, -180.0f) &&
+            joint.rotationMax == glm::vec3(180.0f, 180.0f, 180.0f))) {
         // no constraints
         state.rotation = state.rotation * glm::inverse(state.combinedRotation) * delta * state.combinedRotation;
         state.combinedRotation = delta * state.combinedRotation;
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index 44ac7971a5..ed2d292ee4 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -157,7 +157,7 @@ protected:
     /// first free ancestor.
     float getLimbLength(int jointIndex) const;
     
-    void applyRotationDelta(int jointIndex, const glm::quat& delta);
+    void applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain = true);
     
 private:
     

From 905320a9632e59060150361a08b8122e5d045695 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 3 Dec 2013 11:30:47 -0800
Subject: [PATCH 03/14] That didn't really help.  What did help was simply
 ignoring the wrist constraints.

---
 interface/src/avatar/SkeletonModel.cpp | 22 ++--------------------
 1 file changed, 2 insertions(+), 20 deletions(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 047a45206f..42c57ed5bd 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -141,25 +141,7 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
     float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
     glm::quat palmRotation;
     getJointRotation(jointIndex, palmRotation, true);
-    
-    // start by rotating the elbow into place
-    int parentIndex = geometry.joints[jointIndex].parentIndex;
-    if (parentIndex != -1) {
-        glm::vec3 boneVector = extractTranslation(_jointStates[jointIndex].transform) -
-            extractTranslation(_jointStates[parentIndex].transform);
-        float boneLength = glm::length(boneVector);
-        if (boneLength > EPSILON) {
-            boneVector /= boneLength;
-            applyRotationDelta(parentIndex, rotationBetween(
-                glm::cross(boneVector, glm::cross(palmRotation * geometry.palmDirection, boneVector)),
-                glm::cross(boneVector, glm::cross(palm.getNormal(), boneVector))), false);
-            updateJointState(jointIndex);
-            getJointRotation(jointIndex, palmRotation, true);
-        }
-    }
-    
-    // continue with the wrist
-    applyRotationDelta(jointIndex, rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()));
+    applyRotationDelta(jointIndex, rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()), false);
     getJointRotation(jointIndex, palmRotation, true);
     
     // sort the finger indices by raw x, get the average direction
@@ -181,7 +163,7 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
     float directionLength = glm::length(direction);
     const int MIN_ROTATION_FINGERS = 3;
     if (directionLength > EPSILON && palm.getNumFingers() >= MIN_ROTATION_FINGERS) {
-        applyRotationDelta(jointIndex, rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction));
+        applyRotationDelta(jointIndex, rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction), false);
         getJointRotation(jointIndex, palmRotation, true);
     }
     

From 70ca1106b11a3d38704056b20855d8e41f2dbaa0 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Tue, 3 Dec 2013 17:28:36 -0800
Subject: [PATCH 04/14] Working on testing the Leap/Hydra hands against the
 avatar body.

---
 interface/src/Util.cpp                 |  5 ++-
 interface/src/Util.h                   |  2 +
 interface/src/avatar/Hand.cpp          | 20 +++++++++
 interface/src/avatar/SkeletonModel.cpp | 21 ++-------
 interface/src/renderer/Model.cpp       | 60 ++++++++++++++++++++++----
 interface/src/renderer/Model.h         |  9 ++++
 libraries/avatars/src/HandData.cpp     | 42 ++++++++++++++++++
 libraries/avatars/src/HandData.h       | 15 +++++--
 8 files changed, 144 insertions(+), 30 deletions(-)

diff --git a/interface/src/Util.cpp b/interface/src/Util.cpp
index a5c12b3edc..476e77cd97 100644
--- a/interface/src/Util.cpp
+++ b/interface/src/Util.cpp
@@ -229,7 +229,10 @@ glm::vec3 extractScale(const glm::mat4& matrix) {
 }
 
 float extractUniformScale(const glm::mat4& matrix) {
-    glm::vec3 scale = extractScale(matrix);
+    return extractUniformScale(extractScale(matrix));
+}
+
+float extractUniformScale(const glm::vec3& scale) {
     return (scale.x + scale.y + scale.z) / 3.0f;
 }
 
diff --git a/interface/src/Util.h b/interface/src/Util.h
index b80731bd20..d5254a3904 100644
--- a/interface/src/Util.h
+++ b/interface/src/Util.h
@@ -65,6 +65,8 @@ glm::vec3 extractScale(const glm::mat4& matrix);
 
 float extractUniformScale(const glm::mat4& matrix);
 
+float extractUniformScale(const glm::vec3& scale);
+
 double diffclock(timeval *clock1,timeval *clock2);
 
 void renderMouseVoxelGrid(const float& mouseVoxelX, const float& mouseVoxelY, const float& mouseVoxelZ, const float& mouseVoxelS);
diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index f733a40c9f..658ca2bd89 100755
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -97,6 +97,26 @@ void Hand::calculateGeometry() {
     _baseOrientation = _owningAvatar->getOrientation();
     _basePosition = head.calculateAverageEyePosition() + _baseOrientation * leapHandsOffsetFromFace * head.getScale();
     
+    // use position to obtain the left and right palm indices
+    int leftPalmIndex, rightPalmIndex;   
+    getLeftRightPalmIndices(leftPalmIndex, rightPalmIndex);
+    
+    // check for collisions
+    for (int i = 0; i < getNumPalms(); i++) {
+        PalmData& palm = getPalms()[i];
+        if (!palm.isActive()) {
+            continue;
+        }
+        const float PALM_RADIUS = 0.01f;
+        glm::vec3 penetration;
+        int skipIndex = (i == leftPalmIndex) ? _owningAvatar->getSkeletonModel().getLeftHandJointIndex() :
+            (i == rightPalmIndex) ? _owningAvatar->getSkeletonModel().getRightHandJointIndex() : -1;
+        if (_owningAvatar->getSkeletonModel().findSpherePenetration(palm.getPosition(),
+                PALM_RADIUS * _owningAvatar->getScale(), penetration, skipIndex)) {
+            palm.addToPosition(-penetration);
+        }
+    }
+    
     // generate finger tip balls....
     _leapFingerTipBalls.clear();
     for (size_t i = 0; i < getNumPalms(); ++i) {
diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 42c57ed5bd..d3d7b6198c 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -29,24 +29,9 @@ void SkeletonModel::simulate(float deltaTime) {
     Model::simulate(deltaTime);
 
     // find the left and rightmost active Leap palms
-    HandData& hand = _owningAvatar->getHand();
-    int leftPalmIndex = -1;
-    float leftPalmX = FLT_MAX;
-    int rightPalmIndex = -1;    
-    float rightPalmX = -FLT_MAX;
-    for (int i = 0; i < hand.getNumPalms(); i++) {
-        if (hand.getPalms()[i].isActive()) {
-            float x = hand.getPalms()[i].getRawPosition().x;
-            if (x < leftPalmX) {
-                leftPalmIndex = i;
-                leftPalmX = x;
-            }
-            if (x > rightPalmX) {
-                rightPalmIndex = i;
-                rightPalmX = x;
-            }
-        }
-    }
+    int leftPalmIndex, rightPalmIndex;   
+    HandData& hand = _owningAvatar->getHand(); 
+    hand.getLeftRightPalmIndices(leftPalmIndex, rightPalmIndex);
     
     const float HAND_RESTORATION_RATE = 0.25f;
     
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 8be7a967f7..c2937f4cea 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -8,6 +8,8 @@
 
 #include <glm/gtx/transform.hpp>
 
+#include <GeometryUtil.h>
+
 #include "Application.h"
 #include "Model.h"
 
@@ -474,35 +476,35 @@ bool Model::getEyePositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePos
 }
 
 bool Model::setLeftHandPosition(const glm::vec3& position) {
-    return isActive() && setJointPosition(_geometry->getFBXGeometry().leftHandJointIndex, position);
+    return setJointPosition(getLeftHandJointIndex(), position);
 }
 
 bool Model::restoreLeftHandPosition(float percent) {
-    return isActive() && restoreJointPosition(_geometry->getFBXGeometry().leftHandJointIndex, percent);
+    return restoreJointPosition(getLeftHandJointIndex(), percent);
 }
 
 bool Model::setLeftHandRotation(const glm::quat& rotation) {
-    return isActive() && setJointRotation(_geometry->getFBXGeometry().leftHandJointIndex, rotation);
+    return setJointRotation(getLeftHandJointIndex(), rotation);
 }
 
 float Model::getLeftArmLength() const {
-    return isActive() ? getLimbLength(_geometry->getFBXGeometry().leftHandJointIndex) : 0.0f;
+    return getLimbLength(getLeftHandJointIndex());
 }
 
 bool Model::setRightHandPosition(const glm::vec3& position) {
-    return isActive() && setJointPosition(_geometry->getFBXGeometry().rightHandJointIndex, position);
+    return setJointPosition(getRightHandJointIndex(), position);
 }
 
 bool Model::restoreRightHandPosition(float percent) {
-    return isActive() && restoreJointPosition(_geometry->getFBXGeometry().rightHandJointIndex, percent);
+    return restoreJointPosition(getRightHandJointIndex(), percent);
 }
 
 bool Model::setRightHandRotation(const glm::quat& rotation) {
-    return isActive() && setJointRotation(_geometry->getFBXGeometry().rightHandJointIndex, rotation);
+    return setJointRotation(getRightHandJointIndex(), rotation);
 }
 
 float Model::getRightArmLength() const {
-    return isActive() ? getLimbLength(_geometry->getFBXGeometry().rightHandJointIndex) : 0.0f;
+    return getLimbLength(getRightHandJointIndex());
 }
 
 void Model::setURL(const QUrl& url) {
@@ -523,6 +525,48 @@ glm::vec4 Model::computeAverageColor() const {
     return _geometry ? _geometry->computeAverageColor() : glm::vec4(1.0f, 1.0f, 1.0f, 1.0f);
 }
 
+bool Model::findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
+        glm::vec3& penetration, int skipIndex) const {
+    const glm::vec3 relativeCenter = penetratorCenter - _translation;
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    bool didPenetrate = false;
+    glm::vec3 totalPenetration;
+    float radiusScale = extractUniformScale(_scale);
+    for (int i = 0; i < _jointStates.size(); i++) {
+        const FBXJoint& joint = geometry.joints[i];
+        glm::vec3 end = extractTranslation(_jointStates[i].transform);
+        float endRadius = joint.boneRadius * radiusScale;
+        glm::vec3 start = end;
+        float startRadius = joint.boneRadius * radiusScale;
+        glm::vec3 bonePenetration;
+        if (joint.parentIndex != -1) {
+            if (skipIndex != -1) {
+                int ancestorIndex = joint.parentIndex;
+                do {
+                    if (ancestorIndex == skipIndex) {
+                        goto outerContinue;
+                    }
+                    ancestorIndex = geometry.joints[ancestorIndex].parentIndex;
+                    
+                } while (ancestorIndex != -1);
+            }
+            start = extractTranslation(_jointStates[joint.parentIndex].transform);
+            startRadius = geometry.joints[joint.parentIndex].boneRadius;
+        }
+        if (findSphereCapsulePenetration(relativeCenter, penetratorRadius, start, end,
+                (startRadius + endRadius) / 2.0f, bonePenetration)) {
+            totalPenetration = addPenetrations(totalPenetration, bonePenetration);
+            didPenetrate = true; 
+        }
+        outerContinue: ;
+    }
+    if (didPenetrate) {
+        penetration = totalPenetration;
+        return true;
+    }
+    return false;
+}
+
 void Model::updateJointState(int index) {
     JointState& state = _jointStates[index];
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index ed2d292ee4..171d45442f 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -54,6 +54,12 @@ public:
     Q_INVOKABLE void setURL(const QUrl& url);
     const QUrl& getURL() const { return _url; }
     
+    /// Returns the index of the left hand joint, or -1 if not found.
+    int getLeftHandJointIndex() const { return isActive() ? _geometry->getFBXGeometry().leftHandJointIndex : -1; }
+    
+    /// Returns the index of the right hand joint, or -1 if not found.
+    int getRightHandJointIndex() const { return isActive() ? _geometry->getFBXGeometry().rightHandJointIndex : -1; }
+    
     /// Returns the position of the head joint.
     /// \return whether or not the head was found
     bool getHeadPosition(glm::vec3& headPosition) const;
@@ -105,6 +111,9 @@ public:
     /// Returns the average color of all meshes in the geometry.
     glm::vec4 computeAverageColor() const;
 
+    bool findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
+        glm::vec3& penetration, int skipIndex = -1) const;
+
 protected:
 
     QSharedPointer<NetworkGeometry> _geometry;
diff --git a/libraries/avatars/src/HandData.cpp b/libraries/avatars/src/HandData.cpp
index cfa43725b8..9ea3084ffa 100644
--- a/libraries/avatars/src/HandData.cpp
+++ b/libraries/avatars/src/HandData.cpp
@@ -29,11 +29,40 @@ HandData::HandData(AvatarData* owningAvatar) :
     addNewPalm();
 }
 
+glm::vec3 HandData::worldPositionToLeapPosition(const glm::vec3& worldPosition) const {
+    return glm::inverse(_baseOrientation) * (worldPosition - _basePosition) / LEAP_UNIT_SCALE;
+}
+
+glm::vec3 HandData::worldVectorToLeapVector(const glm::vec3& worldVector) const {
+    return glm::inverse(_baseOrientation) * worldVector / LEAP_UNIT_SCALE;
+}
+
 PalmData& HandData::addNewPalm()  {
     _palms.push_back(PalmData(this));
     return _palms.back();
 }
 
+void HandData::getLeftRightPalmIndices(int& leftPalmIndex, int& rightPalmIndex) const {
+    leftPalmIndex = -1;
+    float leftPalmX = FLT_MAX;
+    rightPalmIndex = -1;    
+    float rightPalmX = -FLT_MAX;
+    for (int i = 0; i < _palms.size(); i++) {
+        const PalmData& palm = _palms[i];
+        if (palm.isActive()) {
+            float x = palm.getRawPosition().x;
+            if (x < leftPalmX) {
+                leftPalmIndex = i;
+                leftPalmX = x;
+            }
+            if (x > rightPalmX) {
+                rightPalmIndex = i;
+                rightPalmX = x;
+            }
+        }
+    }
+}
+
 PalmData::PalmData(HandData* owningHandData) :
 _rawPosition(0, 0, 0),
 _rawNormal(0, 1, 0),
@@ -49,6 +78,19 @@ _owningHandData(owningHandData)
     }
 }
 
+void PalmData::addToPosition(const glm::vec3& delta) {
+    // convert to Leap coordinates, then add to palm and finger positions
+    glm::vec3 leapDelta = _owningHandData->worldVectorToLeapVector(delta);
+    _rawPosition += leapDelta;
+    for (int i = 0; i < getNumFingers(); i++) {
+        FingerData& finger = _fingers[i];
+        if (finger.isActive()) {
+            finger.setRawTipPosition(finger.getTipRawPosition() + leapDelta);
+            finger.setRawRootPosition(finger.getRootRawPosition() + leapDelta);
+        }
+    }
+}
+
 FingerData::FingerData(PalmData* owningPalmData, HandData* owningHandData) :
 _tipRawPosition(0, 0, 0),
 _rootRawPosition(0, 0, 0),
diff --git a/libraries/avatars/src/HandData.h b/libraries/avatars/src/HandData.h
index b8a06d53ad..a267707b41 100755
--- a/libraries/avatars/src/HandData.h
+++ b/libraries/avatars/src/HandData.h
@@ -47,6 +47,8 @@ const int BUTTON_3 = 8;
 const int BUTTON_4 = 16;
 const int BUTTON_FWD = 128;
 
+const float LEAP_UNIT_SCALE = 0.001f; ///< convert mm to meters
+
 class HandData {
 public:
     HandData(AvatarData* owningAvatar);
@@ -54,20 +56,25 @@ public:
     
     // These methods return the positions in Leap-relative space.
     // To convert to world coordinates, use Hand::leapPositionToWorldPosition.
-    
+
     // position conversion
     glm::vec3 leapPositionToWorldPosition(const glm::vec3& leapPosition) {
-        const float unitScale = 0.001;            // convert mm to meters
-        return _basePosition + _baseOrientation * (leapPosition * unitScale);
+        return _basePosition + _baseOrientation * (leapPosition * LEAP_UNIT_SCALE);
     }
     glm::vec3 leapDirectionToWorldDirection(const glm::vec3& leapDirection) {
         return glm::normalize(_baseOrientation * leapDirection);
     }
+    glm::vec3 worldPositionToLeapPosition(const glm::vec3& worldPosition) const;
+    glm::vec3 worldVectorToLeapVector(const glm::vec3& worldVector) const;
 
     std::vector<PalmData>& getPalms()    { return _palms; }
     size_t                 getNumPalms() { return _palms.size(); }
     PalmData&              addNewPalm();
 
+    /// Finds the indices of the left and right palms according to their locations, or -1 if either or
+    /// both is not found.
+    void getLeftRightPalmIndices(int& leftPalmIndex, int& rightPalmIndex) const;
+
     void setFingerTrailLength(unsigned int length);
     void updateFingerTrails();
 
@@ -153,6 +160,8 @@ public:
     void setVelocity(const glm::vec3& velocity) { _velocity = velocity; }
     const glm::vec3& getVelocity()  const { return _velocity; }
 
+    void addToPosition(const glm::vec3& delta);
+
     void incrementFramesWithoutData()          { _numFramesWithoutData++; }
     void resetFramesWithoutData()              { _numFramesWithoutData = 0; }
     int  getFramesWithoutData()          const { return _numFramesWithoutData; }

From 9b3560d414c0c8b83d866c1f9b9d3a860aabd226 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Thu, 5 Dec 2013 11:10:22 -0800
Subject: [PATCH 05/14] Skip the hands' parents as well as the hands
 themselves.

---
 interface/src/avatar/Hand.cpp    | 8 +++++---
 interface/src/renderer/Model.cpp | 4 ++++
 interface/src/renderer/Model.h   | 3 +++
 3 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index 79e8f2fdc4..1b44f75536 100755
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -165,9 +165,11 @@ void Hand::calculateGeometry() {
         }
         const float PALM_RADIUS = 0.01f;
         glm::vec3 penetration;
-        int skipIndex = (i == leftPalmIndex) ? _owningAvatar->getSkeletonModel().getLeftHandJointIndex() :
-            (i == rightPalmIndex) ? _owningAvatar->getSkeletonModel().getRightHandJointIndex() : -1;
-        if (_owningAvatar->getSkeletonModel().findSpherePenetration(palm.getPosition(),
+        const Model& skeletonModel = _owningAvatar->getSkeletonModel();
+        int skipIndex = skeletonModel.getParentJointIndex(
+            (i == leftPalmIndex) ? skeletonModel.getLeftHandJointIndex() :
+                (i == rightPalmIndex) ? skeletonModel.getRightHandJointIndex() : -1);
+        if (skeletonModel.findSpherePenetration(palm.getPosition(),
                 PALM_RADIUS * _owningAvatar->getScale(), penetration, skipIndex)) {
             palm.addToPosition(-penetration);
         }
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index c2937f4cea..5e2256de6b 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -454,6 +454,10 @@ bool Model::render(float alpha) {
     return true;
 }
 
+int Model::getParentJointIndex(int jointIndex) const {
+    return (isActive() && jointIndex != -1) ? _geometry->getFBXGeometry().joints.at(jointIndex).parentIndex : -1;
+}
+
 bool Model::getHeadPosition(glm::vec3& headPosition) const {
     return isActive() && getJointPosition(_geometry->getFBXGeometry().headJointIndex, headPosition);
 }
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index 171d45442f..43aee019f2 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -60,6 +60,9 @@ public:
     /// Returns the index of the right hand joint, or -1 if not found.
     int getRightHandJointIndex() const { return isActive() ? _geometry->getFBXGeometry().rightHandJointIndex : -1; }
     
+    /// Returns the index of the parent of the indexed joint, or -1 if not found.
+    int getParentJointIndex(int jointIndex) const;
+    
     /// Returns the position of the head joint.
     /// \return whether or not the head was found
     bool getHeadPosition(glm::vec3& headPosition) const;

From 0ea0887f6639aef12ae41eb1881ec811d8fd8b4f Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Thu, 5 Dec 2013 11:21:56 -0800
Subject: [PATCH 06/14] Add a scale factor to bone radii.

---
 interface/src/avatar/Hand.cpp    | 2 +-
 interface/src/renderer/Model.cpp | 4 ++--
 interface/src/renderer/Model.h   | 2 +-
 3 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index 1b44f75536..a4beb9775c 100755
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -170,7 +170,7 @@ void Hand::calculateGeometry() {
             (i == leftPalmIndex) ? skeletonModel.getLeftHandJointIndex() :
                 (i == rightPalmIndex) ? skeletonModel.getRightHandJointIndex() : -1);
         if (skeletonModel.findSpherePenetration(palm.getPosition(),
-                PALM_RADIUS * _owningAvatar->getScale(), penetration, skipIndex)) {
+                PALM_RADIUS * _owningAvatar->getScale(), penetration, 0.001f, skipIndex)) {
             palm.addToPosition(-penetration);
         }
     }
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 5e2256de6b..c9c69a7a87 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -530,12 +530,12 @@ glm::vec4 Model::computeAverageColor() const {
 }
 
 bool Model::findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
-        glm::vec3& penetration, int skipIndex) const {
+        glm::vec3& penetration, float boneScale, int skipIndex) const {
     const glm::vec3 relativeCenter = penetratorCenter - _translation;
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     bool didPenetrate = false;
     glm::vec3 totalPenetration;
-    float radiusScale = extractUniformScale(_scale);
+    float radiusScale = extractUniformScale(_scale) * boneScale;
     for (int i = 0; i < _jointStates.size(); i++) {
         const FBXJoint& joint = geometry.joints[i];
         glm::vec3 end = extractTranslation(_jointStates[i].transform);
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index 43aee019f2..d39972da47 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -115,7 +115,7 @@ public:
     glm::vec4 computeAverageColor() const;
 
     bool findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
-        glm::vec3& penetration, int skipIndex = -1) const;
+        glm::vec3& penetration, float boneScale = 1.0f, int skipIndex = -1) const;
 
 protected:
 

From 1edfc90e5e0314808993ca351d999eefbce582cb Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Thu, 5 Dec 2013 12:02:48 -0800
Subject: [PATCH 07/14] Missed a scale multiplier.

---
 interface/src/renderer/Model.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index c9c69a7a87..3f3f432ebd 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -555,7 +555,7 @@ bool Model::findSpherePenetration(const glm::vec3& penetratorCenter, float penet
                 } while (ancestorIndex != -1);
             }
             start = extractTranslation(_jointStates[joint.parentIndex].transform);
-            startRadius = geometry.joints[joint.parentIndex].boneRadius;
+            startRadius = geometry.joints[joint.parentIndex].boneRadius * radiusScale;
         }
         if (findSphereCapsulePenetration(relativeCenter, penetratorRadius, start, end,
                 (startRadius + endRadius) / 2.0f, bonePenetration)) {

From b90c5bdae81345cda69b0640d73bce6493862315 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Thu, 5 Dec 2013 14:00:38 -0800
Subject: [PATCH 08/14] Option to render collision proxies, more general avatar
 penetration test, fix for meshes with only one joint influence.

---
 interface/src/Menu.cpp                 |  1 +
 interface/src/Menu.h                   |  1 +
 interface/src/avatar/Avatar.cpp        | 24 ++++++++++++++++++
 interface/src/avatar/Avatar.h          |  9 +++++++
 interface/src/avatar/FaceModel.cpp     | 11 +++++++++
 interface/src/avatar/FaceModel.h       |  1 +
 interface/src/avatar/Hand.cpp          |  4 +--
 interface/src/avatar/SkeletonModel.cpp |  5 ++++
 interface/src/renderer/FBXReader.cpp   | 27 +++++++++++++-------
 interface/src/renderer/Model.cpp       | 34 ++++++++++++++++++++++++++
 interface/src/renderer/Model.h         |  2 ++
 11 files changed, 108 insertions(+), 11 deletions(-)

diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp
index 8a3debfa5f..220ed25a2c 100644
--- a/interface/src/Menu.cpp
+++ b/interface/src/Menu.cpp
@@ -318,6 +318,7 @@ Menu::Menu() :
     
     addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::Avatars, 0, true);
     addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::AvatarAsBalls);
+    addCheckableActionToQMenuAndActionHash(avatarOptionsMenu, MenuOption::CollisionProxies);
     
     addActionToQMenuAndActionHash(avatarOptionsMenu,
                                   MenuOption::VoxelMode,
diff --git a/interface/src/Menu.h b/interface/src/Menu.h
index 65bcdc11f5..a2d003af4a 100644
--- a/interface/src/Menu.h
+++ b/interface/src/Menu.h
@@ -153,6 +153,7 @@ namespace MenuOption {
     const QString Bandwidth = "Bandwidth Display";
     const QString BandwidthDetails = "Bandwidth Details";
     const QString ChatCircling = "Chat Circling";
+    const QString CollisionProxies = "Collision Proxies";
     const QString Collisions = "Collisions";
     const QString CopyVoxels = "Copy";
     const QString CoverageMap = "Render Coverage Map";
diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp
index 0db3e6e3ee..b5835df32f 100755
--- a/interface/src/avatar/Avatar.cpp
+++ b/interface/src/avatar/Avatar.cpp
@@ -16,6 +16,8 @@
 #include <PacketHeaders.h>
 #include <SharedUtil.h>
 
+#include <GeometryUtil.h>
+
 #include "Application.h"
 #include "Avatar.h"
 #include "DataServerClient.h"
@@ -818,6 +820,28 @@ bool Avatar::findRayIntersection(const glm::vec3& origin, const glm::vec3& direc
     return true;
 }
 
+bool Avatar::findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
+        glm::vec3& penetration, int skeletonSkipIndex) {
+    bool didPenetrate = false;
+    glm::vec3 totalPenetration;
+    glm::vec3 skeletonPenetration;
+    if (_skeletonModel.findSpherePenetration(penetratorCenter, penetratorRadius,
+            skeletonPenetration, 1.0f, skeletonSkipIndex)) {
+        addPenetrations(totalPenetration, skeletonPenetration);
+        didPenetrate = true; 
+    }
+    glm::vec3 facePenetration;
+    if (_head.getFaceModel().findSpherePenetration(penetratorCenter, penetratorRadius, facePenetration)) {
+        addPenetrations(totalPenetration, facePenetration);
+        didPenetrate = true; 
+    }
+    if (didPenetrate) {
+        penetration = totalPenetration;
+        return true;
+    }
+    return false;
+}
+
 int Avatar::parseData(unsigned char* sourceBuffer, int numBytes) {
     // change in position implies movement
     glm::vec3 oldPosition = _position;
diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h
index cc18ce7f1a..774e98be67 100755
--- a/interface/src/avatar/Avatar.h
+++ b/interface/src/avatar/Avatar.h
@@ -174,6 +174,15 @@ public:
     /// \return whether or not the ray intersected
     bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const;
 
+    /// Checks for penetration between the described sphere and the avatar.
+    /// \param penetratorCenter the center of the penetration test sphere
+    /// \param penetratorRadius the radius of the penetration test sphere
+    /// \param penetration[out] the vector in which to store the penetration
+    /// \param skeletonSkipIndex if not -1, the index of a joint to skip (along with its descendents) in the skeleton model
+    /// \return whether or not the sphere penetrated
+    bool findSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
+        glm::vec3& penetration, int skeletonSkipIndex = -1);
+
     virtual int parseData(unsigned char* sourceBuffer, int numBytes);
 
     static void renderJointConnectingCone(glm::vec3 position1, glm::vec3 position2, float radius1, float radius2);
diff --git a/interface/src/avatar/FaceModel.cpp b/interface/src/avatar/FaceModel.cpp
index 1e3c50f193..ebcb8cfa34 100644
--- a/interface/src/avatar/FaceModel.cpp
+++ b/interface/src/avatar/FaceModel.cpp
@@ -11,6 +11,7 @@
 #include "Avatar.h"
 #include "FaceModel.h"
 #include "Head.h"
+#include "Menu.h"
 
 FaceModel::FaceModel(Head* owningHead) :
     _owningHead(owningHead)
@@ -46,6 +47,16 @@ void FaceModel::simulate(float deltaTime) {
     Model::simulate(deltaTime);
 }
 
+bool FaceModel::render(float alpha) {
+    if (!Model::render(alpha)) {
+        return false;
+    }
+    if (Menu::getInstance()->isOptionChecked(MenuOption::CollisionProxies)) {
+        renderCollisionProxies(alpha);
+    }
+    return true;
+}
+
 void FaceModel::maybeUpdateNeckRotation(const JointState& parentState, const FBXJoint& joint, JointState& state) {
     // get the rotation axes in joint space and use them to adjust the rotation
     glm::mat3 axes = glm::mat3_cast(_rotation);
diff --git a/interface/src/avatar/FaceModel.h b/interface/src/avatar/FaceModel.h
index acf2d2baf4..d0f0f6baef 100644
--- a/interface/src/avatar/FaceModel.h
+++ b/interface/src/avatar/FaceModel.h
@@ -22,6 +22,7 @@ public:
     FaceModel(Head* owningHead);
 
     void simulate(float deltaTime);
+    bool render(float alpha);
     
 protected:
 
diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index a4beb9775c..29f344e199 100755
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -169,8 +169,8 @@ void Hand::calculateGeometry() {
         int skipIndex = skeletonModel.getParentJointIndex(
             (i == leftPalmIndex) ? skeletonModel.getLeftHandJointIndex() :
                 (i == rightPalmIndex) ? skeletonModel.getRightHandJointIndex() : -1);
-        if (skeletonModel.findSpherePenetration(palm.getPosition(),
-                PALM_RADIUS * _owningAvatar->getScale(), penetration, 0.001f, skipIndex)) {
+        if (_owningAvatar->findSpherePenetration(palm.getPosition(),
+                PALM_RADIUS * _owningAvatar->getScale(), penetration, skipIndex)) {
             palm.addToPosition(-penetration);
         }
     }
diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index d3d7b6198c..2605c8438f 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -10,6 +10,7 @@
 
 #include "Application.h"
 #include "Avatar.h"
+#include "Menu.h"
 #include "SkeletonModel.h"
 
 SkeletonModel::SkeletonModel(Avatar* owningAvatar) :
@@ -106,6 +107,10 @@ bool SkeletonModel::render(float alpha) {
     
     Model::render(alpha);
     
+    if (Menu::getInstance()->isOptionChecked(MenuOption::CollisionProxies)) {
+        renderCollisionProxies(alpha);
+    }
+    
     return true;
 }
 
diff --git a/interface/src/renderer/FBXReader.cpp b/interface/src/renderer/FBXReader.cpp
index 9f764af060..61c0ae775b 100644
--- a/interface/src/renderer/FBXReader.cpp
+++ b/interface/src/renderer/FBXReader.cpp
@@ -1308,15 +1308,16 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                     totalWeight += weight;
                     for (QMultiHash<int, int>::const_iterator it = extracted.newIndices.constFind(oldIndex);
                             it != extracted.newIndices.end() && it.key() == oldIndex; it++) {
-                        // expand the bone radius
-                        if (weight > 0.25f) {
+                        // expand the bone radius for vertices with at least 1/4 weight
+                        const float EXPANSION_WEIGHT_THRESHOLD = 0.25f;
+                        if (weight > EXPANSION_WEIGHT_THRESHOLD) {
                             const glm::vec3& vertex = extracted.mesh.vertices.at(it.value());
                             float proj = glm::dot(boneDirection, vertex - boneEnd);
                             if (proj < 0.0f && proj > -boneLength) {
                                 joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance(
                                     vertex, boneEnd + boneDirection * proj));
-                        }   }
-                        
+                            }
+                        }
                         
                         // look for an unused slot in the weights vector
                         glm::vec4& weights = extracted.mesh.clusterWeights[it.value()];
@@ -1338,15 +1339,23 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
             int jointIndex = maxJointIndex;
             FBXJoint& joint = geometry.joints[jointIndex];
             glm::vec3 boneEnd = extractTranslation(inverseModelTransform * joint.bindTransform);
-            glm::vec3 boneStart = boneEnd;
+            glm::vec3 boneDirection;
+            float boneLength;
             if (joint.parentIndex != -1) {
-                boneStart = extractTranslation(inverseModelTransform * geometry.joints[joint.parentIndex].bindTransform);
+                boneDirection = boneEnd - extractTranslation(inverseModelTransform *
+                    geometry.joints[joint.parentIndex].bindTransform);
+                boneLength = glm::length(boneDirection);
+                if (boneLength > EPSILON) {
+                    boneDirection /= boneLength;
+                }
             }
             float radiusScale = extractUniformScale(joint.transform * firstFBXCluster.inverseBindMatrix);
             foreach (const glm::vec3& vertex, extracted.mesh.vertices) {
-                // expand the bone radius
-                joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::length(
-                    computeVectorFromPointToSegment(vertex, boneStart, boneEnd)));
+                float proj = glm::dot(boneDirection, vertex - boneEnd);
+                if (proj < 0.0f && proj > -boneLength) {
+                    joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance(
+                        vertex, boneEnd + boneDirection * proj));
+                }
             }
         }
         extracted.mesh.isEye = (maxJointIndex == geometry.leftEyeJointIndex || maxJointIndex == geometry.rightEyeJointIndex);
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 3f3f432ebd..c9cf5d31b0 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -744,6 +744,40 @@ void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool cons
     state.rotation = newRotation;
 }
 
+void Model::renderCollisionProxies(float alpha) {
+    glPushMatrix();
+    Application::getInstance()->loadTranslatedViewMatrix(_translation);
+
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    float uniformScale = extractUniformScale(_scale);
+    for (int i = 0; i < _jointStates.size(); i++) {
+        glPushMatrix();
+        
+        glm::vec3 position = extractTranslation(_jointStates[i].transform);
+        glTranslatef(position.x, position.y, position.z);
+        
+        glm::quat rotation;
+        getJointRotation(i, rotation);
+        glm::vec3 axis = glm::axis(rotation);
+        glRotatef(glm::angle(rotation), axis.x, axis.y, axis.z);
+        
+        glColor4f(0.75f, 0.75f, 0.75f, alpha);
+        float scaledRadius = geometry.joints[i].boneRadius * uniformScale;
+        const int BALL_SUBDIVISIONS = 10;
+        glutSolidSphere(scaledRadius, BALL_SUBDIVISIONS, BALL_SUBDIVISIONS);
+        
+        glPopMatrix();
+        
+        int parentIndex = geometry.joints[i].parentIndex;
+        if (parentIndex != -1) {
+            Avatar::renderJointConnectingCone(extractTranslation(_jointStates[parentIndex].transform), position,
+                geometry.joints[parentIndex].boneRadius * uniformScale, scaledRadius);
+        }
+    }
+    
+    glPopMatrix();
+}
+
 void Model::setJointTranslation(int jointIndex, int parentIndex, int childIndex, const glm::vec3& translation) {
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     JointState& state = _jointStates[jointIndex];
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index d39972da47..98bff597a0 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -171,6 +171,8 @@ protected:
     
     void applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain = true);
     
+    void renderCollisionProxies(float alpha);
+    
 private:
     
     void setJointTranslation(int jointIndex, int parentIndex, int childIndex, const glm::vec3& translation);

From d2cb6f0243f51e9aa401d5ed607aec12eea21163 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Thu, 5 Dec 2013 14:19:13 -0800
Subject: [PATCH 09/14] Test hand against other avatars.

---
 interface/src/avatar/Hand.cpp | 84 +++++++++++++++++++++++------------
 interface/src/avatar/Hand.h   |  2 +
 2 files changed, 57 insertions(+), 29 deletions(-)

diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index 29f344e199..54e8413f31 100755
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -8,6 +8,8 @@
 
 #include <NodeList.h>
 
+#include <GeometryUtil.h>
+
 #include "Application.h"
 #include "Avatar.h"
 #include "Hand.h"
@@ -15,7 +17,6 @@
 #include "Util.h"
 #include "renderer/ProgramObject.h"
 
-
 using namespace std;
 
 Hand::Hand(Avatar* owningAvatar) :
@@ -59,6 +60,16 @@ void Hand::simulate(float deltaTime, bool isMine) {
         _collisionAge += deltaTime;
     }
     
+    const glm::vec3 leapHandsOffsetFromFace(0.0, -0.2, -0.3);  // place the hand in front of the face where we can see it
+    
+    Head& head = _owningAvatar->getHead();
+    _baseOrientation = _owningAvatar->getOrientation();
+    _basePosition = head.calculateAverageEyePosition() + _baseOrientation * leapHandsOffsetFromFace * head.getScale();
+    
+    if (isMine) {
+        updateCollisions();
+    }
+    
     calculateGeometry();
 
     if (_isRaveGloveActive) {
@@ -126,6 +137,49 @@ void Hand::simulate(float deltaTime, bool isMine) {
     }
 }
 
+void Hand::updateCollisions() {
+    // use position to obtain the left and right palm indices
+    int leftPalmIndex, rightPalmIndex;   
+    getLeftRightPalmIndices(leftPalmIndex, rightPalmIndex);
+    
+    // check for collisions
+    for (int i = 0; i < getNumPalms(); i++) {
+        PalmData& palm = getPalms()[i];
+        if (!palm.isActive()) {
+            continue;
+        }
+        const float PALM_RADIUS = 0.01f;
+        float scaledPalmRadius = PALM_RADIUS * _owningAvatar->getScale();
+        glm::vec3 totalPenetration;
+        
+        // check other avatars
+        NodeList* nodeList = NodeList::getInstance();
+        for (NodeList::iterator node = nodeList->begin(); node != nodeList->end(); node++) {
+            if (node->getLinkedData() && node->getType() == NODE_TYPE_AGENT) {
+                Avatar* otherAvatar = (Avatar*)node->getLinkedData();
+                glm::vec3 avatarPenetration;
+                if (otherAvatar->findSpherePenetration(palm.getPosition(), scaledPalmRadius, avatarPenetration)) {
+                    totalPenetration = addPenetrations(totalPenetration, avatarPenetration);
+                }
+            }
+        }
+            
+        // and the current avatar
+        glm::vec3 owningPenetration;
+        const Model& skeletonModel = _owningAvatar->getSkeletonModel();
+        int skipIndex = skeletonModel.getParentJointIndex(
+            (i == leftPalmIndex) ? skeletonModel.getLeftHandJointIndex() :
+                (i == rightPalmIndex) ? skeletonModel.getRightHandJointIndex() : -1);
+        if (_owningAvatar->findSpherePenetration(palm.getPosition(),
+                PALM_RADIUS * _owningAvatar->getScale(), owningPenetration, skipIndex)) {
+            totalPenetration = addPenetrations(totalPenetration, owningPenetration);
+        }
+        
+        // un-penetrate
+        palm.addToPosition(-totalPenetration);
+    }
+}
+
 void Hand::handleVoxelCollision(PalmData* palm, const glm::vec3& fingerTipPosition, VoxelTreeElement* voxel, float deltaTime) {
     //  Collision between finger and a voxel plays sound
     const float LOWEST_FREQUENCY = 100.f;
@@ -147,34 +201,6 @@ void Hand::handleVoxelCollision(PalmData* palm, const glm::vec3& fingerTipPositi
 }
 
 void Hand::calculateGeometry() {
-    const glm::vec3 leapHandsOffsetFromFace(0.0, -0.2, -0.3);  // place the hand in front of the face where we can see it
-    
-    Head& head = _owningAvatar->getHead();
-    _baseOrientation = _owningAvatar->getOrientation();
-    _basePosition = head.calculateAverageEyePosition() + _baseOrientation * leapHandsOffsetFromFace * head.getScale();
-    
-    // use position to obtain the left and right palm indices
-    int leftPalmIndex, rightPalmIndex;   
-    getLeftRightPalmIndices(leftPalmIndex, rightPalmIndex);
-    
-    // check for collisions
-    for (int i = 0; i < getNumPalms(); i++) {
-        PalmData& palm = getPalms()[i];
-        if (!palm.isActive()) {
-            continue;
-        }
-        const float PALM_RADIUS = 0.01f;
-        glm::vec3 penetration;
-        const Model& skeletonModel = _owningAvatar->getSkeletonModel();
-        int skipIndex = skeletonModel.getParentJointIndex(
-            (i == leftPalmIndex) ? skeletonModel.getLeftHandJointIndex() :
-                (i == rightPalmIndex) ? skeletonModel.getRightHandJointIndex() : -1);
-        if (_owningAvatar->findSpherePenetration(palm.getPosition(),
-                PALM_RADIUS * _owningAvatar->getScale(), penetration, skipIndex)) {
-            palm.addToPosition(-penetration);
-        }
-    }
-    
     // generate finger tip balls....
     _leapFingerTipBalls.clear();
     for (size_t i = 0; i < getNumPalms(); ++i) {
diff --git a/interface/src/avatar/Hand.h b/interface/src/avatar/Hand.h
index e4e9ad4b50..4b5bc204ae 100755
--- a/interface/src/avatar/Hand.h
+++ b/interface/src/avatar/Hand.h
@@ -99,6 +99,8 @@ private:
 
     void renderLeapHands();
     void renderLeapFingerTrails();
+    
+    void updateCollisions();
     void calculateGeometry();
     
     void handleVoxelCollision(PalmData* palm, const glm::vec3& fingerTipPosition, VoxelTreeElement* voxel, float deltaTime);

From 9be30acf823b8eeb5ce4af3882cb34f04e406ca6 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Thu, 5 Dec 2013 14:40:24 -0800
Subject: [PATCH 10/14] Render the palm collision proxies.

---
 interface/src/avatar/Hand.cpp | 25 ++++++++++++++++++++-----
 1 file changed, 20 insertions(+), 5 deletions(-)

diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index 54e8413f31..7e5f86f484 100755
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -137,6 +137,8 @@ void Hand::simulate(float deltaTime, bool isMine) {
     }
 }
 
+const float PALM_COLLISION_RADIUS = 0.01f;
+
 void Hand::updateCollisions() {
     // use position to obtain the left and right palm indices
     int leftPalmIndex, rightPalmIndex;   
@@ -147,9 +149,8 @@ void Hand::updateCollisions() {
         PalmData& palm = getPalms()[i];
         if (!palm.isActive()) {
             continue;
-        }
-        const float PALM_RADIUS = 0.01f;
-        float scaledPalmRadius = PALM_RADIUS * _owningAvatar->getScale();
+        }        
+        float scaledPalmRadius = PALM_COLLISION_RADIUS * _owningAvatar->getScale();
         glm::vec3 totalPenetration;
         
         // check other avatars
@@ -170,8 +171,7 @@ void Hand::updateCollisions() {
         int skipIndex = skeletonModel.getParentJointIndex(
             (i == leftPalmIndex) ? skeletonModel.getLeftHandJointIndex() :
                 (i == rightPalmIndex) ? skeletonModel.getRightHandJointIndex() : -1);
-        if (_owningAvatar->findSpherePenetration(palm.getPosition(),
-                PALM_RADIUS * _owningAvatar->getScale(), owningPenetration, skipIndex)) {
+        if (_owningAvatar->findSpherePenetration(palm.getPosition(), scaledPalmRadius, owningPenetration, skipIndex)) {
             totalPenetration = addPenetrations(totalPenetration, owningPenetration);
         }
         
@@ -269,6 +269,21 @@ void Hand::render() {
     
     _renderAlpha = 1.0;
     
+    if (Menu::getInstance()->isOptionChecked(MenuOption::CollisionProxies)) {
+        for (int i = 0; i < getNumPalms(); i++) {
+            PalmData& palm = getPalms()[i];
+            if (!palm.isActive()) {
+                continue;
+            }
+            glm::vec3 position = palm.getPosition();
+            glPushMatrix();
+            glTranslatef(position.x, position.y, position.z);
+            glColor3f(0.0f, 1.0f, 0.0f);
+            glutSolidSphere(PALM_COLLISION_RADIUS * _owningAvatar->getScale(), 10, 10);
+            glPopMatrix();
+        }
+    }
+    
     if (Menu::getInstance()->isOptionChecked(MenuOption::DisplayLeapHands)) {
         if (!isRaveGloveActive()) {
             renderLeapFingerTrails();

From d238d3c35a26de46e6c603c4b4c435150e86735c Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Thu, 5 Dec 2013 14:49:30 -0800
Subject: [PATCH 11/14] Called addPenetrations wrong.

---
 interface/src/avatar/Avatar.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp
index b5835df32f..54b5f6cc80 100755
--- a/interface/src/avatar/Avatar.cpp
+++ b/interface/src/avatar/Avatar.cpp
@@ -827,12 +827,12 @@ bool Avatar::findSpherePenetration(const glm::vec3& penetratorCenter, float pene
     glm::vec3 skeletonPenetration;
     if (_skeletonModel.findSpherePenetration(penetratorCenter, penetratorRadius,
             skeletonPenetration, 1.0f, skeletonSkipIndex)) {
-        addPenetrations(totalPenetration, skeletonPenetration);
+        totalPenetration = addPenetrations(totalPenetration, skeletonPenetration);
         didPenetrate = true; 
     }
     glm::vec3 facePenetration;
     if (_head.getFaceModel().findSpherePenetration(penetratorCenter, penetratorRadius, facePenetration)) {
-        addPenetrations(totalPenetration, facePenetration);
+        totalPenetration = addPenetrations(totalPenetration, facePenetration);
         didPenetrate = true; 
     }
     if (didPenetrate) {

From ecff3a05420d2dd397fdc70f3c2a6c00c1d2e2e5 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Thu, 5 Dec 2013 15:36:34 -0800
Subject: [PATCH 12/14] Basic sphere/capsule-cone penetration test.

---
 interface/src/renderer/Model.cpp      |  4 +--
 libraries/shared/src/GeometryUtil.cpp | 35 +++++++++++++++++++++++++++
 libraries/shared/src/GeometryUtil.h   | 10 ++++++++
 3 files changed, 47 insertions(+), 2 deletions(-)

diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index c9cf5d31b0..0b2083619c 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -557,8 +557,8 @@ bool Model::findSpherePenetration(const glm::vec3& penetratorCenter, float penet
             start = extractTranslation(_jointStates[joint.parentIndex].transform);
             startRadius = geometry.joints[joint.parentIndex].boneRadius * radiusScale;
         }
-        if (findSphereCapsulePenetration(relativeCenter, penetratorRadius, start, end,
-                (startRadius + endRadius) / 2.0f, bonePenetration)) {
+        if (findSphereCapsuleConePenetration(relativeCenter, penetratorRadius, start, end,
+                startRadius, endRadius, bonePenetration)) {
             totalPenetration = addPenetrations(totalPenetration, bonePenetration);
             didPenetrate = true; 
         }
diff --git a/libraries/shared/src/GeometryUtil.cpp b/libraries/shared/src/GeometryUtil.cpp
index b632cd4bb6..1da05f9fae 100644
--- a/libraries/shared/src/GeometryUtil.cpp
+++ b/libraries/shared/src/GeometryUtil.cpp
@@ -52,6 +52,12 @@ bool findSpherePointPenetration(const glm::vec3& penetratorCenter, float penetra
         penetratorRadius, penetration);
 }
 
+bool findPointSpherePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeCenter,
+       float penetrateeRadius, glm::vec3& penetration) {
+    return findSpherePenetration(penetrateeCenter - penetratorLocation, glm::vec3(0.0f, -1.0f, 0.0f),
+        penetrateeRadius, penetration);
+}
+
 bool findSphereSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
                                  const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration) {
     return findSpherePointPenetration(penetratorCenter, penetratorRadius + penetrateeRadius, penetrateeCenter, penetration);
@@ -69,6 +75,35 @@ bool findSphereCapsulePenetration(const glm::vec3& penetratorCenter, float penet
         penetrateeStart, penetrateeEnd, penetration);
 }
 
+bool findPointCapsuleConePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeStart,
+        const glm::vec3& penetrateeEnd, float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration) {
+    // compute the projection of the point vector onto the segment vector
+    glm::vec3 segmentVector = penetrateeEnd - penetrateeStart;
+    float lengthSquared = glm::dot(segmentVector, segmentVector);
+    if (lengthSquared < EPSILON) { // start and end the same
+        return findPointSpherePenetration(penetratorLocation, penetrateeStart,
+            glm::max(penetrateeStartRadius, penetrateeEndRadius), penetration);
+    }
+    float proj = glm::dot(penetratorLocation - penetrateeStart, segmentVector) / lengthSquared;
+    if (proj <= 0.0f) { // closest to the start
+        return findPointSpherePenetration(penetratorLocation, penetrateeStart, penetrateeStartRadius, penetration);
+        
+    } else if (proj >= 1.0f) { // closest to the end
+        return findPointSpherePenetration(penetratorLocation, penetrateeEnd, penetrateeEndRadius, penetration);
+    
+    } else { // closest to the middle
+        return findPointSpherePenetration(penetratorLocation, penetrateeStart + segmentVector * proj,
+            glm::mix(penetrateeStartRadius, penetrateeEndRadius, proj), penetration);
+    }
+}
+
+bool findSphereCapsuleConePenetration(const glm::vec3& penetratorCenter,
+        float penetratorRadius, const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd,
+        float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration) {
+    return findPointCapsuleConePenetration(penetratorCenter, penetrateeStart, penetrateeEnd,
+        penetrateeStartRadius + penetratorRadius, penetrateeEndRadius + penetratorRadius, penetration);
+}
+
 bool findSpherePlanePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, 
                                 const glm::vec4& penetrateePlane, glm::vec3& penetration) {
     float distance = glm::dot(penetrateePlane, glm::vec4(penetratorCenter, 1.0f)) - penetratorRadius;
diff --git a/libraries/shared/src/GeometryUtil.h b/libraries/shared/src/GeometryUtil.h
index bb053d20c6..8cbb29580a 100644
--- a/libraries/shared/src/GeometryUtil.h
+++ b/libraries/shared/src/GeometryUtil.h
@@ -19,6 +19,9 @@ bool findSpherePenetration(const glm::vec3& penetratorToPenetratee, const glm::v
 bool findSpherePointPenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
                                 const glm::vec3& penetrateeLocation, glm::vec3& penetration);
 
+bool findPointSpherePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeCenter,
+    float penetrateeRadius, glm::vec3& penetration);
+
 bool findSphereSpherePenetration(const glm::vec3& penetratorCenter, float penetratorRadius,
                                  const glm::vec3& penetrateeCenter, float penetrateeRadius, glm::vec3& penetration);
                      
@@ -27,6 +30,13 @@ bool findSphereSegmentPenetration(const glm::vec3& penetratorCenter, float penet
 
 bool findSphereCapsulePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, const glm::vec3& penetrateeStart,
                                   const glm::vec3& penetrateeEnd, float penetrateeRadius, glm::vec3& penetration);
+
+bool findPointCapsuleConePenetration(const glm::vec3& penetratorLocation, const glm::vec3& penetrateeStart,
+    const glm::vec3& penetrateeEnd, float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration);
+    
+bool findSphereCapsuleConePenetration(const glm::vec3& penetratorCenter,
+    float penetratorRadius, const glm::vec3& penetrateeStart, const glm::vec3& penetrateeEnd,
+    float penetrateeStartRadius, float penetrateeEndRadius, glm::vec3& penetration);
                                   
 bool findSpherePlanePenetration(const glm::vec3& penetratorCenter, float penetratorRadius, 
                                 const glm::vec4& penetrateePlane, glm::vec3& penetration);

From 02cd97deb13c0bee58362b9ce52b1c18f99449d4 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Thu, 5 Dec 2013 16:28:33 -0800
Subject: [PATCH 13/14] Adjust palm collision size, use last free index when
 ignoring.

---
 interface/src/avatar/Hand.cpp    | 4 ++--
 interface/src/renderer/Model.cpp | 4 ++++
 interface/src/renderer/Model.h   | 3 +++
 3 files changed, 9 insertions(+), 2 deletions(-)

diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index 7e5f86f484..693923caf6 100755
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -137,7 +137,7 @@ void Hand::simulate(float deltaTime, bool isMine) {
     }
 }
 
-const float PALM_COLLISION_RADIUS = 0.01f;
+const float PALM_COLLISION_RADIUS = 0.03f;
 
 void Hand::updateCollisions() {
     // use position to obtain the left and right palm indices
@@ -168,7 +168,7 @@ void Hand::updateCollisions() {
         // and the current avatar
         glm::vec3 owningPenetration;
         const Model& skeletonModel = _owningAvatar->getSkeletonModel();
-        int skipIndex = skeletonModel.getParentJointIndex(
+        int skipIndex = skeletonModel.getLastFreeJointIndex(
             (i == leftPalmIndex) ? skeletonModel.getLeftHandJointIndex() :
                 (i == rightPalmIndex) ? skeletonModel.getRightHandJointIndex() : -1);
         if (_owningAvatar->findSpherePenetration(palm.getPosition(), scaledPalmRadius, owningPenetration, skipIndex)) {
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 0b2083619c..c0403b6d71 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -458,6 +458,10 @@ int Model::getParentJointIndex(int jointIndex) const {
     return (isActive() && jointIndex != -1) ? _geometry->getFBXGeometry().joints.at(jointIndex).parentIndex : -1;
 }
 
+int Model::getLastFreeJointIndex(int jointIndex) const {
+    return (isActive() && jointIndex != -1) ? _geometry->getFBXGeometry().joints.at(jointIndex).freeLineage.last() : -1;
+}
+
 bool Model::getHeadPosition(glm::vec3& headPosition) const {
     return isActive() && getJointPosition(_geometry->getFBXGeometry().headJointIndex, headPosition);
 }
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index 98bff597a0..3550410492 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -63,6 +63,9 @@ public:
     /// Returns the index of the parent of the indexed joint, or -1 if not found.
     int getParentJointIndex(int jointIndex) const;
     
+    /// Returns the index of the last free ancestor or the indexed joint, or -1 if not found.
+    int getLastFreeJointIndex(int jointIndex) const;
+    
     /// Returns the position of the head joint.
     /// \return whether or not the head was found
     bool getHeadPosition(glm::vec3& headPosition) const;

From 16cfeff00d32c3fef4a0c4dfff448d7d1e9e8107 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Thu, 5 Dec 2013 16:37:40 -0800
Subject: [PATCH 14/14] Ignore further up the hierarchy.

---
 interface/src/avatar/Hand.cpp | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index 693923caf6..cea72398aa 100755
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -165,12 +165,12 @@ void Hand::updateCollisions() {
             }
         }
             
-        // and the current avatar
+        // and the current avatar (ignoring everything below the parent of the parent of the last free joint)
         glm::vec3 owningPenetration;
         const Model& skeletonModel = _owningAvatar->getSkeletonModel();
-        int skipIndex = skeletonModel.getLastFreeJointIndex(
-            (i == leftPalmIndex) ? skeletonModel.getLeftHandJointIndex() :
-                (i == rightPalmIndex) ? skeletonModel.getRightHandJointIndex() : -1);
+        int skipIndex = skeletonModel.getParentJointIndex(skeletonModel.getParentJointIndex(
+            skeletonModel.getLastFreeJointIndex((i == leftPalmIndex) ? skeletonModel.getLeftHandJointIndex() :
+                (i == rightPalmIndex) ? skeletonModel.getRightHandJointIndex() : -1)));
         if (_owningAvatar->findSpherePenetration(palm.getPosition(), scaledPalmRadius, owningPenetration, skipIndex)) {
             totalPenetration = addPenetrations(totalPenetration, owningPenetration);
         }