From 10296577f4ef79da6ec8ee3bdeff81185dbfd978 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Mon, 10 Feb 2014 09:02:29 -0800
Subject: [PATCH] More info when colliding with meshes + stubbery for poking
 avatar.

---
 interface/src/avatar/Avatar.cpp      | 14 ++++++++++++++
 interface/src/avatar/Avatar.h        |  2 ++
 interface/src/avatar/Hand.cpp        |  5 ++---
 interface/src/renderer/Model.cpp     |  2 +-
 libraries/shared/src/CollisionInfo.h |  5 +++--
 5 files changed, 22 insertions(+), 6 deletions(-)

diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp
index 4236e069d1..1ae6331a8a 100644
--- a/interface/src/avatar/Avatar.cpp
+++ b/interface/src/avatar/Avatar.cpp
@@ -437,6 +437,20 @@ 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
+                );
+    }
+}
+
 float Avatar::getPelvisFloatingHeight() const {
     return -_skeletonModel.getBindExtents().minimum.y;
 }
diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h
index ca1b3cf49d..560680e2aa 100755
--- a/interface/src/avatar/Avatar.h
+++ b/interface/src/avatar/Avatar.h
@@ -126,6 +126,8 @@ public:
 
     float getHeight() const;
 
+    void poke(ModelCollisionInfo& collision);
+
 public slots:
     void updateCollisionFlags();
 
diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index 92eb5e5117..feff0a699d 100644
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -219,16 +219,15 @@ void Hand::updateCollisions() {
                                     // add slapback here
                                 }
                             }
-                            
-                            
                         }
                     }
                 }
                 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[i]);
                     }
-                    //  Check for collisions with the other avatar's leap palms
                 }
             }
         }
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 9404e99cd5..d71732cbde 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -474,10 +474,10 @@ bool Model::findSphereCollision(const glm::vec3& penetratorCenter, float penetra
         outerContinue: ;
     }
     if (jointIndex != -1) {
-        // TODO? Andrew to store contactPoint 
         // don't store collisionInfo._model at this stage, let the outer context do that
         collisionInfo._penetration = totalPenetration;
         collisionInfo._jointIndex = jointIndex;
+        collisionInfo._contactPoint = penetratorCenter + penetratorRadius * glm::normalize(totalPenetration);
         return true;
     }
     return false;
diff --git a/libraries/shared/src/CollisionInfo.h b/libraries/shared/src/CollisionInfo.h
index d48c22c876..1fa95cd83a 100644
--- a/libraries/shared/src/CollisionInfo.h
+++ b/libraries/shared/src/CollisionInfo.h
@@ -21,17 +21,18 @@ public:
     CollisionInfo() 
         : _damping(0.f),
         _elasticity(1.f),
+        _contactPoint(0.f), 
         _penetration(0.f), 
         _addedVelocity(0.f) {
     }
 
     ~CollisionInfo() {}
 
-    //glm::vec3 _point;
     //glm::vec3 _normal;
     float _damping;
     float _elasticity;
-    glm::vec3 _penetration; // depth that bodyA is penetrates bodyB
+    glm::vec3 _contactPoint; // world-frame point on bodyA that is deepest into bodyB
+    glm::vec3 _penetration; // depth that bodyA penetrates into bodyB
     glm::vec3 _addedVelocity;
 };