avatar responds to hand-vs-head collisions again

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 {

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);
             }
-        } 
+        }
     }
 }
 

interface/src/avatar/Hand.h

@@ -82,3 +82,4 @@ private:
 };
 
 #endif
+

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 {

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;
+                }
+            }
+        }
     }
 }
 

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