From 831a8cf580a7aea49a4156fb401d20eab48b98a3 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Wed, 5 Mar 2014 12:15:34 -0800
Subject: [PATCH 01/13] remove unused cruft from Model class API

---
 interface/src/renderer/Model.cpp | 16 ----------------
 interface/src/renderer/Model.h   | 16 ----------------
 2 files changed, 32 deletions(-)

diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 1e2c445f37..5055b16c4d 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -418,18 +418,10 @@ bool Model::getRightHandRotation(glm::quat& rotation) const {
     return getJointRotation(getRightHandJointIndex(), rotation);
 }
 
-bool Model::setLeftHandPosition(const glm::vec3& position) {
-    return setJointPosition(getLeftHandJointIndex(), position);
-}
-
 bool Model::restoreLeftHandPosition(float percent) {
     return restoreJointPosition(getLeftHandJointIndex(), percent);
 }
 
-bool Model::setLeftHandRotation(const glm::quat& rotation) {
-    return setJointRotation(getLeftHandJointIndex(), rotation);
-}
-
 bool Model::getLeftShoulderPosition(glm::vec3& position) const {
     return getJointPosition(getLastFreeJointIndex(getLeftHandJointIndex()), position);
 }
@@ -438,18 +430,10 @@ float Model::getLeftArmLength() const {
     return getLimbLength(getLeftHandJointIndex());
 }
 
-bool Model::setRightHandPosition(const glm::vec3& position) {
-    return setJointPosition(getRightHandJointIndex(), position);
-}
-
 bool Model::restoreRightHandPosition(float percent) {
     return restoreJointPosition(getRightHandJointIndex(), percent);
 }
 
-bool Model::setRightHandRotation(const glm::quat& rotation) {
-    return setJointRotation(getRightHandJointIndex(), rotation);
-}
-
 bool Model::getRightShoulderPosition(glm::vec3& position) const {
     return getJointPosition(getLastFreeJointIndex(getRightHandJointIndex()), position);
 }
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index fc4643374f..4891c86779 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -125,19 +125,11 @@ public:
     /// \return true whether or not the rotation was found
     bool getRightHandRotation(glm::quat& rotation) const;
     
-    /// Sets the position of the left hand using inverse kinematics.
-    /// \return whether or not the left hand joint was found
-    bool setLeftHandPosition(const glm::vec3& position);
-    
     /// Restores some percentage of the default position of the left hand.
     /// \param percent the percentage of the default position to restore
     /// \return whether or not the left hand joint was found
     bool restoreLeftHandPosition(float percent = 1.0f);
     
-    /// Sets the rotation of the left hand.
-    /// \return whether or not the left hand joint was found
-    bool setLeftHandRotation(const glm::quat& rotation);
-    
     /// Gets the position of the left shoulder.
     /// \return whether or not the left shoulder joint was found
     bool getLeftShoulderPosition(glm::vec3& position) const;
@@ -145,19 +137,11 @@ public:
     /// Returns the extended length from the left hand to its last free ancestor.
     float getLeftArmLength() const;
     
-    /// Sets the position of the right hand using inverse kinematics.
-    /// \return whether or not the right hand joint was found
-    bool setRightHandPosition(const glm::vec3& position);
-    
     /// Restores some percentage of the default position of the right hand.
     /// \param percent the percentage of the default position to restore
     /// \return whether or not the right hand joint was found
     bool restoreRightHandPosition(float percent = 1.0f);
     
-    /// Sets the rotation of the right hand.
-    /// \return whether or not the right hand joint was found
-    bool setRightHandRotation(const glm::quat& rotation);
-    
     /// Gets the position of the right shoulder.
     /// \return whether or not the right shoulder joint was found
     bool getRightShoulderPosition(glm::vec3& position) const;

From 778bdec6d840ff7547cb65cb24fc639c7782fd29 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 6 Mar 2014 12:34:21 -0800
Subject: [PATCH 02/13] Model now knows its bounding radius

---
 interface/src/renderer/Model.cpp | 10 +++++++++-
 interface/src/renderer/Model.h   |  6 +++++-
 2 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 5055b16c4d..7ef87eaa47 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -7,6 +7,7 @@
 //
 
 #include <glm/gtx/transform.hpp>
+#include <glm/gtx/norm.hpp>
 
 #include <GeometryUtil.h>
 
@@ -23,7 +24,8 @@ Model::Model(QObject* parent) :
     QObject(parent),
     _shapesAreDirty(true),
     _lodDistance(0.0f),
