new collision pipeline for avatar shapes

interface/src/avatar/Avatar.cpp

@@ -489,6 +489,16 @@ 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) {
+    _skeletonModel.updateShapePositions();
+    bool collided = _skeletonModel.findCollisions(shapes, collisions);
+
+    Model& headModel = getHead()->getFaceModel();
+    headModel.updateShapePositions();
+    collided = headModel.findCollisions(shapes, collisions);
+    return collided;
+}
+
 bool Avatar::findParticleCollisions(const glm::vec3& particleCenter, float particleRadius, CollisionList& collisions) {
     if (_collisionFlags & COLLISION_GROUP_PARTICLES) {
         return false;

interface/src/avatar/Avatar.h

@@ -99,6 +99,11 @@ public:
 
     bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const;
 
+    /// \param shapes list of shapes to collide against avatar
+    /// \param collisions list to store collision results
+    /// \return true if at least one shape collided with avatar
+    bool findCollisions(const QVector<const Shape*>& shapes, CollisionList& collisions);
+
     /// 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

interface/src/avatar/Hand.cpp

@@ -96,7 +96,7 @@ void Hand::playSlaps(PalmData& palm, Avatar* avatar)
 const float MAX_COLLISIONS_PER_AVATAR = 32;
 static CollisionList handCollisions(MAX_COLLISIONS_PER_AVATAR);
 
-void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) {
+void Hand::collideAgainstAvatarOld(Avatar* avatar, bool isMyHand) {
     if (!avatar || avatar == _owningAvatar) {
         // don't collide with our own hands (that is done elsewhere)
         return;
@@ -117,17 +117,9 @@ void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) {
             for (int j = 0; j < handCollisions.size(); ++j) {
                 CollisionInfo* collision = handCollisions.getCollision(j);
                 if (isMyHand) {
-                    if (!avatar->collisionWouldMoveAvatar(*collision)) {
+                    // we resolve the hand from collision when it belongs to MyAvatar AND the other Avatar is 
-                        // 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)
-                        // not expected to respond to the collision (hand hit unmovable part of their Avatar)
+                    totalPenetration = addPenetrations(totalPenetration, collision->_penetration);
-                        totalPenetration = addPenetrations(totalPenetration, collision->_penetration);
-                    }
-                } else {
-                    // when !isMyHand then avatar is MyAvatar and we apply the collision
-                    // which might not do anything (hand hit unmovable part of MyAvatar) however
-                    // we don't resolve the hand's penetration because we expect the remote 
-                    // simulation to do the right thing.
-                    avatar->applyCollision(*collision);
                 }
             }
         }
@@ -138,6 +130,66 @@ void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) {
     }
 }
 
+void Hand::collideAgainstAvatar(Avatar* avatar, bool isMyHand) {
+    if (!avatar || avatar == _owningAvatar) {
+        // don't collide hands against ourself (that is done elsewhere)
+        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();
+
+    palmIndices[1] = -1;    // adebug temporarily disable right hand
+    jointIndices[1] = -1;   // adebug temporarily disable right hand
+
+    for (size_t i = 0; i < 1; i++) {
+        int palmIndex = palmIndices[i];
+        int jointIndex = jointIndices[i];
+        if (palmIndex == -1 || jointIndex == -1) {
+            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);
+        bool collided = isMyHand ? avatar->findCollisions(shapes, handCollisions) : avatar->findCollisions(shapes, handCollisions);
+        if (collided) {
+        //if (avatar->findCollisions(shapes, handCollisions)) {
+            glm::vec3 averagePenetration;
+            glm::vec3 averageContactPoint;
+            for (int j = 0; j < handCollisions.size(); ++j) {
+                CollisionInfo* collision = handCollisions.getCollision(j);
+                averagePenetration += 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.addToPosition(-averagePenetration);
+            } else {
+                // someone else's hand against MyAvatar
+                // TODO: submit collision info to MyAvatar which should lean accordingly
+                averageContactPoint /= float(handCollisions.size());
+            }
+        }
+    }
+}
+
 void Hand::collideAgainstOurself() {
     if (!Menu::getInstance()->isOptionChecked(MenuOption::HandsCollideWithSelf)) {
         return;

interface/src/avatar/Hand.h

@@ -56,6 +56,7 @@ public:
     const glm::vec3& getLeapFingerTipBallPosition (int ball) const { return _leapFingerTipBalls [ball].position;}
     const glm::vec3& getLeapFingerRootBallPosition(int ball) const { return _leapFingerRootBalls[ball].position;}
     
+    void collideAgainstAvatarOld(Avatar* avatar, bool isMyHand);
     void collideAgainstAvatar(Avatar* avatar, bool isMyHand);
     void collideAgainstOurself();
 

interface/src/avatar/MyAvatar.cpp

@@ -941,6 +941,11 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
         }
         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;
             float theirCapsuleRadius = 0.25f * (staticScale.x + staticScale.z);
@@ -952,6 +957,7 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
                 // move the avatar out by half the penetration
                 setPosition(_position - 0.5f * penetration);
             }
+            */
 
             // collide our hands against them
             getHand()->collideAgainstAvatar(avatar, true);

interface/src/avatar/SkeletonModel.cpp

@@ -73,6 +73,17 @@ bool SkeletonModel::render(float alpha) {
     return true;
 }
 
+void SkeletonModel::getHandShapes(int jointIndex, QVector<const Shape*>& shapes) const {
+    if (jointIndex == -1) {
+        return;
+    }
+    if (jointIndex == getLeftHandJointIndex()
+        || jointIndex == getRightHandJointIndex()) {
+        // TODO: also add fingers and other hand-parts
+        shapes.push_back(_shapes[jointIndex]);
+    }
+}
+
 class IndexValue {
 public:
     int index;

interface/src/avatar/SkeletonModel.h

@@ -24,6 +24,10 @@ public:
     
     void simulate(float deltaTime, bool delayLoad = false);
     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:
     

interface/src/renderer/Model.cpp

@@ -135,12 +135,11 @@ void Model::createCollisionShapes() {
 }
 
 void Model::updateShapePositions() {
-    float uniformScale = extractUniformScale(_scale);
-    const FBXGeometry& geometry = _geometry->getFBXGeometry();
     if (_shapesAreDirty && _shapes.size() == _jointStates.size()) {
+        float uniformScale = extractUniformScale(_scale);
+        const FBXGeometry& geometry = _geometry->getFBXGeometry();
         for (int i = 0; i < _jointStates.size(); i++) {
             const FBXJoint& joint = geometry.joints[i];
-
             // shape position and rotation are stored in world-frame
             glm::vec3 localPosition = uniformScale * (_jointStates[i].combinedRotation * joint.shapePosition);
             glm::vec3 worldPosition = _rotation * (extractTranslation(_jointStates[i].transform) + localPosition) + _translation;
@@ -508,6 +507,20 @@ bool Model::findRayIntersection(const glm::vec3& origin, const glm::vec3& direct
     return false;
 }
 
+bool Model::findCollisions(const QVector<const Shape*> shapes, CollisionList& collisions) {
+    bool collided = false;
+    for (int i = 0; i < shapes.size(); ++i) {
+        const Shape* theirShape = shapes[i];
+        for (int j = 0; j < _shapes.size(); ++j) {
+            const Shape* ourShape = _shapes[j];
+            if (ShapeCollider::shapeShape(theirShape, ourShape, collisions)) {
+                collided = true;
+            }
+        }
+    }
+    return collided;
+}
+
 bool Model::findSphereCollisions(const glm::vec3& sphereCenter, float sphereRadius,
     CollisionList& collisions, int skipIndex) {
     bool collided = false;

interface/src/renderer/Model.h

@@ -169,6 +169,11 @@ public:
     glm::vec4 computeAverageColor() const;
 
     bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction, float& distance) const;
+    
+    /// \param shapes list of pointers shapes to test against Model
+    /// \param collisions list to store collision results
+    /// \return true if at least one shape collided agains Model
+    bool findCollisions(const QVector<const Shape*> shapes, CollisionList& collisions);
 
     bool findSphereCollisions(const glm::vec3& penetratorCenter, float penetratorRadius,
         CollisionList& collisions, int skipIndex = -1);