Merge pull request #3277 from AndrewMeadows/ragdoll

Ragdoll Part 10: a little more stability

interface/src/avatar/MyAvatar.cpp

@@ -206,9 +206,9 @@ void MyAvatar::simulate(float deltaTime) {
     {
         PerformanceTimer perfTimer("ragdoll");
         if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
-            const float minError = 0.01f;
+            const float minError = 0.00001f;
-            const float maxIterations = 10;
+            const float maxIterations = 3;
-            const quint64 maxUsec = 2000;
+            const quint64 maxUsec = 4000;
             _physicsSimulation.setTranslation(_position);
             _physicsSimulation.stepForward(deltaTime, minError, maxIterations, maxUsec);
         } else {

interface/src/avatar/SkeletonModel.cpp

@@ -432,6 +432,9 @@ bool SkeletonModel::getHeadPosition(glm::vec3& headPosition) const {
 }
 
 bool SkeletonModel::getNeckPosition(glm::vec3& neckPosition) const {
+    if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
+        return isActive() && getVisibleJointPositionInWorldFrame(_geometry->getFBXGeometry().neckJointIndex, neckPosition);
+    }
     return isActive() && getJointPositionInWorldFrame(_geometry->getFBXGeometry().neckJointIndex, neckPosition);
 }
 
@@ -445,11 +448,16 @@ bool SkeletonModel::getNeckParentRotationFromDefaultOrientation(glm::quat& neckP
     }
     int parentIndex = geometry.joints.at(geometry.neckJointIndex).parentIndex;
     glm::quat worldFrameRotation;
-    if (getJointRotationInWorldFrame(parentIndex, worldFrameRotation)) {
+    bool success = false;
-        neckParentRotation = worldFrameRotation * _jointStates[parentIndex].getFBXJoint().inverseDefaultRotation;
+    if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
-        return true;
+        success = getVisibleJointRotationInWorldFrame(parentIndex, worldFrameRotation);
+    } else {
+        success = getJointRotationInWorldFrame(parentIndex, worldFrameRotation);
     }
-    return false;
+    if (success) {
+        neckParentRotation = worldFrameRotation * _jointStates[parentIndex].getFBXJoint().inverseDefaultRotation;
+    }
+    return success;
 }
 
 bool SkeletonModel::getEyePositions(glm::vec3& firstEyePosition, glm::vec3& secondEyePosition) const {

libraries/shared/src/ContactPoint.cpp

@@ -13,47 +13,76 @@
 #include "Shape.h"
 #include "SharedUtil.h"
 
-ContactPoint::ContactPoint() : _lastFrame(0), _shapeA(NULL), _shapeB(NULL), 
+// This parameter helps keep the actual point of contact slightly inside each shape 
-        _offsetA(0.0f), _offsetB(0.0f), _normal(0.0f) {
+// which allows the collisions to happen almost every frame for more frequent updates.
+const float CONTACT_PENETRATION_ALLOWANCE = 0.005f;
+
+ContactPoint::ContactPoint() : 
+        _lastFrame(0), _shapeA(NULL), _shapeB(NULL), 
+        _offsetA(0.0f), _offsetB(0.0f), 
+        _relativeMassA(0.5f), _relativeMassB(0.5f),
+        _numPointsA(0), _numPoints(0), _normal(0.0f) {
 }
 
-ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) : _lastFrame(frame),
+ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) : 
-        _shapeA(collision.getShapeA()), _shapeB(collision.getShapeB()), _offsetA(0.0f), _offsetB(0.0f), 
+        _lastFrame(frame), _shapeA(collision.getShapeA()), _shapeB(collision.getShapeB()), 
+        _offsetA(0.0f), _offsetB(0.0f), 
+        _relativeMassA(0.5f), _relativeMassB(0.5f), 
         _numPointsA(0), _numPoints(0), _normal(0.0f) {
 
-    _contactPoint = collision._contactPoint - 0.5f * collision._penetration;
+    glm::vec3 pointA = collision._contactPoint;
-    _offsetA = collision._contactPoint - _shapeA->getTranslation();
+    glm::vec3 pointB = collision._contactPoint - collision._penetration;
-    _offsetB = collision._contactPoint - collision._penetration - _shapeB->getTranslation();
+
     float pLength = glm::length(collision._penetration);
     if (pLength > EPSILON) {
         _normal = collision._penetration / pLength;
     }
-
     if (_shapeA->getID() > _shapeB->getID()) {
         // swap so that _shapeA always has lower ID
         _shapeA = collision.getShapeB();
         _shapeB = collision.getShapeA();
-
-        glm::vec3 temp = _offsetA;
-        _offsetA = _offsetB;
-        _offsetB = temp;
         _normal = - _normal;
+        pointA = pointB;
+        pointB = collision._contactPoint;
     }
 
+    // bring the contact points inside the shapes to help maintain collision updates
+    pointA -= CONTACT_PENETRATION_ALLOWANCE * _normal;
+    pointB += CONTACT_PENETRATION_ALLOWANCE * _normal;
+
+    _offsetA = pointA - _shapeA->getTranslation();
+    _offsetB = pointB - _shapeB->getTranslation();
+
     _shapeA->getVerletPoints(_points);
     _numPointsA = _points.size();
     _shapeB->getVerletPoints(_points);
     _numPoints = _points.size();
 
+    // compute and cache relative masses
+    float massA = EPSILON;
+    for (int i = 0; i < _numPointsA; ++i) {
+        massA += _points[i]->getMass();
+    }
+    float massB = EPSILON;
+    for (int i = _numPointsA; i < _numPoints; ++i) {
+        massB += _points[i]->getMass();
+    }
+    float invTotalMass = 1.0f / (massA + massB);
+    _relativeMassA = massA * invTotalMass;
+    _relativeMassB = massB * invTotalMass;
+
+    // _contactPoint will be the weighted average of the two
+    _contactPoint = _relativeMassA * pointA + _relativeMassB * pointB;
+
     // compute offsets for shapeA
     for (int i = 0; i < _numPointsA; ++i) {
-        glm::vec3 offset = _points[i]->_position - collision._contactPoint;
+        glm::vec3 offset = _points[i]->_position - pointA;
         _offsets.push_back(offset);
         _distances.push_back(glm::length(offset));
     }
     // compute offsets for shapeB
     for (int i = _numPointsA; i < _numPoints; ++i) {
-        glm::vec3 offset = _points[i]->_position - collision._contactPoint + collision._penetration;
+        glm::vec3 offset = _points[i]->_position - pointB;
         _offsets.push_back(offset);
         _distances.push_back(glm::length(offset));
     }
@@ -61,8 +90,7 @@ ContactPoint::ContactPoint(const CollisionInfo& collision, quint32 frame) : _las
 
 // virtual 
 float ContactPoint::enforce() {
-    int numPoints = _points.size();
+    for (int i = 0; i < _numPoints; ++i) {
-    for (int i = 0; i < numPoints; ++i) {
         glm::vec3& position = _points[i]->_position;
         // TODO: use a fast distance approximation
         float newDistance = glm::distance(_contactPoint, position);
@@ -82,8 +110,8 @@ void ContactPoint::buildConstraints() {
     glm::vec3 pointA = _shapeA->getTranslation() + _offsetA;
     glm::vec3 pointB = _shapeB->getTranslation() + _offsetB;
     glm::vec3 penetration = pointA - pointB;
-    float pDotN = glm::dot(penetration, _normal);
+   float pDotN = glm::dot(penetration, _normal);
-    bool actuallyMovePoints = (pDotN > EPSILON);
+    bool constraintViolation = (pDotN > CONTACT_PENETRATION_ALLOWANCE);
 
     // the contact point will be the average of the two points on the shapes
     _contactPoint = 0.5f * (pointA + pointB);
@@ -96,39 +124,45 @@ void ContactPoint::buildConstraints() {
     // that this makes it easier for limbs to tunnel through during collisions.
     const float HACK_STRENGTH = 0.5f;
 
-    int numPoints = _points.size();
+    if (constraintViolation) {
-    for (int i = 0; i < numPoints; ++i) {
+        for (int i = 0; i < _numPoints; ++i) {
-        VerletPoint* point = _points[i];
+            VerletPoint* point = _points[i];
-        glm::vec3 offset = _offsets[i];
+            glm::vec3 offset = _offsets[i];
-
-        // split delta into parallel and perpendicular components
-        glm::vec3 delta = _contactPoint + offset - point->_position;
-        glm::vec3 paraDelta = glm::dot(delta, _normal) * _normal;
-        glm::vec3 perpDelta = delta - paraDelta;
-        
-        // use the relative sizes of the components to decide how much perpenducular delta to use
-        // perpendicular < parallel ==> static friciton ==> perpFactor = 1.0
-        // perpendicular > parallel ==> dynamic friciton ==> cap to length of paraDelta ==> perpFactor < 1.0
-        float paraLength = glm::length(paraDelta);
-        float perpLength = glm::length(perpDelta);
-        float perpFactor = (perpLength > paraLength && perpLength > EPSILON) ? (paraLength / perpLength) : 1.0f;
-
-        // recombine the two components to get the final delta
-        delta = paraDelta + perpFactor * perpDelta;
     
-        glm::vec3 targetPosition = point->_position + delta;
+            // split delta into parallel and perpendicular components
-        _distances[i] = glm::distance(_contactPoint, targetPosition);
+            glm::vec3 delta = _contactPoint + offset - point->_position;
-        if (actuallyMovePoints) {
+            glm::vec3 paraDelta = glm::dot(delta, _normal) * _normal;
+            glm::vec3 perpDelta = delta - paraDelta;
+            
+            // use the relative sizes of the components to decide how much perpenducular delta to use
+            // perpendicular < parallel ==> static friction ==> perpFactor = 1.0
+            // perpendicular > parallel ==> dynamic friction ==> cap to length of paraDelta ==> perpFactor < 1.0
+            float paraLength = glm::length(paraDelta);
+            float perpLength = glm::length(perpDelta);
+            float perpFactor = (perpLength > paraLength && perpLength > EPSILON) ? (paraLength / perpLength) : 1.0f;
+    
+            // recombine the two components to get the final delta
+            delta = paraDelta + perpFactor * perpDelta;
+        
+            glm::vec3 targetPosition = point->_position + delta;
+            _distances[i] = glm::distance(_contactPoint, targetPosition);
             point->_position += HACK_STRENGTH * delta;
         }
+    } else {
+        for (int i = 0; i < _numPoints; ++i) {
+            _distances[i] = glm::length(glm::length(_offsets[i]));
+        }
     }
 }
 
 void ContactPoint::updateContact(const CollisionInfo& collision, quint32 frame) {
     _lastFrame = frame;
-    _contactPoint = collision._contactPoint - 0.5f * collision._penetration;
+
-    _offsetA = collision._contactPoint - collision._shapeA->getTranslation();
+    // compute contact points on surface of each shape
-    _offsetB = collision._contactPoint - collision._penetration - collision._shapeB->getTranslation();
+    glm::vec3 pointA = collision._contactPoint;
+    glm::vec3 pointB = pointA - collision._penetration;
+
+    // compute the normal (which points from A into B)
     float pLength = glm::length(collision._penetration);
     if (pLength > EPSILON) {
         _normal = collision._penetration / pLength;
@@ -138,19 +172,26 @@ void ContactPoint::updateContact(const CollisionInfo& collision, quint32 frame)
 
     if (collision._shapeA->getID() > collision._shapeB->getID()) {
         // our _shapeA always has lower ID
-        glm::vec3 temp = _offsetA;
-        _offsetA = _offsetB;
-        _offsetB = temp;
         _normal = - _normal;
+        pointA = pointB;
+        pointB = collision._contactPoint;
     }
+    
+    // bring the contact points inside the shapes to help maintain collision updates
+    pointA -= CONTACT_PENETRATION_ALLOWANCE * _normal;
+    pointB += CONTACT_PENETRATION_ALLOWANCE * _normal;
+
+    // compute relative offsets to per-shape contact points
+    _offsetA = pointA - collision._shapeA->getTranslation();
+    _offsetB = pointB - collision._shapeB->getTranslation();
 
     // compute offsets for shapeA
     assert(_offsets.size() == _numPoints);
     for (int i = 0; i < _numPointsA; ++i) {
-        _offsets[i] = (_points[i]->_position - collision._contactPoint);
+        _offsets[i] = _points[i]->_position - pointA;
     }
     // compute offsets for shapeB
     for (int i = _numPointsA; i < _numPoints; ++i) {
-        _offsets[i] = (_points[i]->_position - collision._contactPoint + collision._penetration);
+        _offsets[i] = _points[i]->_position - pointB;
     }
 }

libraries/shared/src/ContactPoint.h

@@ -41,6 +41,8 @@ protected:
     glm::vec3 _offsetA; // contact point relative to A's center
     glm::vec3 _offsetB; // contact point relative to B's center
     glm::vec3 _contactPoint; // a "virtual" point that is added to the simulation
+    float _relativeMassA; // massA / totalMass
+    float _relativeMassB; // massB / totalMass
     int _numPointsA; // number of VerletPoints that belong to _shapeA
     int _numPoints; // total number of VerletPoints
     QVector<VerletPoint*> _points; // points that belong to colliding shapes