-    _pupilDilation(0.0f) {
+    _pupilDilation(0.0f),
+    _boundingRadius(0.f) {
     // we may have been created in the network thread, but we live in the main thread
     moveToThread(Application::getInstance()->thread());
 }
@@ -136,6 +138,7 @@ void Model::createCollisionShapes() {
 
 void Model::updateShapePositions() {
     if (_shapesAreDirty && _shapes.size() == _jointStates.size()) {
+        _boundingRadius = 0.f;
         float uniformScale = extractUniformScale(_scale);
         const FBXGeometry& geometry = _geometry->getFBXGeometry();
         for (int i = 0; i < _jointStates.size(); i++) {
@@ -145,7 +148,12 @@ void Model::updateShapePositions() {
             glm::vec3 worldPosition = extractTranslation(_jointStates[i].transform) + jointToShapeOffset + _translation;
             _shapes[i]->setPosition(worldPosition);
             _shapes[i]->setRotation(_jointStates[i].combinedRotation * joint.shapeRotation);
+            float distance2 = glm::distance2(worldPosition, _translation);
+            if (distance2 > _boundingRadius) {
+                _boundingRadius = distance2;
+            }
         }
+        _boundingRadius = sqrtf(_boundingRadius);
         _shapesAreDirty = false;
     }
 }
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index 4891c86779..88bfbfd3aa 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -172,6 +172,8 @@ public:
     /// Use the collision to affect the model
     void applyCollision(CollisionInfo& collision);
 
+    float getBoundingRadius() const { return _boundingRadius; }
+
 protected:
 
     QSharedPointer<NetworkGeometry> _geometry;
@@ -228,7 +230,7 @@ protected:
     /// Computes and returns the extended length of the limb terminating at the specified joint and starting at the joint's
     /// first free ancestor.
     float getLimbLength(int jointIndex) const;
-    
+
     void applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain = true);
     
 private:
@@ -257,6 +259,8 @@ private:
     QVector<glm::vec3> _blendedNormals;
     
     QVector<Model*> _attachments;
+
+    float _boundingRadius;
     
     static ProgramObject _program;
     static ProgramObject _normalMapProgram;

From bca0ea15021e7856d97f22409bf720e3a4dc04b3 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 6 Mar 2014 12:36:52 -0800
Subject: [PATCH 03/13] Adding per-palm penetration accumulator

---
 libraries/avatars/src/HandData.cpp | 11 ++++++-----
 libraries/avatars/src/HandData.h   |  4 ++++
 2 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/libraries/avatars/src/HandData.cpp b/libraries/avatars/src/HandData.cpp
index e4bb187f28..67d284d368 100644
--- a/libraries/avatars/src/HandData.cpp
+++ b/libraries/avatars/src/HandData.cpp
@@ -67,11 +67,12 @@ void HandData::getLeftRightPalmIndices(int& leftPalmIndex, int& rightPalmIndex)
 }
 
 PalmData::PalmData(HandData* owningHandData) :
-_rawRotation(0, 0, 0, 1),
-_rawPosition(0, 0, 0),
-_rawNormal(0, 1, 0),
-_rawVelocity(0, 0, 0),
-_rotationalVelocity(0, 0, 0),
+_rawRotation(0.f, 0.f, 0.f, 1.f),
+_rawPosition(0.f),
+_rawNormal(0.f, 1.f, 0.f),
+_rawVelocity(0.f),
+_rotationalVelocity(0.f),
+_totalPenetration(0.f),
 _controllerButtons(0),
 _isActive(false),
 _leapID(LEAPID_INVALID),
diff --git a/libraries/avatars/src/HandData.h b/libraries/avatars/src/HandData.h
index cdab9f71e9..0d1371b79d 100755
--- a/libraries/avatars/src/HandData.h
+++ b/libraries/avatars/src/HandData.h
@@ -168,6 +168,9 @@ public:
     void setRawVelocity(const glm::vec3& velocity) { _rawVelocity = velocity; }
     const glm::vec3& getRawVelocity()  const { return _rawVelocity; }
     void addToPosition(const glm::vec3& delta);
+
+    void addToPenetration(const glm::vec3& penetration) { _totalPenetration += penetration; }
+    void resolvePenetrations() { addToPosition(-_totalPenetration); _totalPenetration = glm::vec3(0.f); }
     
     void setTipPosition(const glm::vec3& position) { _tipPosition = position; }
     const glm::vec3 getTipPosition() const { return _owningHandData->leapPositionToWorldPosition(_tipPosition); }
@@ -217,6 +220,7 @@ private:
     
     glm::vec3 _tipPosition;
     glm::vec3 _tipVelocity;
+    glm::vec3 _totalPenetration;    // accumulator for per-frame penetrations
     int _controllerButtons;
     int _lastControllerButtons;
     float _trigger;

From 37b088ebfa5d457b66148fc423b4ee6339c7ca60 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 6 Mar 2014 12:38:07 -0800
Subject: [PATCH 04/13] Hands can collide with avatar head again

---
 interface/src/avatar/Avatar.cpp        | 31 ++++++++-------
 interface/src/avatar/Avatar.h          |  7 ++--
 interface/src/avatar/Hand.cpp          | 33 ++++------------
 interface/src/avatar/Hand.h            |  4 --
 interface/src/avatar/Head.cpp          | 35 +++++++----------
 interface/src/avatar/Head.h            |  3 +-
 interface/src/avatar/MyAvatar.cpp      | 52 ++++++++++++++------------
 interface/src/avatar/SkeletonModel.cpp | 26 ++++++++++---
 interface/src/avatar/SkeletonModel.h   |  5 ++-
 9 files changed, 96 insertions(+), 100 deletions(-)

diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp
index 3963a84724..9c27d81dcb 100644
--- a/interface/src/avatar/Avatar.cpp
+++ b/interface/src/avatar/Avatar.cpp
@@ -489,13 +489,19 @@ bool Avatar::findSphereCollisions(const glm::vec3& penetratorCenter, float penet
     //return getHead()->getFaceModel().findSphereCollisions(penetratorCenter, penetratorRadius, collisions);
 }
 
-bool Avatar::findCollisions(const QVector<const Shape*>& shapes, CollisionList& collisions) {
+void Avatar::updateShapePositions() {
     _skeletonModel.updateShapePositions();
-    bool collided = _skeletonModel.findCollisions(shapes, collisions);
-
     Model& headModel = getHead()->getFaceModel();
     headModel.updateShapePositions();
-    collided = headModel.findCollisions(shapes, collisions);
+}
+
+bool Avatar::findCollisions(const QVector<const Shape*>& shapes, CollisionList& collisions) {
+    // TODO: Andrew to fix: also collide against _skeleton
+    //bool collided = _skeletonModel.findCollisions(shapes, collisions);
+
+    Model& headModel = getHead()->getFaceModel();
+    //collided = headModel.findCollisions(shapes, collisions) || collided;
+    bool collided = headModel.findCollisions(shapes, collisions);
     return collided;
 }
 
@@ -710,15 +716,14 @@ bool Avatar::collisionWouldMoveAvatar(CollisionInfo& collision) const {
     return false;
 }
 
-void Avatar::applyCollision(CollisionInfo& collision) {
-    if (!collision._data || collision._type != MODEL_COLLISION) {
-        return;
-    }
-    // TODO: make skeleton also respond to collisions
-    Model* model = static_cast<Model*>(collision._data);
-    if (model == &(getHead()->getFaceModel())) {
-        getHead()->applyCollision(collision);
-    }
+void Avatar::applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration) {
+    // ATM we only support collision with head
+    getHead()->applyCollision(contactPoint, penetration);
+}
+
+float Avatar::getBoundingRadius() const {
+    // TODO: also use head model when computing the avatar's bounding radius
+    return _skeletonModel.getBoundingRadius();
 }
 
 float Avatar::getPelvisFloatingHeight() const {
diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h
index 1bbf7df039..2aa7eac737 100755
--- a/interface/src/avatar/Avatar.h
+++ b/interface/src/avatar/Avatar.h
@@ -138,10 +138,11 @@ public:
     /// \return true if we expect the avatar would move as a result of the collision
     bool collisionWouldMoveAvatar(CollisionInfo& collision) const;
 
-    /// \param collision a data structure for storing info about collisions against Models
-    void applyCollision(CollisionInfo& collision);
+    void applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration);
 
-    float getBoundingRadius() const { return 0.5f * getSkeletonHeight(); }
+    /// \return bounding radius of avatar
+    virtual float getBoundingRadius() const;
+    void updateShapePositions();
 
 public slots:
     void updateCollisionFlags();
diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index ce2f2a242e..a154e76a05 100644
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -145,10 +145,7 @@ void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) {
     jointIndices[0] = skeletonModel.getLeftHandJointIndex();
     jointIndices[1] = skeletonModel.getRightHandJointIndex();
 
-    palmIndices[1] = -1;    // adebug temporarily disable right hand
-    jointIndices[1] = -1;   // adebug temporarily disable right hand
-
-    for (size_t i = 0; i < 1; i++) {
+    for (size_t i = 0; i < 2; i++) {
         int palmIndex = palmIndices[i];
         int jointIndex = jointIndices[i];
         if (palmIndex == -1 || jointIndex == -1) {
@@ -165,9 +162,8 @@ void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) {
         handCollisions.clear();
         QVector<const Shape*> shapes;
         skeletonModel.getHandShapes(jointIndex, shapes);
-        bool collided = isMyHand ? avatar->findCollisions(shapes, handCollisions) : avatar->findCollisions(shapes, handCollisions);
-        if (collided) {
-        //if (avatar->findCollisions(shapes, handCollisions)) {
+
+        if (avatar->findCollisions(shapes, handCollisions)) {
             glm::vec3 averagePenetration;
             glm::vec3 averageContactPoint;
             for (int j = 0; j < handCollisions.size(); ++j) {
@@ -180,13 +176,14 @@ void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) {
                 // our hand against other avatar 
                 // for now we resolve it to test shapes/collisions
                 // TODO: only partially resolve this penetration
-                palm.addToPosition(-averagePenetration);
+                palm.addToPenetration(averagePenetration);
             } else {
                 // someone else's hand against MyAvatar
                 // TODO: submit collision info to MyAvatar which should lean accordingly
                 averageContactPoint /= float(handCollisions.size());
+                avatar->applyCollision(averageContactPoint, averagePenetration);
             }
-        }
+        } 
     }
 }
 
@@ -218,7 +215,7 @@ void Hand::collideAgainstOurself() {
                 totalPenetration = addPenetrations(totalPenetration, collision->_penetration);
             }
             // resolve penetration
-            palm.addToPosition(-totalPenetration);
+            palm.addToPenetration(totalPenetration);
         }
     }
 }
