MuscleConstraints for ragdoll; velocity for verlet points

interface/src/avatar/SkeletonModel.cpp

@@ -21,6 +21,7 @@
 #include "Avatar.h"
 #include "Hand.h"
 #include "Menu.h"
+#include "MuscleConstraint.h"
 #include "SkeletonModel.h"
 
 SkeletonModel::SkeletonModel(Avatar* owningAvatar, QObject* parent) : 
@@ -507,6 +508,7 @@ void SkeletonModel::renderRagdoll() {
 // virtual
 void SkeletonModel::initRagdollPoints() {
     clearRagdollConstraintsAndPoints();
+    _muscleConstraints.clear();
 
     // one point for each joint
     int numJoints = _jointStates.size();
@@ -514,8 +516,7 @@ void SkeletonModel::initRagdollPoints() {
     for (int i = 0; i < numJoints; ++i) {
         const JointState& state = _jointStates.at(i);
         glm::vec3 position = state.getPosition();
-        _ragdollPoints[i]._position = position;
+        _ragdollPoints[i].initPosition(position);
-        _ragdollPoints[i]._lastPosition = position;
     }
 }
 
@@ -525,11 +526,12 @@ void SkeletonModel::buildRagdollConstraints() {
     const int numPoints = _ragdollPoints.size();
     assert(numPoints == _jointStates.size());
 
+    float minBone = FLT_MAX;
+    float maxBone = -FLT_MAX;
     QMultiMap<int, int> families;
     for (int i = 0; i < numPoints; ++i) {
         const JointState& state = _jointStates.at(i);
-        const FBXJoint& joint = state.getFBXJoint();
+        int parentIndex = state.getParentIndex();
-        int parentIndex = joint.parentIndex;
         if (parentIndex == -1) {
             FixedConstraint* anchor = new FixedConstraint(&(_ragdollPoints[i]), glm::vec3(0.0f));
             _ragdollConstraints.push_back(anchor);
@@ -539,20 +541,59 @@ void SkeletonModel::buildRagdollConstraints() {
             _ragdollConstraints.push_back(bone);
             families.insert(parentIndex, i);
         }
+        float boneLength = glm::length(state.getPositionInParentFrame());
+        if (boneLength > maxBone) {
+            maxBone = boneLength;
+        } else if (boneLength < minBone) {
+            minBone = boneLength;
+        }
     }
     // Joints that have multiple children effectively have rigid constraints between the children
-    // in the parent frame, so we add constraints between children in the same family.
+    // in the parent frame, so we add DistanceConstraints between children in the same family.
     QMultiMap<int, int>::iterator itr = families.begin();
     while (itr != families.end()) {
         QList<int> children = families.values(itr.key());
-        if (children.size() > 1) {
+        int numChildren = children.size();
-            for (int i = 1; i < children.size(); ++i) {
+        if (numChildren > 1) {
+            for (int i = 1; i < numChildren; ++i) {
                 DistanceConstraint* bone = new DistanceConstraint(&(_ragdollPoints[children[i-1]]), &(_ragdollPoints[children[i]]));
                 _ragdollConstraints.push_back(bone);
             }
+            if (numChildren > 2) {
+                DistanceConstraint* bone = new DistanceConstraint(&(_ragdollPoints[children[numChildren-1]]), &(_ragdollPoints[children[0]]));
+                _ragdollConstraints.push_back(bone);
+            }
         }
         ++itr;
     }
+
+    float MAX_STRENGTH = 0.3f;
+    float MIN_STRENGTH = 0.005f;
+    // each joint gets a MuscleConstraint to its parent
+    for (int i = 1; i < numPoints; ++i) {
+        const JointState& state = _jointStates.at(i);
+        int p = state.getParentIndex();
+        if (p == -1) {
+            continue;
+        }
+        MuscleConstraint* constraint = new MuscleConstraint(&(_ragdollPoints[p]), &(_ragdollPoints[i]));
+        _ragdollConstraints.push_back(constraint);
+        _muscleConstraints.push_back(constraint);
+
+        // Short joints are more susceptible to wiggle so we modulate the strength based on the joint's length: 
+        // long = weak and short = strong.
+        constraint->setIndices(p, i);
+        float boneLength = glm::length(state.getPositionInParentFrame());
+
+        float strength = MIN_STRENGTH + (MAX_STRENGTH - MIN_STRENGTH) * (maxBone - boneLength) / (maxBone - minBone);
+        if (!families.contains(i)) {
+            // Although muscles only pull on the children not parents, nevertheless those joints that have
+            // parents AND children are more stable than joints at the end such as fingers.  For such joints we
+            // bestow maximum strength which helps reduce wiggle.
+            strength = MAX_MUSCLE_STRENGTH;
+        }
+        constraint->setStrength(strength);
+    }
     
 }
 
@@ -600,7 +641,8 @@ void SkeletonModel::updateVisibleJointStates() {
 
 // virtual 
 void SkeletonModel::stepRagdollForward(float deltaTime) {
-    moveShapesTowardJoints(deltaTime);
+    Ragdoll::stepRagdollForward(deltaTime);
+    updateMuscles();
 }
 
 float DENSITY_OF_WATER = 1000.0f; // kg/m^3
@@ -668,14 +710,16 @@ void SkeletonModel::buildShapes() {
     if (_ragdollPoints.size() == numStates) {
         int numJoints = _jointStates.size();
         for (int i = 0; i < numJoints; ++i) {
-            _ragdollPoints[i]._lastPosition = _ragdollPoints.at(i)._position;
+            _ragdollPoints[i].initPosition(_jointStates.at(i).getPosition());
-            _ragdollPoints[i]._position = _jointStates.at(i).getPosition();
         }
     }
     enforceRagdollConstraints();
 }
 
 void SkeletonModel::moveShapesTowardJoints(float deltaTime) {
+    // KEEP: although we don't currently use this method we may eventually need it to help
+    // unravel a skelton that has become tangled in its constraints.  So let's keep this
+    // around for a while just in case.
     const int numStates = _jointStates.size();
     assert(_jointStates.size() == _ragdollPoints.size());
     if (_ragdollPoints.size() != numStates) {
@@ -686,41 +730,26 @@ void SkeletonModel::moveShapesTowardJoints(float deltaTime) {
     const float RAGDOLL_FOLLOWS_JOINTS_TIMESCALE = 0.05f;
     float fraction = glm::clamp(deltaTime / RAGDOLL_FOLLOWS_JOINTS_TIMESCALE, 0.0f, 1.0f);
 
-    // SIMPLE LINEAR SLAVING -- KEEP this implementation for reference
+    float oneMinusFraction = 1.0f - fraction; 
-    //float oneMinusFraction = 1.0f - fraction; 
-    //for (int i = 0; i < numStates; ++i) {
-    //    _ragdollPoints[i]._lastPosition = _ragdollPoints[i]._position;
-    //    _ragdollPoints[i]._position = oneMinusFraction * _ragdollPoints[i]._position + fraction * _jointStates.at(i).getPosition();
-    //}
-    // SIMPLE LINEAR SLAVING -- KEEP
-
-    // parent-relative linear slaving
     for (int i = 0; i < numStates; ++i) {
-        JointState& state = _jointStates[i];
+        _ragdollPoints[i].initPosition(oneMinusFraction * _ragdollPoints[i]._position + 
-        _ragdollPoints[i]._lastPosition = _ragdollPoints.at(i)._position;
+                fraction * _jointStates.at(i).getPosition());
+    }
+}
 
-        int p = state.getParentIndex();
+void SkeletonModel::updateMuscles() {
-        if (p == -1) {
+    int numConstraints = _muscleConstraints.size();
-            _ragdollPoints[i]._position = glm::vec3(0.0f);
+    for (int i = 0; i < numConstraints; ++i) {
+        MuscleConstraint* constraint = _muscleConstraints[i];
+        int j = constraint->getParentIndex();
+        if (j == -1) {
             continue;
         }
-        if (state.getDistanceToParent() < EPSILON) {
+        int k = constraint->getChildIndex();
-            _ragdollPoints[i]._position = _ragdollPoints.at(p)._position;
+        if (k == -1) {
             continue;
         }
-
+        constraint->setChildOffset(_jointStates.at(k).getPosition() - _jointStates.at(j).getPosition());
-        glm::vec3 bone = _ragdollPoints.at(i)._lastPosition - _ragdollPoints.at(p)._lastPosition;
-        const JointState& parentState = _jointStates.at(p);
-        glm::vec3 targetBone = state.getPosition() - parentState.getPosition();
-
-        glm::vec3 newBone = (1.0f - fraction) * bone + fraction * targetBone;
-        float boneLength = glm::length(newBone);
-        if (boneLength > EPSILON) {
-            // slam newBone's length to that of the joint helps maintain distance constraints
-            newBone *= state.getDistanceToParent() / boneLength;
-        }
-        // set the new position relative to parent's new position
-        _ragdollPoints[i]._position = _ragdollPoints.at(p)._position + newBone;
     }
 }
 
@@ -740,8 +769,7 @@ void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) {
         int parentIndex = joint.parentIndex;
         if (parentIndex == -1) {
             transforms[i] = _jointStates[i].getTransform();
-            _ragdollPoints[i]._position = extractTranslation(transforms[i]);
+            _ragdollPoints[i].initPosition(extractTranslation(transforms[i]));
-            _ragdollPoints[i]._lastPosition = _ragdollPoints[i]._position;
             continue;
         }
         
@@ -749,8 +777,7 @@ void SkeletonModel::computeBoundingShape(const FBXGeometry& geometry) {
         transforms[i] = transforms[parentIndex] * glm::translate(joint.translation) 
             * joint.preTransform * glm::mat4_cast(modifiedRotation) * joint.postTransform;
         // setting the ragdollPoints here slams the VerletShapes into their default positions
-        _ragdollPoints[i]._position = extractTranslation(transforms[i]);
+        _ragdollPoints[i].initPosition(extractTranslation(transforms[i]));
-        _ragdollPoints[i]._lastPosition = _ragdollPoints[i]._position;
     }
 
     // compute bounding box that encloses all shapes

interface/src/avatar/SkeletonModel.h

@@ -18,6 +18,7 @@
 #include <Ragdoll.h>
 
 class Avatar;
+class MuscleConstraint;
 
 /// A skeleton loaded from a model.
 class SkeletonModel : public Model, public Ragdoll {
@@ -100,6 +101,7 @@ public:
     virtual void stepRagdollForward(float deltaTime);
 
     void moveShapesTowardJoints(float fraction);
+    void updateMuscles();
 
     void computeBoundingShape(const FBXGeometry& geometry);
     void renderBoundingCollisionShapes(float alpha);
@@ -144,6 +146,7 @@ private:
 
     CapsuleShape _boundingShape;
     glm::vec3 _boundingShapeLocalOffset;
+    QVector<MuscleConstraint*> _muscleConstraints;
 };
 
 #endif // hifi_SkeletonModel_h