Merge pull request #3295 from AndrewMeadows/ragdoll

Ragdoll Part 11: ragdoll collisions move avatar

interface/src/avatar/MyAvatar.cpp

@@ -206,12 +206,21 @@ void MyAvatar::simulate(float deltaTime) {
 
     {
         PerformanceTimer perfTimer("ragdoll");
-        if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
+        Ragdoll* ragdoll = _skeletonModel.getRagdoll();
+        if (ragdoll && Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
             const float minError = 0.00001f;
             const float maxIterations = 3;
             const quint64 maxUsec = 4000;
             _physicsSimulation.setTranslation(_position);
             _physicsSimulation.stepForward(deltaTime, minError, maxIterations, maxUsec);
+
+            // harvest any displacement of the Ragdoll that is a result of collisions
+            glm::vec3 ragdollDisplacement = ragdoll->getAndClearAccumulatedMovement();
+            const float MAX_RAGDOLL_DISPLACEMENT_2 = 1.0f;
+            float length2 = glm::length2(ragdollDisplacement);
+            if (length2 > EPSILON && length2 < MAX_RAGDOLL_DISPLACEMENT_2) {
+                setPosition(getPosition() + ragdollDisplacement);
+            }
         } else {
             _skeletonModel.moveShapesTowardJoints(1.0f);
         }

interface/src/avatar/SkeletonModel.cpp

@@ -14,14 +14,11 @@
 
 #include <VerletCapsuleShape.h>
 #include <VerletSphereShape.h>
-#include <DistanceConstraint.h>
-#include <FixedConstraint.h>
 
 #include "Application.h"
 #include "Avatar.h"
 #include "Hand.h"
 #include "Menu.h"
-#include "MuscleConstraint.h"
 #include "SkeletonModel.h"
 #include "SkeletonRagdoll.h"
 
@@ -604,6 +601,7 @@ void SkeletonModel::buildShapes() {
 
     float uniformScale = extractUniformScale(_scale);
     const int numStates = _jointStates.size();
+    float totalMass = 0.0f;
     for (int i = 0; i < numStates; i++) {
         JointState& state = _jointStates[i];
         const FBXJoint& joint = state.getFBXJoint();
@@ -622,26 +620,31 @@ void SkeletonModel::buildShapes() {
         if (type == Shape::SPHERE_SHAPE) {
             shape = new VerletSphereShape(radius, &(points[i]));
             shape->setEntity(this);
-            points[i].setMass(massScale * glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume()));
+            float mass = massScale * glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume());
+            points[i].setMass(mass);
+            totalMass += mass;
         } else if (type == Shape::CAPSULE_SHAPE) {
             assert(parentIndex != -1);
             shape = new VerletCapsuleShape(radius, &(points[parentIndex]), &(points[i]));
             shape->setEntity(this);
-            points[i].setMass(massScale * glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume()));
+            float mass = massScale * glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume());
+            points[i].setMass(mass);
+            totalMass += mass;
         } 
         if (parentIndex != -1) {
             // always disable collisions between joint and its parent
             if (shape) {
                 disableCollisions(i, parentIndex);
             }
-        } else {
-            // give the base joint a very large mass since it doesn't actually move
-            // in the local-frame simulation (it defines the origin)
-            points[i].setMass(VERY_BIG_MASS);
-        }
+        } 
         _shapes.push_back(shape);
     }
 
+    // set the mass of the root
+    if (numStates > 0) {
+        points[0].setMass(totalMass);
+    }
+
     // This method moves the shapes to their default positions in Model frame.
     computeBoundingShape(geometry);
 

interface/src/avatar/SkeletonRagdoll.cpp