@@ -428,19 +425,3 @@ void Hand::renderLeapHands(bool isMine) {
     glPopMatrix();
 }
 
-
-void Hand::setLeapHands(const std::vector<glm::vec3>& handPositions,
-                          const std::vector<glm::vec3>& handNormals) {
-    for (size_t i = 0; i < getNumPalms(); ++i) {
-        PalmData& palm = getPalms()[i];
-        if (i < handPositions.size()) {
-            palm.setActive(true);
-            palm.setRawPosition(handPositions[i]);
-            palm.setRawNormal(handNormals[i]);
-        }
-        else {
-            palm.setActive(false);
-        }
-    }
-}
-
diff --git a/interface/src/avatar/Hand.h b/interface/src/avatar/Hand.h
index a1b1875424..6f64f62c9c 100755
--- a/interface/src/avatar/Hand.h
+++ b/interface/src/avatar/Hand.h
@@ -79,10 +79,6 @@ private:
     float _collisionAge;
     float _collisionDuration;
     
-    // private methods
-    void setLeapHands(const std::vector<glm::vec3>& handPositions,
-                      const std::vector<glm::vec3>& handNormals);
-
     void renderLeapHands(bool isMine);
     void renderLeapFingerTrails();
     
diff --git a/interface/src/avatar/Head.cpp b/interface/src/avatar/Head.cpp
index 74bf983eb1..7e9d549509 100644
--- a/interface/src/avatar/Head.cpp
+++ b/interface/src/avatar/Head.cpp
@@ -213,32 +213,25 @@ float Head::getTweakedRoll() const {
     return glm::clamp(_roll + _tweakedRoll, MIN_HEAD_ROLL, MAX_HEAD_ROLL);
 }
 
