It is now possible to "poke" moveable parts of the avatar skeleton.

interface/src/avatar/Avatar.cpp

@@ -276,16 +276,12 @@ bool Avatar::findSphereCollisions(const glm::vec3& penetratorCenter, float penet
     bool didPenetrate = false;
     glm::vec3 skeletonPenetration;
     ModelCollisionInfo collisionInfo;
-    int jointIndex = _skeletonModel.findSphereCollision(penetratorCenter, penetratorRadius, 
-            collisionInfo, 1.0f, skeletonSkipIndex);
-    if (jointIndex != -1) {
+    if (_skeletonModel.findSphereCollision(penetratorCenter, penetratorRadius, collisionInfo, 1.0f, skeletonSkipIndex)) {
         collisionInfo._model = &_skeletonModel;
         collisions.push_back(collisionInfo);
         didPenetrate = true; 
     }
-    glm::vec3 facePenetration;
-    jointIndex = _head.getFaceModel().findSphereCollision(penetratorCenter, penetratorRadius, collisionInfo);
-    if (jointIndex != -1) {
+    if (_head.getFaceModel().findSphereCollision(penetratorCenter, penetratorRadius, collisionInfo)) {
         collisionInfo._model = &(_head.getFaceModel());
         collisions.push_back(collisionInfo);
         didPenetrate = true; 
@@ -447,18 +443,13 @@ float Avatar::getHeight() const {
     return extents.maximum.y - extents.minimum.y;
 }
 
-void Avatar::poke(ModelCollisionInfo& collision) {
-    if (collision._model == &_skeletonModel
-        && collision._jointIndex != -1) {
-        // TODO: Andrew to make this work
-        printf("ADEBUG model = 0x%x  joint = %d  p = [%e, %e, %e]\n",
-                collision._model,
-                collision._jointIndex,
-                collision._contactPoint.x,
-                collision._contactPoint.y,
-                collision._contactPoint.z
-                );
+bool Avatar::poke(ModelCollisionInfo& collision) {
+    // ATM poke() can only affect the Skeleton (not the head)
+    // TODO: make poke affect head
+    if (collision._model == &_skeletonModel && collision._jointIndex != -1) {
+        return _skeletonModel.poke(collision);
     }
+    return false;
 }
 
 float Avatar::getPelvisFloatingHeight() const {

interface/src/avatar/Avatar.h

@@ -128,7 +128,9 @@ public:
 
     float getHeight() const;
 
-    void poke(ModelCollisionInfo& collision);
+    /// \param collision a data structure for storing info about collisions against Models
+    /// \return true if the collision affects the Avatar models
+    bool poke(ModelCollisionInfo& collision);
 
 public slots:
     void updateCollisionFlags();

interface/src/avatar/Hand.cpp

@@ -224,9 +224,9 @@ void Hand::updateCollisions() {
                 }
                 if (avatar->findSphereCollisions(palm.getPosition(), scaledPalmRadius, collisions)) {
                     for (size_t j = 0; j < collisions.size(); ++j) {
-                        totalPenetration = addPenetrations(totalPenetration, collisions[j]._penetration);
-                        // TODO: Andrew to poke avatar using collision info
-                        avatar->poke(collisions[j]);
+                        if (!avatar->poke(collisions[j])) {
+                            totalPenetration = addPenetrations(totalPenetration, collisions[j]._penetration);
+                        }
                     }
                 }
             }

interface/src/renderer/Model.cpp

@@ -552,6 +552,9 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position, int last
     glm::vec3 relativePosition = position - _translation;
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     const QVector<int>& freeLineage = geometry.joints.at(jointIndex).freeLineage;
+    if (freeLineage.isEmpty()) {
+        return false;
+    }
     if (lastFreeIndex == -1) {
         lastFreeIndex = freeLineage.last();
     }
@@ -710,6 +713,37 @@ void Model::renderCollisionProxies(float alpha) {
     glPopMatrix();
 }
 
+bool Model::poke(ModelCollisionInfo& collision) {
+    // This needs work.  At the moment it can wiggle joints that are free to move (such as arms)
+    // but unmovable joints (such as torso) cannot be influenced at all.
+    glm::vec3 jointPosition(0.f);
+    if (getJointPosition(collision._jointIndex, jointPosition)) {
+        int jointIndex = collision._jointIndex;
+        const FBXJoint& joint = _geometry->getFBXGeometry().joints[jointIndex];
+        if (joint.parentIndex != -1) {
+            // compute the approximate distance (travel) that the joint needs to move
+            glm::vec3 start;
+            getJointPosition(joint.parentIndex, start);
+            glm::vec3 contactPoint = collision._contactPoint - start;
+            glm::vec3 penetrationEnd = contactPoint + collision._penetration;
+            glm::vec3 axis = glm::cross(contactPoint, penetrationEnd);
+            float travel = glm::length(axis);
+            const float MIN_TRAVEL = 1.0e-8f;
+            if (travel > MIN_TRAVEL) {
+                // compute the new position of the joint
+                float angle = asinf(travel / (glm::length(contactPoint) * glm::length(penetrationEnd)));
+                axis = glm::normalize(axis);
+                glm::vec3 end;
+                getJointPosition(jointIndex, end);
+                glm::vec3 newEnd = start + glm::angleAxis(glm::degrees(angle), axis) * (end - start);
+                // try to move it
+                return setJointPosition(jointIndex, newEnd, -1, true);
+            }
+        }
+    }
+    return false;
+}
+
 void Model::deleteGeometry() {
     foreach (Model* attachment, _attachments) {
         delete attachment;

interface/src/renderer/Model.h

@@ -164,6 +164,10 @@ public:
     
     void renderCollisionProxies(float alpha);
 
+    /// \param collisionInfo info about the collision
+    /// \return true if collision affects the Model
+    bool poke(ModelCollisionInfo& collisionInfo);
+
 protected:
 
     QSharedPointer<NetworkGeometry> _geometry;