Compute more correct masses for ragdoll parts

also pin root ragdoll shape at the local-frame origin
for stability

interface/src/avatar/MyAvatar.cpp

@@ -199,8 +199,8 @@ void MyAvatar::simulate(float deltaTime) {
     if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
         PerformanceTimer perfTimer("MyAvatar::simulate/head Simulate");
         const int minError = 0.005f;
-        const float maxIterations = 4;
-        const quint64 maxUsec = 1000;
+        const float maxIterations = 8;
+        const quint64 maxUsec = 2000;
         _physicsSimulation.stepForward(deltaTime, minError, maxIterations, maxUsec);
     }
 

interface/src/avatar/SkeletonModel.cpp

@@ -31,6 +31,13 @@ SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent) :
 void SkeletonModel::setJointStates(QVector<JointState> states) {
     Model::setJointStates(states);
 
+    // the SkeletonModel override of updateJointState() will clear the translation part
+    // of its root joint and we need that done before we try to build shapes hence we
+    // recompute all joint transforms at this time.
+    for (int i = 0; i < _jointStates.size(); i++) {
+        updateJointState(i);
+    }
+
     clearShapes();
     clearRagdollConstraintsAndPoints();
     if (_enableShapes) {
@@ -95,26 +102,12 @@ void SkeletonModel::simulate(float deltaTime, bool fullUpdate) {
         applyPalmData(geometry.leftHandJointIndex, hand->getPalms()[leftPalmIndex]);
         applyPalmData(geometry.rightHandJointIndex, hand->getPalms()[rightPalmIndex]);
     }
-    
-    simulateRagdoll(deltaTime);
-}
-
-void SkeletonModel::simulateRagdoll(float deltaTime) {
-    const int numStates = _jointStates.size();
-    assert(numStates == _ragdollPoints.size());
-
-    float fraction = 0.1f; // fraction = 0.1f left intentionally low for demo purposes
-    moveShapesTowardJoints(fraction);
-
-    // enforce the constraints
-    float MIN_CONSTRAINT_ERROR = 0.005f; // 5mm
-    int MAX_ITERATIONS = 4;
-    int iterations = 0;
-    float delta = 0.0f;
-    do {
-        delta = enforceRagdollConstraints();
-        ++iterations;
-    } while (delta > MIN_CONSTRAINT_ERROR && iterations < MAX_ITERATIONS);
+    if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {    
+        const int numStates = _jointStates.size();
+        assert(numStates == _ragdollPoints.size());
+        float fraction = 0.1f; // fraction = 0.1f left intentionally low for demo purposes
+        moveShapesTowardJoints(fraction);
+    }
 }
 
 void SkeletonModel::getHandShapes(int jointIndex, QVector<const Shape*>& shapes) const {
@@ -541,7 +534,7 @@ void SkeletonModel::buildRagdollConstraints() {
         const FBXJoint& joint = state.getFBXJoint();
         int parentIndex = joint.parentIndex;
         if (parentIndex == -1) {
-            FixedConstraint* anchor = new FixedConstraint(&(_ragdollPoints[i]._position), glm::vec3(0.0f));
+            FixedConstraint* anchor = new FixedConstraint(&(_ragdollPoints[i]), glm::vec3(0.0f));
             _ragdollConstraints.push_back(anchor);
         } else { 
             DistanceConstraint* bone = new DistanceConstraint(&(_ragdollPoints[i]), &(_ragdollPoints[parentIndex]));
@@ -554,6 +547,10 @@ void SkeletonModel::buildRagdollConstraints() {
 void SkeletonModel::stepRagdollForward(float deltaTime) {
 }
 
+float DENSITY_OF_WATER = 1000.0f; // kg/m^3
+float MIN_JOINT_MASS = 1.0f;
+float VERY_BIG_MASS = 1.0e6f;
+
 // virtual
 void SkeletonModel::buildShapes() {
     if (!_geometry || _rootIndex == -1) {
@@ -570,7 +567,8 @@ void SkeletonModel::buildShapes() {
     float uniformScale = extractUniformScale(_scale);
     const int numStates = _jointStates.size();
     for (int i = 0; i < numStates; i++) {
-        const FBXJoint& joint = geometry.joints[i];
+        JointState& state = _jointStates[i];
+        const FBXJoint& joint = state.getFBXJoint();
         float radius = uniformScale * joint.boneRadius;
         float halfHeight = 0.5f * uniformScale * joint.distanceToParent;
         Shape::Type type = joint.shapeType;
@@ -579,27 +577,40 @@ void SkeletonModel::buildShapes() {
             type = Shape::SPHERE_SHAPE;
         }
         Shape* shape = NULL;
+        int parentIndex = joint.parentIndex;
         if (type == Shape::SPHERE_SHAPE) {
             shape = new VerletSphereShape(radius, &(_ragdollPoints[i]));
             shape->setEntity(this);
+            _ragdollPoints[i]._mass = glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume());
         } else if (type == Shape::CAPSULE_SHAPE) {
-            int parentIndex = joint.parentIndex;
             assert(parentIndex != -1);
             shape = new VerletCapsuleShape(radius, &(_ragdollPoints[parentIndex]), &(_ragdollPoints[i]));
             shape->setEntity(this);
+            _ragdollPoints[i]._mass = glm::max(MIN_JOINT_MASS, DENSITY_OF_WATER * shape->getVolume());
         } 
+        if (parentIndex != -1) {
+            // always disable collisions between joint and its parent
+            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)
+            _ragdollPoints[i]._mass = VERY_BIG_MASS;
+        }
         _shapes.push_back(shape);
     }
 
-    // This method moves the shapes to their default positions in Model frame
+    // This method moves the shapes to their default positions in Model frame.
     computeBoundingShape(geometry);
 
-    // while the shapes are in their default position...
-    disableSelfCollisions();
+    // While the shapes are in their default position we disable collisions between
+    // joints that are currently colliding.
+    disableCurrentSelfCollisions();
+
     buildRagdollConstraints();
 
     // ... then move shapes back to current joint positions
     moveShapesTowardJoints(1.0f);
+    enforceRagdollConstraints();
 }
 
 void SkeletonModel::moveShapesTowardJoints(float fraction) {

interface/src/avatar/SkeletonModel.h

@@ -30,7 +30,6 @@ public:
     void setJointStates(QVector<JointState> states);
 
     void simulate(float deltaTime, bool fullUpdate = true);
-    void simulateRagdoll(float deltaTime);
 
     /// \param jointIndex index of hand joint
     /// \param shapes[out] list in which is stored pointers to hand shapes

libraries/shared/src/PhysicsEntity.cpp

@@ -164,8 +164,8 @@ bool PhysicsEntity::findPlaneCollisions(const glm::vec4& plane, CollisionList& c
 }
 
 // -----------------------------------------------------------
-// TODO: enforce this maximum when shapes are built.  The gotcha here is that
-// the Model class (derived from PhysicsEntity) expects numShapes == numJoints, 
+// TODO: enforce this maximum when shapes are actually built.  The gotcha here is 
+// that the Model class (derived from PhysicsEntity) expects numShapes == numJoints, 
 // so we have to modify that code to be safe.
 const int MAX_SHAPES_PER_ENTITY = 256;
 
@@ -211,7 +211,7 @@ bool PhysicsEntity::collisionsAreEnabled(int shapeIndexA, int shapeIndexB) const
     return false;
 }
 
-void PhysicsEntity::disableSelfCollisions() {
+void PhysicsEntity::disableCurrentSelfCollisions() {
     CollisionList collisions(10);
     int numShapes = _shapes.size();
     for (int i = 0; i < numShapes; ++i) {
@@ -220,6 +220,9 @@ void PhysicsEntity::disableSelfCollisions() {
             continue;
         }
         for (int j = i+1; j < numShapes; ++j) {
+            if (!collisionsAreEnabled(i, j)) {
+                continue;
+            }
             const Shape* otherShape = _shapes.at(j);
             if (otherShape && ShapeCollider::collideShapes(shape, otherShape, collisions)) {
                 disableCollisions(i, j);

libraries/shared/src/PhysicsEntity.h

@@ -58,7 +58,7 @@ public:
     void disableCollisions(int shapeIndexA, int shapeIndexB);
     bool collisionsAreEnabled(int shapeIndexA, int shapeIndexB) const;
 
-    void disableSelfCollisions();
+    void disableCurrentSelfCollisions();
 
 protected:
     glm::vec3 _translation;

libraries/shared/src/PhysicsSimulation.cpp

@@ -146,7 +146,6 @@ void PhysicsSimulation::removeRagdoll(Ragdoll* doll) {
 // (10b) figure out how to slave dupe JointStates to physical shapes
 // (11) add and enforce angular contraints for joints
 void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIterations, quint64 maxUsec) {
-    static int adebug = 0; ++adebug;
     quint64 startTime = usecTimestampNow();
     quint64 expiry = startTime + maxUsec;
 
@@ -156,9 +155,6 @@ void PhysicsSimulation::stepForward(float deltaTime, float minError, int maxIter
     enforceConstraints(minError, maxIterations, expiry - usecTimestampNow());
 
     _stepTime = usecTimestampNow()- startTime;
-    if (0 == (adebug % 200)) {
-        std::cout << " adebug nC = " << _numCollisions << "  i = " << _numIterations << "  e = " << _constraintError << "  t = " << _stepTime << std::endl;  // adebug
-    }
 }
 
 void PhysicsSimulation::moveRagdolls(float deltaTime) {

libraries/shared/src/Ragdoll.cpp

@@ -35,18 +35,21 @@ void VerletPoint::applyAccumulatedDelta() {
 // ----------------------------------------------------------------------------
 // FixedConstraint
 // ----------------------------------------------------------------------------
-FixedConstraint::FixedConstraint(glm::vec3* point, const glm::vec3& anchor) : _point(point), _anchor(anchor) {
+FixedConstraint::FixedConstraint(VerletPoint* point, const glm::vec3& anchor) : _point(point), _anchor(anchor) {
 }
 
 float FixedConstraint::enforce() {
     assert(_point != NULL);
-    float distance = glm::distance(_anchor, *_point);
-    *_point = _anchor;
+    // TODO: use fast approximate sqrt here
+    float distance = glm::distance(_anchor, _point->_position);
+    _point->_position = _anchor;
     return distance;
 }
 
-void FixedConstraint::setPoint(glm::vec3* point) {
+void FixedConstraint::setPoint(VerletPoint* point) {
+    assert(point);
     _point = point;
+    _point->_mass = Shape::MAX_MASS;
 }
 
 void FixedConstraint::setAnchor(const glm::vec3& anchor) {

libraries/shared/src/Ragdoll.h

@@ -59,12 +59,12 @@ protected:
 
 class FixedConstraint : public Constraint {
 public:
-    FixedConstraint(glm::vec3* point, const glm::vec3& anchor);
+    FixedConstraint(VerletPoint* point, const glm::vec3& anchor);
     float enforce();
-    void setPoint(glm::vec3* point);
+    void setPoint(VerletPoint* point);
     void setAnchor(const glm::vec3& anchor);
 private:
-    glm::vec3* _point;
+    VerletPoint* _point;
     glm::vec3 _anchor;
 };