-void Head::applyCollision(CollisionInfo& collision) {
-    // HACK: the collision proxies for the FaceModel are bad.  As a temporary workaround
-    // we collide against a hard coded collision proxy.
-    // TODO: get a better collision proxy here.
-    const float HEAD_RADIUS = 0.15f;
-    const glm::vec3 HEAD_CENTER = _position;
-
-    // collide the contactPoint against the collision proxy to obtain a new penetration
-    // NOTE: that penetration is in opposite direction (points the way out for the point, not the sphere)
-    glm::vec3 penetration;
-    if (findPointSpherePenetration(collision._contactPoint, HEAD_CENTER, HEAD_RADIUS, penetration)) {
-        // compute lean angles
-        Avatar* owningAvatar = static_cast<Avatar*>(_owningAvatar);
-        glm::quat bodyRotation = owningAvatar->getOrientation();
-        glm::vec3 neckPosition;
-        if (owningAvatar->getSkeletonModel().getNeckPosition(neckPosition)) {
-            glm::vec3 xAxis = bodyRotation * glm::vec3(1.f, 0.f, 0.f);
-            glm::vec3 zAxis = bodyRotation * glm::vec3(0.f, 0.f, 1.f);
+void Head::applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration) {
+    // compute lean angles
+    Avatar* owningAvatar = static_cast<Avatar*>(_owningAvatar);
+    glm::quat bodyRotation = owningAvatar->getOrientation();
+    glm::vec3 neckPosition;
+    if (owningAvatar->getSkeletonModel().getNeckPosition(neckPosition)) {
+        glm::vec3 yAxis = bodyRotation * glm::vec3(0.f, 1.f, 0.f);
+        glm::vec3 leverArm = _position - neckPosition;
+        if (glm::dot(leverArm, yAxis) > 0.f) {
             float neckLength = glm::length(_position - neckPosition);
             if (neckLength > 0.f) {
-                float forward = glm::dot(collision._penetration, zAxis) / neckLength;
-                float sideways = - glm::dot(collision._penetration, xAxis) / neckLength;
+                glm::vec3 xAxis = bodyRotation * glm::vec3(1.f, 0.f, 0.f);
+                glm::vec3 zAxis = bodyRotation * glm::vec3(0.f, 0.f, 1.f);
+                float forward = glm::dot(penetration, zAxis) / neckLength;
+                float sideways = - glm::dot(penetration, xAxis) / neckLength;
                 addLean(sideways, forward);
             }
         }
-    } 
+    }
 }
 
 void Head::renderLookatVectors(glm::vec3 leftEyePosition, glm::vec3 rightEyePosition, glm::vec3 lookatPosition) {
diff --git a/interface/src/avatar/Head.h b/interface/src/avatar/Head.h
index 7e7a96a3a7..d127912555 100644
--- a/interface/src/avatar/Head.h
+++ b/interface/src/avatar/Head.h
@@ -78,7 +78,8 @@ public:
     virtual float getTweakedYaw() const;
     virtual float getTweakedRoll() const;
 
-    void  applyCollision(CollisionInfo& collisionInfo);
+    // move the head to avoid given collision info
+    void applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration);
     
 private:
     // disallow copies of the Head, copy of owning Avatar is disallowed too
diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index de07ed55fa..da08f7a5ee 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -182,26 +182,6 @@ void MyAvatar::simulate(float deltaTime) {
         _velocity += _scale * _gravity * (GRAVITY_EARTH * deltaTime);
     }
 
-    if (_collisionFlags != 0) {
-        Camera* myCamera = Application::getInstance()->getCamera();
-
-        float radius = getSkeletonHeight() * COLLISION_RADIUS_SCALE;
-        if (myCamera->getMode() == CAMERA_MODE_FIRST_PERSON && !OculusManager::isConnected()) {
-            radius = myCamera->getAspectRatio() * (myCamera->getNearClip() / cos(myCamera->getFieldOfView() / 2.f));
-            radius *= COLLISION_RADIUS_SCALAR;
-        }
-
-        if (_collisionFlags & COLLISION_GROUP_ENVIRONMENT) {
-            updateCollisionWithEnvironment(deltaTime, radius);
-        }
-        if (_collisionFlags & COLLISION_GROUP_VOXELS) {
-            updateCollisionWithVoxels(deltaTime, radius);
-        }
-        if (_collisionFlags & COLLISION_GROUP_AVATARS) {
-            updateCollisionWithAvatars(deltaTime);
-        }
-    }
-
     // add thrust to velocity
     _velocity += _thrust * deltaTime;
 
@@ -318,6 +298,30 @@ void MyAvatar::simulate(float deltaTime) {
 
     // Zero thrust out now that we've added it to velocity in this frame
     _thrust = glm::vec3(0, 0, 0);
+
+    // now that we're done stepping the avatar forward in time, compute new collisions
+    if (_collisionFlags != 0) {
+        Camera* myCamera = Application::getInstance()->getCamera();
+
+        float radius = getSkeletonHeight() * COLLISION_RADIUS_SCALE;
+        if (myCamera->getMode() == CAMERA_MODE_FIRST_PERSON && !OculusManager::isConnected()) {
+            radius = myCamera->getAspectRatio() * (myCamera->getNearClip() / cos(myCamera->getFieldOfView() / 2.f));
+            radius *= COLLISION_RADIUS_SCALAR;
+        }
+
+        if (_collisionFlags & COLLISION_GROUP_ENVIRONMENT) {
+            updateCollisionWithEnvironment(deltaTime, radius);
+        }
+        if (_collisionFlags & COLLISION_GROUP_VOXELS) {
+            updateCollisionWithVoxels(deltaTime, radius);
+        }
+        if (_collisionFlags & COLLISION_GROUP_AVATARS) {
+            updateCollisionWithAvatars(deltaTime);
+        }
+    }
+
+    // resolve collision results
+    _skeletonModel.syncToPalms();
     
     // consider updating our billboard
     maybeUpdateBillboard();
@@ -919,14 +923,17 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
         // no need to compute a bunch of stuff if we have one or fewer avatars
         return;
     }
+    updateShapePositions();
     float myBoundingRadius = getBoundingRadius();
 
+    /* TODO: Andrew to fix Avatar-Avatar body collisions
     // HACK: body-body collision uses two coaxial capsules with axes parallel to y-axis
     // TODO: make the collision work without assuming avatar orientation
     Extents myStaticExtents = _skeletonModel.getStaticExtents();
     glm::vec3 staticScale = myStaticExtents.maximum - myStaticExtents.minimum;
     float myCapsuleRadius = 0.25f * (staticScale.x + staticScale.z);
     float myCapsuleHeight = staticScale.y;
+    */
 
     CollisionInfo collisionInfo;
     foreach (const AvatarSharedPointer& avatarPointer, avatars) {
@@ -935,16 +942,13 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
             // don't collide with ourselves
             continue;
         }
+        avatar->updateShapePositions();
         float distance = glm::length(_position - avatar->getPosition());        
         if (_distanceToNearestAvatar > distance) {
             _distanceToNearestAvatar = distance;
         }
         float theirBoundingRadius = avatar->getBoundingRadius();
         if (distance < myBoundingRadius + theirBoundingRadius) {
-            _skeletonModel.updateShapePositions();
-            Model& headModel = getHead()->getFaceModel();
-            headModel.updateShapePositions();
-
             /* TODO: Andrew to fix Avatar-Avatar body collisions
             Extents theirStaticExtents = _skeletonModel.getStaticExtents();
             glm::vec3 staticScale = theirStaticExtents.maximum - theirStaticExtents.minimum;
diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 7c7202566a..cf542a41d5 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -62,6 +62,19 @@ void SkeletonModel::simulate(float deltaTime, bool delayLoad) {
     }
 }
 
+void SkeletonModel::syncToPalms() {
+    int leftPalmIndex, rightPalmIndex;
+    Hand* hand = _owningAvatar->getHand();
+    hand->getLeftRightPalmIndices(leftPalmIndex, rightPalmIndex);
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    if (leftPalmIndex != -1 && rightPalmIndex != -1) {
+        applyPalmData(geometry.leftHandJointIndex, geometry.leftFingerJointIndices, geometry.leftFingertipJointIndices,
+            hand->getPalms()[leftPalmIndex]);
+        applyPalmData(geometry.rightHandJointIndex, geometry.rightFingerJointIndices, geometry.rightFingertipJointIndices,
+            hand->getPalms()[rightPalmIndex]);
+    }
+}
+
 bool SkeletonModel::render(float alpha) {
 
     if (_jointStates.isEmpty()) {
@@ -154,7 +167,7 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
 
     // no point in continuing if there are no fingers
     if (palm.getNumFingers() == 0 || fingerJointIndices.isEmpty()) {
-        stretchArm(jointIndex, palm.getPosition());
+        stretchArm(jointIndex, palm);
         return;
     }
 
@@ -173,7 +186,7 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
         setJointRotation(fingerJointIndex, rotationBetween(palmRotation * jointVector, fingerVector) * palmRotation, true);
     }
     
-    stretchArm(jointIndex, palm.getPosition());
+    stretchArm(jointIndex, palm);
 }
 
 void SkeletonModel::updateJointState(int index) {
@@ -196,7 +209,8 @@ void SkeletonModel::maybeUpdateLeanRotation(const JointState& parentState, const
         glm::angleAxis(-_owningAvatar->getHead()->getLeanForward(), glm::normalize(inverse * axes[0])) * joint.rotation;
 }
 
-void SkeletonModel::stretchArm(int jointIndex, const glm::vec3& position) {
+void SkeletonModel::stretchArm(int jointIndex, PalmData& palm) {
+//        const glm::vec3& position) {
     // find out where the hand is pointing
     glm::quat handRotation;
     getJointRotation(jointIndex, handRotation, true);
@@ -212,11 +226,11 @@ void SkeletonModel::stretchArm(int jointIndex, const glm::vec3& position) {
     glm::quat elbowRotation;
     getJointRotation(joint.parentIndex, elbowRotation, true);
     applyRotationDelta(joint.parentIndex, rotationBetween(elbowRotation * forwardVector, handVector), false);
-    
+
     // set position according to normal length
     float scale = extractUniformScale(_scale);
-    glm::vec3 handPosition = position - _translation;
-    glm::vec3 elbowPosition = handPosition - handVector * joint.distanceToParent * scale;
+    palm.resolvePenetrations();
+    glm::vec3 elbowPosition = palm.getPosition() - _translation; - handVector * (joint.distanceToParent * scale);
     
     // set shoulder orientation to point to elbow
     const FBXJoint& parentJoint = geometry.joints.at(joint.parentIndex);
diff --git a/interface/src/avatar/SkeletonModel.h b/interface/src/avatar/SkeletonModel.h
index 3d95d805ea..1c6091a5cb 100644
--- a/interface/src/avatar/SkeletonModel.h
+++ b/interface/src/avatar/SkeletonModel.h
@@ -23,12 +23,13 @@ public:
     SkeletonModel(Avatar* owningAvatar);
     
     void simulate(float deltaTime, bool delayLoad = false);
+    void syncToPalms();
     bool render(float alpha);
 
     /// \param jointIndex index of hand joint
     /// \param shapes[out] list in which is stored pointers to hand shapes
     void getHandShapes(int jointIndex, QVector<const Shape*>& shapes) const;
-    
+
 protected:
     
     void applyHandPosition(int jointIndex, const glm::vec3& position);
@@ -45,7 +46,7 @@ private:
     
     /// Using the current position and rotation of the identified (hand) joint, computes a
     /// reasonable stretched configuration for the connected arm.
-    void stretchArm(int jointIndex, const glm::vec3& position);
+    void stretchArm(int jointIndex, PalmData& palm);
     
     Avatar* _owningAvatar;
 };

From 817ae8e0525983e727b6643af21d1aa5b5af293d Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Mon, 10 Mar 2014 08:38:17 -0700
Subject: [PATCH 05/13] degrees instead of radians for head lean deltas

---
 interface/src/avatar/Head.cpp     | 4 ++--
 libraries/shared/src/SharedUtil.h | 1 +
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/interface/src/avatar/Head.cpp b/interface/src/avatar/Head.cpp
index 7e9d549509..1d1af08645 100644
--- a/interface/src/avatar/Head.cpp
+++ b/interface/src/avatar/Head.cpp
@@ -226,8 +226,8 @@ void Head::applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetr
             if (neckLength > 0.f) {
                 glm::vec3 xAxis = bodyRotation * glm::vec3(1.f, 0.f, 0.f);
                 glm::vec3 zAxis = bodyRotation * glm::vec3(0.f, 0.f, 1.f);
-                float forward = glm::dot(penetration, zAxis) / neckLength;
-                float sideways = - glm::dot(penetration, xAxis) / neckLength;
+                float forward = DEGREES_PER_RADIAN * glm::dot(penetration, zAxis) / neckLength;
+                float sideways = DEGREES_PER_RADIAN * glm::dot(penetration, xAxis) / neckLength;
                 addLean(sideways, forward);
             }
         }
diff --git a/libraries/shared/src/SharedUtil.h b/libraries/shared/src/SharedUtil.h
index b08e0413a6..0c14dbec91 100644
--- a/libraries/shared/src/SharedUtil.h
+++ b/libraries/shared/src/SharedUtil.h
@@ -50,6 +50,7 @@ static const float ONE_THIRD        = 0.333333f;
 static const float PIE              = 3.141592f;
 static const float PI_TIMES_TWO		= 3.141592f * 2.0f;
 static const float PI_OVER_180      = 3.141592f / 180.0f;
+static const float DEGREES_PER_RADIAN = 180.0f / PIE;
 static const float EPSILON          = 0.000001f;	//smallish positive number - used as margin of error for some computations
 static const float SQUARE_ROOT_OF_2 = (float)sqrt(2.f);
 static const float SQUARE_ROOT_OF_3 = (float)sqrt(3.f);

From 5e494d522d0c9852c15e90106fb57caa0fe9d0cb Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Wed, 12 Mar 2014 15:49:17 -0700
Subject: [PATCH 06/13] fixing signed/unsigned comparison

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

diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index 830e997c80..13bbf23045 100644
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -196,7 +196,7 @@ void Hand::collideAgainstOurself() {
     float scaledPalmRadius = PALM_COLLISION_RADIUS * _owningAvatar->getScale();
     
     const Model& skeletonModel = _owningAvatar->getSkeletonModel();
-    for (size_t i = 0; i < getNumPalms(); i++) {
+    for (int i = 0; i < int(getNumPalms()); i++) {
         PalmData& palm = getPalms()[i];
         if (!palm.isActive()) {
             continue;

From cd7da205de5ee91da3a0e95f644edb551297ca07 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Wed, 12 Mar 2014 15:49:46 -0700
Subject: [PATCH 07/13] improved index sanity checking

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

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index f58391601c..5cd4e368d0 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -91,7 +91,7 @@ bool SkeletonModel::render(float alpha) {
 }
 
 void SkeletonModel::getHandShapes(int jointIndex, QVector<const Shape*>& shapes) const {
-    if (jointIndex == -1) {
+    if (jointIndex < 0 || jointIndex >= int(_shapes.size())) {
         return;
     }
     if (jointIndex == getLeftHandJointIndex()

From 5934a17475176c547713098e87cbd024ef578303 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 25 Mar 2014 15:27:28 -0700
Subject: [PATCH 08/13] fix sphere-capsule collisions at the capsule caps

---
 libraries/shared/src/ShapeCollider.cpp | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/libraries/shared/src/ShapeCollider.cpp b/libraries/shared/src/ShapeCollider.cpp
index e83e6842a9..70f9b9c4bf 100644
--- a/libraries/shared/src/ShapeCollider.cpp
+++ b/libraries/shared/src/ShapeCollider.cpp
@@ -86,7 +86,8 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, Col
     if (absAxialDistance < totalRadius + capsuleB->getHalfHeight()) {
         glm::vec3 radialAxis = BA + axialDistance * capsuleAxis; // points from A to axis of B
         float radialDistance2 = glm::length2(radialAxis);
-        if (radialDistance2 > totalRadius * totalRadius) {
+        float totalRadius2 = totalRadius * totalRadius;
+        if (radialDistance2 > totalRadius2) {
             // sphere is too far from capsule axis
             return false;
         }
@@ -95,6 +96,9 @@ bool sphereCapsule(const SphereShape* sphereA, const CapsuleShape* capsuleB, Col
             float sign = (axialDistance > 0.f) ? 1.f : -1.f;
             radialAxis = BA + (sign * capsuleB->getHalfHeight()) * capsuleAxis;
             radialDistance2 = glm::length2(radialAxis);
+            if (radialDistance2 > totalRadius2) {
+                return false;
+            }
         }
         if (radialDistance2 > EPSILON * EPSILON) {
             CollisionInfo* collision = collisions.getNewCollision();
@@ -147,7 +151,8 @@ bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, Col
     if (absAxialDistance < totalRadius + capsuleA->getHalfHeight()) {
         glm::vec3 radialAxis = AB + axialDistance * capsuleAxis; // from sphereB to axis of capsuleA
         float radialDistance2 = glm::length2(radialAxis);
-        if (radialDistance2 > totalRadius * totalRadius) {
+        float totalRadius2 = totalRadius * totalRadius;
+        if (radialDistance2 > totalRadius2) {
             // sphere is too far from capsule axis
             return false;
         }
@@ -162,6 +167,9 @@ bool capsuleSphere(const CapsuleShape* capsuleA, const SphereShape* sphereB, Col
             closestApproach = capsuleA->getPosition() + (sign * capsuleA->getHalfHeight()) * capsuleAxis;
             radialAxis = closestApproach - sphereB->getPosition();
             radialDistance2 = glm::length2(radialAxis);
+            if (radialDistance2 > totalRadius2) {
+                return false;
+            }
         }
         if (radialDistance2 > EPSILON * EPSILON) {
             CollisionInfo* collision = collisions.getNewCollision();

From 06fbd49ffabb4b0954e57b3dfc0a7ee658c872ef Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 25 Mar 2014 15:29:22 -0700
Subject: [PATCH 09/13] Adding StreamUtils for common debug output

---
 libraries/shared/src/StreamUtils.cpp     | 75 ++++++++++++++++++++++++
 libraries/shared/src/StreamUtils.h       | 39 ++++++++++++
 tests/physics/src/CollisionInfoTests.cpp |  7 ++-
 tests/physics/src/ShapeColliderTests.cpp |  5 +-
 4 files changed, 124 insertions(+), 2 deletions(-)
 create mode 100644 libraries/shared/src/StreamUtils.cpp
 create mode 100644 libraries/shared/src/StreamUtils.h

diff --git a/libraries/shared/src/StreamUtils.cpp b/libraries/shared/src/StreamUtils.cpp
new file mode 100644
index 0000000000..44c81f9464
--- /dev/null
+++ b/libraries/shared/src/StreamUtils.cpp
@@ -0,0 +1,75 @@
+//
+//  StreamUtils.cpp
+//
+//  Created by Andrew Meadows on 2014.02.21
+//  Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
+//
+
+#include <glm/gtc/type_ptr.hpp>
+
+#include "StreamUtils.h"
+
+const char* hex_digits = "0123456789abcdef";
+
+void StreamUtil::dump(std::ostream& s, const QByteArray& buffer) {
+    int row_size = 32;
+    int i = 0;
+    while (i < buffer.size()) {
+        for(int j = 0; i < buffer.size() && j < row_size; ++j) {
+            char byte = buffer[i];
+            s << hex_digits[(byte >> 4) & 0x0f] << hex_digits[byte & 0x0f] << " ";
+            ++i;
+        }
+        s << "\n";
+    }
+}
+
+std::ostream& operator<<(std::ostream& s, const glm::vec3& v) {
+    s << "<" << v.x << " " << v.y << " " << v.z << ">";
+    return s;
+}
+
+std::ostream& operator<<(std::ostream& s, const glm::quat& q) {
+    s << "<" << q.x << " " << q.y << " " << q.z << " " << q.w << ">";
+    return s;
+}
+
+std::ostream& operator<<(std::ostream& s, const glm::mat4& m) {
+    s << "[";
+    for (int j = 0; j < 4; ++j) {
+        s << " " << m[0][j] << " " << m[1][j] << " " << m[2][j] << " " << m[3][j] << ";";
+    }
+    s << "  ]";
+    return s;
+}
+
+// less common utils can be enabled with DEBUG
+#ifdef DEBUG
+
+std::ostream& operator<<(std::ostream& s, const CollisionInfo& c) {
+    s << "{penetration=" << c._penetration 
+        << ", contactPoint=" << c._contactPoint
+        << ", addedVelocity=" << c._addedVelocity
+        << "}";
+    return s;
+}
+
+std::ostream& operator<<(std::ostream& s, const SphereShape& sphere) {
+    s << "{type='sphere', center=" << sphere.getPosition()
+        << ", radius=" << sphere.getRadius()
+        << "}";
+    return s;
+}
+
+std::ostream& operator<<(std::ostream& s, const CapsuleShape& capsule) {
+    s << "{type='capsule', center=" << capsule.getPosition()
+        << ", radius=" << capsule.getRadius()
+        << ", length=" << (2.f * capsule.getHalfHeight())
+        << ", begin=" << capsule.getStartPoint()
+        << ", end=" << capsule.getEndPoint()
+        << "}";
+    return s;
+}
+
+#endif // DEBUG
+
diff --git a/libraries/shared/src/StreamUtils.h b/libraries/shared/src/StreamUtils.h
new file mode 100644
index 0000000000..1a17a94658
--- /dev/null
+++ b/libraries/shared/src/StreamUtils.h
@@ -0,0 +1,39 @@
+//
+//  StreamUtils.h
+//
+//  Created by Andrew Meadows on 2014.02.21
+//  Copyright (c) 2014 High Fidelity, Inc. All rights reserved.
+//
+
+#ifndef __tests__StreamUtils__
+#define __tests__StreamUtils__
+
+#include <iostream>
+
+#include <QByteArray>
+
+#include <glm/glm.hpp>
+#include <glm/gtx/quaternion.hpp>
+
+
+namespace StreamUtil {
+    // dump the buffer, 32 bytes per row, each byte in hex, separated by whitespace
+    void dump(std::ostream& s, const QByteArray& buffer);
+}
+
+std::ostream& operator<<(std::ostream& s, const glm::vec3& v);
+std::ostream& operator<<(std::ostream& s, const glm::quat& q);
+std::ostream& operator<<(std::ostream& s, const glm::mat4& m);
+
+// less common utils can be enabled with DEBUG
+#ifdef DEBUG
+#include "CollisionInfo.h"
+#include "SphereShape.h"
+#include "CapsuleShape.h"
+std::ostream& operator<<(std::ostream& s, const CollisionInfo& c);
+std::ostream& operator<<(std::ostream& s, const SphereShape& shape);
+std::ostream& operator<<(std::ostream& s, const CapsuleShape& capsule);
+#endif // DEBUG
+
+
+#endif // __tests__StreamUtils__
diff --git a/tests/physics/src/CollisionInfoTests.cpp b/tests/physics/src/CollisionInfoTests.cpp
index 43601978ae..241204d036 100644
--- a/tests/physics/src/CollisionInfoTests.cpp
+++ b/tests/physics/src/CollisionInfoTests.cpp
@@ -13,12 +13,17 @@
 
 #include <CollisionInfo.h>
 #include <SharedUtil.h>
+#include <StreamUtils.h>
 
 #include "CollisionInfoTests.h"
-#include "PhysicsTestUtil.h"
 
 
 /*
+
+static glm::vec3 xAxis(1.f, 0.f, 0.f);
+static glm::vec3 xZxis(0.f, 1.f, 0.f);
+static glm::vec3 xYxis(0.f, 0.f, 1.f);
+
 void CollisionInfoTests::rotateThenTranslate() {
     CollisionInfo collision;
     collision._penetration = xAxis;
diff --git a/tests/physics/src/ShapeColliderTests.cpp b/tests/physics/src/ShapeColliderTests.cpp
index 6d7e9a6db1..4a2e648323 100644
--- a/tests/physics/src/ShapeColliderTests.cpp
+++ b/tests/physics/src/ShapeColliderTests.cpp
@@ -16,11 +16,14 @@
 #include <ShapeCollider.h>
 #include <SharedUtil.h>
 #include <SphereShape.h>
+#include <StreamUtils.h>
 
-#include "PhysicsTestUtil.h"
 #include "ShapeColliderTests.h"
 
 const glm::vec3 origin(0.f);
+static const glm::vec3 xAxis(1.f, 0.f, 0.f);
+static const glm::vec3 yAxis(0.f, 1.f, 0.f);
+static const glm::vec3 zAxis(0.f, 0.f, 1.f);
 
 void ShapeColliderTests::sphereMissesSphere() {
     // non-overlapping spheres of unequal size

From d90b325b1c4208ade3adcee7777044aba082b4d0 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 25 Mar 2014 15:31:46 -0700
Subject: [PATCH 10/13] Add penetration resolution helper for Hand class

---
 interface/src/avatar/Hand.cpp     | 8 ++++++++
 interface/src/avatar/Hand.h       | 2 ++
 interface/src/avatar/MyAvatar.cpp | 4 +++-
 3 files changed, 13 insertions(+), 1 deletion(-)

diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index 61eecae696..65dce3bda2 100644
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -9,6 +9,7 @@
 #include <NodeList.h>
 
 #include <GeometryUtil.h>
+#include <StreamUtils.h>
 
 #include "Application.h"
 #include "Avatar.h"
@@ -219,6 +220,13 @@ void Hand::collideAgainstOurself() {
     }
 }
 
+void Hand::resolvePenetrations() {
+    for (size_t i = 0; i < getNumPalms(); ++i) {
+        PalmData& palm = getPalms()[i];
+        palm.resolvePenetrations();
+    }
+}
+
 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;
diff --git a/interface/src/avatar/Hand.h b/interface/src/avatar/Hand.h
index 6f64f62c9c..93a40707cc 100755
--- a/interface/src/avatar/Hand.h
+++ b/interface/src/avatar/Hand.h
@@ -60,6 +60,8 @@ public:
     void collideAgainstAvatar(Avatar* avatar, bool isMyHand);
     void collideAgainstOurself();
 
+    void resolvePenetrations();
+
 private:
     // disallow copies of the Hand, copy of owning Avatar is disallowed too
     Hand(const Hand&);
diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 961ab5b431..c1d05d9e7a 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -895,7 +895,6 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
     float myCapsuleHeight = staticScale.y;
     */
 
-    CollisionInfo collisionInfo;
     foreach (const AvatarSharedPointer& avatarPointer, avatars) {
         Avatar* avatar = static_cast<Avatar*>(avatarPointer.data());
         if (static_cast<Avatar*>(this) == avatar) {
@@ -926,10 +925,13 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
             // collide our hands against them
             getHand()->collideAgainstAvatar(avatar, true);
 
+            /* TODO: Andrew to fix them-into-us collision checks
             // collide their hands against us
             avatar->getHand()->collideAgainstAvatar(this, false);
+            */
         }
     }
+    getHand()->resolvePenetrations();
 }
 
 class SortedAvatar {

From 9a4db152ffa42532b76b9d5794a3de2b44a9e422 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Wed, 26 Mar 2014 13:20:18 -0700
Subject: [PATCH 11/13] avatar responds to hand-vs-head collisions again

---
 interface/src/avatar/Avatar.cpp        | 18 ++++++++++++--
 interface/src/avatar/Hand.cpp          | 31 ++++++-----------------
 interface/src/avatar/Hand.h            |  1 +
 interface/src/avatar/MyAvatar.cpp      | 12 ++++-----
 interface/src/avatar/SkeletonModel.cpp | 34 ++++++++++++++------------
 interface/src/avatar/SkeletonModel.h   |  1 -
 6 files changed, 48 insertions(+), 49 deletions(-)

diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp
index 718fcb824e..de9b33d9c7 100644
--- a/interface/src/avatar/Avatar.cpp
+++ b/interface/src/avatar/Avatar.cpp
@@ -759,8 +759,22 @@ bool Avatar::collisionWouldMoveAvatar(CollisionInfo& collision) const {
 }
 
 void Avatar::applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration) {
-    // ATM we only support collision with head
-    getHead()->applyCollision(contactPoint, penetration);
+    // compute lean angles
+    glm::vec3 leverAxis = contactPoint - getPosition();
+    float leverLength = glm::length(leverAxis);
+    if (leverLength > EPSILON) {
+        glm::quat bodyRotation = getOrientation();
+        glm::vec3 xAxis = bodyRotation * glm::vec3(1.f, 0.f, 0.f);
+        glm::vec3 zAxis = bodyRotation * glm::vec3(0.f, 0.f, 1.f);
+
+        leverAxis = leverAxis / leverLength;
+        glm::vec3 effectivePenetration = penetration - glm::dot(penetration, leverAxis) * leverAxis;
+        // we use the small-angle approximation for sine below to compute the length of 
+        // the opposite side of a narrow right triangle
+        float sideways = - glm::dot(effectivePenetration, xAxis) / leverLength;
+        float forward = glm::dot(effectivePenetration, zAxis) / leverLength;
+        getHead()->addLean(sideways, forward);
+    }
 }
 
 float Avatar::getBoundingRadius() const {
diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index 531ad0e8f2..750fae7b2a 100644
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -110,8 +110,6 @@ void Hand::collideAgainstAvatarOld(Avatar* avatar, bool isMyHand) {
             for (int j = 0; j < handCollisions.size(); ++j) {
                 CollisionInfo* collision = handCollisions.getCollision(j);
                 if (isMyHand) {
-                    // we resolve the hand from collision when it belongs to MyAvatar AND the other Avatar is 
-                    // not expected to respond to the collision (hand hit unmovable part of their Avatar)
                     totalPenetration = addPenetrations(totalPenetration, collision->_penetration);
                 }
             }
@@ -129,54 +127,39 @@ void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) {
         return;
     }
 
-    // 2 = NUM_HANDS
-    int palmIndices[2];
-    getLeftRightPalmIndices(*palmIndices, *(palmIndices + 1));
-
     const SkeletonModel& skeletonModel = _owningAvatar->getSkeletonModel();
     int jointIndices[2];
     jointIndices[0] = skeletonModel.getLeftHandJointIndex();
     jointIndices[1] = skeletonModel.getRightHandJointIndex();
 
     for (size_t i = 0; i < 2; i++) {
-        int palmIndex = palmIndices[i];
         int jointIndex = jointIndices[i];
-        if (palmIndex == -1 || jointIndex == -1) {
+        if (jointIndex < 0) {
             continue;
         }
-        PalmData& palm = _palms[palmIndex];
-        if (!palm.isActive()) {
-            continue;
-        }
-        if (isMyHand && Menu::getInstance()->isOptionChecked(MenuOption::PlaySlaps)) {
-            playSlaps(palm, avatar);
-        }
 
         handCollisions.clear();
         QVector<const Shape*> shapes;
         skeletonModel.getHandShapes(jointIndex, shapes);
 
         if (avatar->findCollisions(shapes, handCollisions)) {
-            glm::vec3 averagePenetration;
+            glm::vec3 totalPenetration(0.f);
             glm::vec3 averageContactPoint;
             for (int j = 0; j < handCollisions.size(); ++j) {
                 CollisionInfo* collision = handCollisions.getCollision(j);
-                averagePenetration += collision->_penetration;
+                totalPenetration += collision->_penetration;
                 averageContactPoint += collision->_contactPoint;
             }
-            averagePenetration /= float(handCollisions.size());
             if (isMyHand) {
                 // our hand against other avatar 
-                // for now we resolve it to test shapes/collisions
-                // TODO: only partially resolve this penetration
-                palm.addToPenetration(averagePenetration);
+                // TODO: resolve this penetration when we don't think the other avatar will yield
+                //palm.addToPenetration(averagePenetration);
             } else {
                 // someone else's hand against MyAvatar
-                // TODO: submit collision info to MyAvatar which should lean accordingly
                 averageContactPoint /= float(handCollisions.size());
-                avatar->applyCollision(averageContactPoint, averagePenetration);
+                avatar->applyCollision(averageContactPoint, totalPenetration);
             }
-        } 
+        }
     }
 }
 
diff --git a/interface/src/avatar/Hand.h b/interface/src/avatar/Hand.h
index 427c33e9c3..3be3bc0f77 100755
--- a/interface/src/avatar/Hand.h
+++ b/interface/src/avatar/Hand.h
@@ -82,3 +82,4 @@ private:
 };
 
 #endif