@@ -70,10 +70,7 @@ void SkeletonRagdoll::buildConstraints() {
     for (int i = 0; i < numPoints; ++i) {
         const JointState& state = jointStates.at(i);
         int parentIndex = state.getParentIndex();
-        if (parentIndex == -1) {
-            FixedConstraint* anchor = new FixedConstraint(&_translationInSimulationFrame, &(_points[i]));
-            _fixedConstraints.push_back(anchor);
-        } else { 
+        if (parentIndex != -1) {
             DistanceConstraint* bone = new DistanceConstraint(&(_points[i]), &(_points[parentIndex]));
             bone->setDistance(state.getDistanceToParent());
             _boneConstraints.push_back(bone);

interface/src/avatar/SkeletonRagdoll.h

@@ -33,7 +33,9 @@ public:
     virtual void initPoints();
     virtual void buildConstraints();
 
+protected:
     void updateMuscles();
+
 private:
     Model* _model;
     QVector<MuscleConstraint*> _muscleConstraints;

libraries/shared/src/ContactPoint.cpp

@@ -96,10 +96,10 @@ float ContactPoint::enforce() {
     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);
+    _contactPoint = _relativeMassA * pointA + _relativeMassB * pointB;
 
     if (constraintViolation) {
-        for (int i = 0; i < _numPoints; ++i) {
+        for (int i = 0; i < _numPointsA; ++i) {
             VerletPoint* point = _points[i];
             glm::vec3 offset = _offsets[i];
     
@@ -111,8 +111,31 @@ float ContactPoint::enforce() {
             // 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 paraLength = _relativeMassB * glm::length(paraDelta);
+            float perpLength = _relativeMassA * 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 += delta;
+        }
+        for (int i = _numPointsA; i < _numPoints; ++i) {
+            VerletPoint* point = _points[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 friction ==> perpFactor = 1.0
+            // perpendicular > parallel ==> dynamic friction ==> cap to length of paraDelta ==> perpFactor < 1.0
+            float paraLength = _relativeMassA * glm::length(paraDelta);
+            float perpLength = _relativeMassB * glm::length(perpDelta);
             float perpFactor = (perpLength > paraLength && perpLength > EPSILON) ? (paraLength / perpLength) : 1.0f;
     
             // recombine the two components to get the final delta

libraries/shared/src/PhysicsSimulation.cpp

@@ -163,10 +163,10 @@ bool PhysicsSimulation::addRagdoll(Ragdoll* doll) {
 }
 
 void PhysicsSimulation::removeRagdoll(Ragdoll* doll) {
-    int numDolls = _otherRagdolls.size();
-    if (doll->_simulation != this) {
+    if (!doll || doll->_simulation != this) {
         return;
     }
+    int numDolls = _otherRagdolls.size();
     for (int i = 0; i < numDolls; ++i) {
         if (doll == _otherRagdolls[i]) {
             if (i == numDolls - 1) {
@@ -205,10 +205,11 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
         }
     }
 
+    bool collidedWithOtherRagdoll = false;
     int iterations = 0;
     float error = 0.0f;
     do {
-        computeCollisions();
+        collidedWithOtherRagdoll = computeCollisions() || collidedWithOtherRagdoll;
         updateContacts();
         resolveCollisions();
 
@@ -225,6 +226,14 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
         now = usecTimestampNow();
     } while (_collisions.size() != 0 && (iterations < maxIterations) && (error > minError) && (now < expiry));
 
+    // the collisions may have moved the main ragdoll from the simulation center
+    // so we remove this offset (potentially storing it as movement of the Ragdoll owner)
+    _ragdoll->removeRootOffset(collidedWithOtherRagdoll);
+
+    // also remove any offsets from the other ragdolls
+    for (int i = 0; i < numDolls; ++i) {
+        _otherRagdolls[i]->removeRootOffset(false);
+    }
     pruneContacts();
 }
 
@@ -237,7 +246,7 @@ void PhysicsSimulation::moveRagdolls(float deltaTime) {
     }
 }
 
-void PhysicsSimulation::computeCollisions() {
+bool PhysicsSimulation::computeCollisions() {
     PerformanceTimer perfTimer("collide");
     _collisions.clear();
 
@@ -258,11 +267,13 @@ void PhysicsSimulation::computeCollisions() {
     }
 
     // collide main ragdoll with others
+    bool otherCollisions = false;
     int numEntities = _otherEntities.size();
     for (int i = 0; i < numEntities; ++i) {
         const QVector<Shape*> otherShapes = _otherEntities.at(i)->getShapes();
-        ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisions);
+        otherCollisions = ShapeCollider::collideShapesWithShapes(shapes, otherShapes, _collisions) || otherCollisions;
     }
+    return otherCollisions;
 }
 
 void PhysicsSimulation::resolveCollisions() {

libraries/shared/src/PhysicsSimulation.h

@@ -53,9 +53,11 @@ public:
 
 protected:
     void moveRagdolls(float deltaTime);
-    void computeCollisions();
-    void resolveCollisions();
 
+    /// \return true if main ragdoll collides with other avatar
+    bool computeCollisions();
+
+    void resolveCollisions();
     void enforceContacts();
     void applyContactFriction();
     void updateContacts();

libraries/shared/src/Ragdoll.cpp

@@ -19,7 +19,8 @@
 #include "PhysicsSimulation.h"
 #include "SharedUtil.h" // for EPSILON
 
-Ragdoll::Ragdoll() : _massScale(1.0f), _translation(0.0f), _translationInSimulationFrame(0.0f), _simulation(NULL) {
+Ragdoll::Ragdoll() : _massScale(1.0f), _translation(0.0f), _translationInSimulationFrame(0.0f), 
+        _accumulatedMovement(0.0f), _simulation(NULL) {
 }
 
 Ragdoll::~Ragdoll() {
@@ -116,3 +117,27 @@ void Ragdoll::setMassScale(float scale) {
         _massScale = scale;
     }
 }
+
+void Ragdoll::removeRootOffset(bool accumulateMovement) {
+    const int numPoints = _points.size();
+    if (numPoints > 0) {
+        // shift all points so that the root aligns with the the ragdoll's position in the simulation
+        glm::vec3 offset = _translationInSimulationFrame - _points[0]._position;
+        float offsetLength = glm::length(offset);
+        if (offsetLength > EPSILON) {
+            for (int i = 0; i < numPoints; ++i) {
+                _points[i].shift(offset);
+            }
+            const float MIN_ROOT_OFFSET = 0.02f;
+            if (accumulateMovement && offsetLength > MIN_ROOT_OFFSET) {
+                _accumulatedMovement -= (1.0f - MIN_ROOT_OFFSET / offsetLength) * offset;
+            }
+        }
+    }
+}
+
+glm::vec3 Ragdoll::getAndClearAccumulatedMovement() {
+    glm::vec3 movement = _accumulatedMovement;
+    _accumulatedMovement = glm::vec3(0.0f);
+    return movement;
+}

libraries/shared/src/Ragdoll.h

@@ -56,6 +56,10 @@ public:
     virtual void initPoints() = 0;
     virtual void buildConstraints() = 0;
 
+    void removeRootOffset(bool accumulateMovement);
+
+    glm::vec3 getAndClearAccumulatedMovement();
+
 protected:
     float _massScale;
     glm::vec3 _translation;  // world-frame
@@ -66,6 +70,12 @@ protected:
     QVector<VerletPoint> _points;
     QVector<DistanceConstraint*> _boneConstraints;
     QVector<FixedConstraint*> _fixedConstraints;
+
+    // The collisions are typically done in a simulation frame that is slaved to the center of one of the Ragdolls.
+    // To allow the Ragdoll to provide feedback of its own displacement we store it in _accumulatedMovement.
+    // The owner of the Ragdoll can harvest this displacement to update the rest of the object positions in the simulation.
+    glm::vec3 _accumulatedMovement;
+
 private:
     void updateSimulationTransforms(const glm::vec3& translation, const glm::quat& rotation);
 

libraries/shared/src/VerletPoint.cpp

@@ -39,6 +39,11 @@ void VerletPoint::move(const glm::vec3& deltaPosition, const glm::quat& deltaRot
     _lastPosition += deltaPosition + (deltaRotation * arm - arm);
 }
 
+void VerletPoint::shift(const glm::vec3& deltaPosition) {
+    _position += deltaPosition;
+    _lastPosition += deltaPosition;
+}
+
 void VerletPoint::setMass(float mass) {
     const float MIN_MASS = 1.0e-6f;
     const float MAX_MASS = 1.0e18f;

libraries/shared/src/VerletPoint.h

@@ -25,6 +25,7 @@ public:
     void accumulateDelta(const glm::vec3& delta);
     void applyAccumulatedDelta();
     void move(const glm::vec3& deltaPosition, const glm::quat& deltaRotation, const glm::vec3& oldPivot);
+    void shift(const glm::vec3& deltaPosition);
 
     void setMass(float mass);
     float getMass() const { return _mass; }