Merge pull request #1981 from AndrewMeadows/avatar-interaction

Avatar body-body collisions

interface/src/avatar/Avatar.cpp

@@ -445,9 +445,16 @@ float Avatar::getHeight() const {
     return extents.maximum.y - extents.minimum.y;
 }
 
-bool Avatar::poke(ModelCollisionInfo& collision) {
-    // ATM poke() can only affect the Skeleton (not the head)
+bool Avatar::isPokeable(ModelCollisionInfo& collision) const {
+    // ATM only the Skeleton is pokeable
     // TODO: make poke affect head
+    if (collision._model == &_skeletonModel && collision._jointIndex != -1) {
+        return _skeletonModel.isPokeable(collision);
+    }
+    return false;
+}
+
+bool Avatar::poke(ModelCollisionInfo& collision) {
     if (collision._model == &_skeletonModel && collision._jointIndex != -1) {
         return _skeletonModel.poke(collision);
     }

interface/src/avatar/Avatar.h

@@ -128,6 +128,9 @@ public:
 
     float getHeight() const;
 
+    /// \return true if we expect the avatar would move as a result of the collision
+    bool isPokeable(ModelCollisionInfo& collision) const;
+
     /// \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);

interface/src/avatar/Hand.cpp

@@ -183,8 +183,9 @@ void Hand::updateCollisions() {
                     }
                 }
                 if (avatar->findSphereCollisions(palm.getPosition(), scaledPalmRadius, collisions)) {
-                    for (size_t j = 0; j < collisions.size(); ++j) {
-                        if (!avatar->poke(collisions[j])) {
+                    for (int j = 0; j < collisions.size(); ++j) {
+                        // we don't resolve penetrations that would poke the other avatar
+                        if (!avatar->isPokeable(collisions[j])) {
                             totalPenetration = addPenetrations(totalPenetration, collisions[j]._penetration);
                         }
                     }
@@ -200,7 +201,7 @@ void Hand::updateCollisions() {
                 skeletonModel.getLastFreeJointIndex((i == leftPalmIndex) ? skeletonModel.getLeftHandJointIndex() :
                     (i == rightPalmIndex) ? skeletonModel.getRightHandJointIndex() : -1)));
             if (_owningAvatar->findSphereCollisions(palm.getPosition(), scaledPalmRadius, collisions, skipIndex)) {
-                for (size_t j = 0; j < collisions.size(); ++j) {
+                for (int j = 0; j < collisions.size(); ++j) {
                     totalPenetration = addPenetrations(totalPenetration, collisions[j]._penetration);
                 }
             }

interface/src/avatar/MyAvatar.cpp

@@ -225,8 +225,6 @@ void MyAvatar::simulate(float deltaTime) {
             updateCollisionWithVoxels(deltaTime, radius);
         }
         if (_collisionFlags & COLLISION_GROUP_AVATARS) {
-            // Note, hand-vs-avatar collisions are done elsewhere
-            // This is where we avatar-vs-avatar bounding capsule
             updateCollisionWithAvatars(deltaTime);
         }
     }
@@ -926,7 +924,43 @@ void MyAvatar::updateCollisionSound(const glm::vec3 &penetration, float deltaTim
     }
 }
 
-const float DEFAULT_HAND_RADIUS = 0.1f;
+bool findAvatarAvatarPenetration(const glm::vec3 positionA, float radiusA, float heightA,
+        const glm::vec3 positionB, float radiusB, float heightB, glm::vec3& penetration) {
+    glm::vec3 positionBA = positionB - positionA;
+    float xzDistance = sqrt(positionBA.x * positionBA.x + positionBA.z * positionBA.z);
+    if (xzDistance < (radiusA + radiusB)) {
+        float yDistance = fabs(positionBA.y);
+        float halfHeights = 0.5 * (heightA + heightB);
+        if (yDistance < halfHeights) {
+            // cylinders collide
+            if (xzDistance > 0.f) {
+                positionBA.y = 0.f;
+                // note, penetration should point from A into B
+                penetration = positionBA * ((radiusA + radiusB - xzDistance) / xzDistance);
+                return true;
+            } else {
+                // exactly coaxial -- we'll return false for this case
+                return false;
+            }
+        } else if (yDistance < halfHeights + radiusA + radiusB) {
+            // caps collide
+            if (positionBA.y < 0.f) {
+                // A is above B
+                positionBA.y += halfHeights;
+                float BA = glm::length(positionBA);
+                penetration = positionBA * (radiusA + radiusB - BA) / BA;
+                return true;
+            } else {
+                // A is below B
+                positionBA.y -= halfHeights;
+                float BA = glm::length(positionBA);
+                penetration = positionBA * (radiusA + radiusB - BA) / BA;
+                return true;
+            }
+        }
+    }
+    return false;
+}
 
 void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
     //  Reset detector for nearest avatar
@@ -936,7 +970,14 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
         // no need to compute a bunch of stuff if we have one or fewer avatars
         return;
     }
-    float myRadius = getHeight();
+    float myBoundingRadius = 0.5f * getHeight();
+
+    // 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) {
@@ -949,9 +990,20 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
         if (_distanceToNearestAvatar > distance) {
             _distanceToNearestAvatar = distance;
         }
-        float theirRadius = avatar->getHeight();
-        if (distance < myRadius + theirRadius) {
-            // TODO: Andrew to make avatar-avatar capsule collisions work here
+        float theirBoundingRadius = 0.5f * avatar->getHeight();
+        if (distance < myBoundingRadius + theirBoundingRadius) {
+            Extents theirStaticExtents = _skeletonModel.getStaticExtents();
+            glm::vec3 staticScale = theirStaticExtents.maximum - theirStaticExtents.minimum;
+            float theirCapsuleRadius = 0.25f * (staticScale.x + staticScale.z);
+            float theirCapsuleHeight = staticScale.y;
+
+            glm::vec3 penetration(0.f);
+            if (findAvatarAvatarPenetration(_position, myCapsuleRadius, myCapsuleHeight,
+                avatar->getPosition(), theirCapsuleRadius, theirCapsuleHeight, penetration)) {
+                // move the avatar out by half the penetration
+                setPosition(_position - 0.5f * penetration);
+                glm::vec3 pushOut = 0.5f * penetration;
+            }
         }
     }
 }