+
diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 14e948835d..5fc9f874aa 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -322,9 +322,6 @@ void MyAvatar::simulate(float deltaTime) {
         }
     }
 
-    // resolve collision results
-    _skeletonModel.syncToPalms();
-    
     // consider updating our billboard
     maybeUpdateBillboard();
 }
@@ -925,15 +922,16 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
             */
 
             // collide our hands against them
-            getHand()->collideAgainstAvatar(avatar, true);
+            // TODO: make this work when we can figure out when the other avatar won't yeild
+            // (for example, we're colling against their chest or leg)
+            //getHand()->collideAgainstAvatar(avatar, true);
 
-            /* TODO: Andrew to fix them-into-us collision checks
             // collide their hands against us
             avatar->getHand()->collideAgainstAvatar(this, false);
-            */
         }
     }
-    getHand()->resolvePenetrations();
+    // TODO: uncomment this when we handle collisions that won't affect other avatar
+    //getHand()->resolvePenetrations();
 }
 
 class SortedAvatar {
diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 510b37782e..b4746a39d2 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -62,27 +62,31 @@ void SkeletonModel::simulate(float deltaTime, bool fullUpdate) {
     }
 }
 
-void SkeletonModel::syncToPalms() {
-    int leftPalmIndex, rightPalmIndex;
-    Hand* hand = _owningAvatar->getHand();
-    hand->getLeftRightPalmIndices(leftPalmIndex, rightPalmIndex);
-    const FBXGeometry& geometry = _geometry->getFBXGeometry();
-    if (leftPalmIndex != -1 && rightPalmIndex != -1) {
-        applyPalmData(geometry.leftHandJointIndex, geometry.leftFingerJointIndices, geometry.leftFingertipJointIndices,
-            hand->getPalms()[leftPalmIndex]);
-        applyPalmData(geometry.rightHandJointIndex, geometry.rightFingerJointIndices, geometry.rightFingertipJointIndices,
-            hand->getPalms()[rightPalmIndex]);
-    }
-}
-
 void SkeletonModel::getHandShapes(int jointIndex, QVector<const Shape*>& shapes) const {
     if (jointIndex < 0 || jointIndex >= int(_shapes.size())) {
         return;
     }
     if (jointIndex == getLeftHandJointIndex()
         || jointIndex == getRightHandJointIndex()) {
-        // TODO: also add fingers and other hand-parts
-        shapes.push_back(_shapes[jointIndex]);
+        // get all shapes that have this hand as an ancestor in the skeleton heirarchy
+        const FBXGeometry& geometry = _geometry->getFBXGeometry();
+        for (int i = 0; i < _jointStates.size(); i++) {
+            const FBXJoint& joint = geometry.joints[i];
+            if (i == jointIndex) {
+                // this shape is the hand
+                shapes.push_back(_shapes[i]);
+            } else {
+                int parentIndex = joint.parentIndex;
+                while (parentIndex != -1) {
+                    if (parentIndex == jointIndex) {
+                        // this shape is a child of the hand
+                        shapes.push_back(_shapes[i]);
+                        break;
+                    }
+                    parentIndex = geometry.joints[parentIndex].parentIndex;
+                }
+            }
+        }
     }
 }
 
diff --git a/interface/src/avatar/SkeletonModel.h b/interface/src/avatar/SkeletonModel.h
index 0c8ff72f5a..213a53d9ed 100644
--- a/interface/src/avatar/SkeletonModel.h
+++ b/interface/src/avatar/SkeletonModel.h
@@ -23,7 +23,6 @@ public:
     SkeletonModel(Avatar* owningAvatar);
     
     void simulate(float deltaTime, bool fullUpdate = true);
-    void syncToPalms();
 
     /// \param jointIndex index of hand joint
     /// \param shapes[out] list in which is stored pointers to hand shapes

From 7a649084bd5364a9cc950466f66d8e56fcb9abd9 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Wed, 26 Mar 2014 14:07:21 -0700
Subject: [PATCH 12/13] reduce the lean recovery timescale

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

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 5fc9f874aa..618cda1199 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -362,7 +362,7 @@ void MyAvatar::updateFromGyros(float deltaTime) {
         }
     } else {
         // restore rotation, lean to neutral positions
-        const float RESTORE_PERIOD = 1.f;   // seconds
+        const float RESTORE_PERIOD = 0.25f;   // seconds
         float restorePercentage = glm::clamp(deltaTime/RESTORE_PERIOD, 0.f, 1.f);
         head->setPitchTweak(glm::mix(head->getPitchTweak(), 0.0f, restorePercentage));
         head->setYawTweak(glm::mix(head->getYawTweak(), 0.0f, restorePercentage));

From 63c4af895049dac5bb652f6c36833271373b3d6b Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Wed, 26 Mar 2014 14:10:43 -0700
Subject: [PATCH 13/13] remove cruft: Head::applyCollision()

---
 interface/src/avatar/Head.cpp | 21 ---------------------
 interface/src/avatar/Head.h   |  3 ---
 2 files changed, 24 deletions(-)

diff --git a/interface/src/avatar/Head.cpp b/interface/src/avatar/Head.cpp
index b844c08e9a..b1265a1e51 100644
--- a/interface/src/avatar/Head.cpp
+++ b/interface/src/avatar/Head.cpp
@@ -212,27 +212,6 @@ float Head::getTweakedRoll() const {
     return glm::clamp(_roll + _rollTweak, MIN_HEAD_ROLL, MAX_HEAD_ROLL);
 }
 
-void Head::applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration) {
-    // compute lean angles
-    Avatar* owningAvatar = static_cast<Avatar*>(_owningAvatar);
-    glm::quat bodyRotation = owningAvatar->getOrientation();
-    glm::vec3 neckPosition;
-    if (owningAvatar->getSkeletonModel().getNeckPosition(neckPosition)) {
-        glm::vec3 yAxis = bodyRotation * glm::vec3(0.f, 1.f, 0.f);
-        glm::vec3 leverArm = _position - neckPosition;
-        if (glm::dot(leverArm, yAxis) > 0.f) {
-            float neckLength = glm::length(_position - neckPosition);
-            if (neckLength > 0.f) {
-                glm::vec3 xAxis = bodyRotation * glm::vec3(1.f, 0.f, 0.f);
-                glm::vec3 zAxis = bodyRotation * glm::vec3(0.f, 0.f, 1.f);
-                float forward = DEGREES_PER_RADIAN * glm::dot(penetration, zAxis) / neckLength;
-                float sideways = DEGREES_PER_RADIAN * glm::dot(penetration, xAxis) / neckLength;
-                addLean(sideways, forward);
-            }
-        }
-    }
-}
-
 void Head::renderLookatVectors(glm::vec3 leftEyePosition, glm::vec3 rightEyePosition, glm::vec3 lookatPosition) {
 
     Application::getInstance()->getGlowEffect()->begin();
diff --git a/interface/src/avatar/Head.h b/interface/src/avatar/Head.h
index ef38ae3ed0..733323abc5 100644
--- a/interface/src/avatar/Head.h
+++ b/interface/src/avatar/Head.h
@@ -82,9 +82,6 @@ public:
     virtual float getTweakedPitch() const;
     virtual float getTweakedYaw() const;
     virtual float getTweakedRoll() const;
-
-    // move the head to avoid given collision info
-    void applyCollision(const glm::vec3& contactPoint, const glm::vec3& penetration);
     
 private:
     // disallow copies of the Head, copy of owning Avatar is disallowed too