interface/src/renderer/FBXReader.cpp

@@ -1274,6 +1274,8 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
 
     geometry.bindExtents.minimum = glm::vec3(FLT_MAX, FLT_MAX, FLT_MAX);
     geometry.bindExtents.maximum = glm::vec3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
+    geometry.staticExtents.minimum = glm::vec3(FLT_MAX, FLT_MAX, FLT_MAX);
+    geometry.staticExtents.maximum = glm::vec3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
     
     QVariantHash springs = mapping.value("spring").toHash();
     QVariant defaultSpring = springs.value("default");
@@ -1430,6 +1432,8 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                         boneDirection /= boneLength;
                     }
                 }
+                bool jointIsStatic = joint.freeLineage.isEmpty();
+                glm::vec3 jointTranslation = extractTranslation(geometry.offset * joint.bindTransform);
                 float radiusScale = extractUniformScale(joint.transform * fbxCluster.inverseBindMatrix);
                 float totalWeight = 0.0f;
                 for (int j = 0; j < cluster.indices.size(); j++) {
@@ -1447,6 +1451,11 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                                 joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance(
                                     vertex, boneEnd + boneDirection * proj));
                             }
+                            if (jointIsStatic) {
+                                // expand the extents of static (nonmovable) joints
+                                geometry.staticExtents.minimum = glm::min(geometry.staticExtents.minimum, vertex + jointTranslation);
+                                geometry.staticExtents.maximum = glm::max(geometry.staticExtents.maximum, vertex + jointTranslation);
+                            }
                         }
 
                         // look for an unused slot in the weights vector

interface/src/renderer/FBXReader.h

@@ -159,6 +159,7 @@ public:
     glm::vec3 neckPivot;
     
     Extents bindExtents;
+    Extents staticExtents;
     
     QVector<FBXAttachment> attachments;
 };

interface/src/renderer/Model.cpp

@@ -305,6 +305,15 @@ Extents Model::getBindExtents() const {
     return scaledExtents;
 }
 
+Extents Model::getStaticExtents() const {
+    if (!isActive()) {
+        return Extents();
+    }
+    const Extents& staticExtents = _geometry->getFBXGeometry().staticExtents;
+    Extents scaledExtents = { staticExtents.minimum * _scale, staticExtents.maximum * _scale };
+    return scaledExtents;
+}
+
 int Model::getParentJointIndex(int jointIndex) const {
     return (isActive() && jointIndex != -1) ? _geometry->getFBXGeometry().joints.at(jointIndex).parentIndex : -1;
 }
@@ -713,6 +722,18 @@ void Model::renderCollisionProxies(float alpha) {
     glPopMatrix();
 }
 
+bool Model::isPokeable(ModelCollisionInfo& collision) const {
+    // the joint is pokable by a collision if it exists and is free to move
+    const FBXJoint& joint = _geometry->getFBXGeometry().joints[collision._jointIndex];
+    if (joint.parentIndex == -1 || _jointStates.isEmpty()) {
+        return false;
+    }
+    // an empty freeLineage means the joint can't move
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    const QVector<int>& freeLineage = geometry.joints.at(collision._jointIndex).freeLineage;
+    return !freeLineage.isEmpty();
+}
+
 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.

interface/src/renderer/Model.h

@@ -66,6 +66,9 @@ public:
     
     /// Returns the extents of the model in its bind pose.
     Extents getBindExtents() const;
+
+    /// Returns the extents of the unmovable joints of the model.
+    Extents getStaticExtents() const;
     
     /// Returns a reference to the shared geometry.
     const QSharedPointer<NetworkGeometry>& getGeometry() const { return _geometry; }
@@ -164,6 +167,9 @@ public:
     
     void renderCollisionProxies(float alpha);
 
+    /// \return true if the collision would move the model
+    bool isPokeable(ModelCollisionInfo& collision) const;
+
     /// \param collisionInfo info about the collision
     /// \return true if collision affects the Model
     bool poke(ModelCollisionInfo& collisionInfo);