Merge pull request #3209 from AndrewMeadows/ragdoll

Ragdoll Part 5: send visible transforms when using ragdoll

interface/src/avatar/MyAvatar.cpp

@@ -162,9 +162,16 @@ void MyAvatar::simulate(float deltaTime) {
         PerformanceTimer perfTimer("joints");
         // copy out the skeleton joints from the model
         _jointData.resize(_skeletonModel.getJointStateCount());
-        for (int i = 0; i < _jointData.size(); i++) {
-            JointData& data = _jointData[i];
-            data.valid = _skeletonModel.getJointState(i, data.rotation);
+        if (Menu::getInstance()->isOptionChecked(MenuOption::CollideAsRagdoll)) {
+            for (int i = 0; i < _jointData.size(); i++) {
+                JointData& data = _jointData[i];
+                data.valid = _skeletonModel.getVisibleJointState(i, data.rotation);
+            }
+        } else {
+            for (int i = 0; i < _jointData.size(); i++) {
+                JointData& data = _jointData[i];
+                data.valid = _skeletonModel.getJointState(i, data.rotation);
+            }
         }
     }
 

interface/src/avatar/SkeletonModel.cpp

@@ -533,6 +533,7 @@ void SkeletonModel::buildRagdollConstraints() {
             _ragdollConstraints.push_back(anchor);
         } else { 
             DistanceConstraint* bone = new DistanceConstraint(&(_ragdollPoints[i]), &(_ragdollPoints[parentIndex]));
+            bone->setDistance(state.getDistanceToParent());
             _ragdollConstraints.push_back(bone);
             families.insert(parentIndex, i);
         }
@@ -597,11 +598,7 @@ void SkeletonModel::updateVisibleJointStates() {
 
 // virtual 
 void SkeletonModel::stepRagdollForward(float deltaTime) {
-    // NOTE: increasing this timescale reduces vibrations in the ragdoll solution and reduces tunneling 
-    // but makes the shapes slower to follow the body (introduces lag).
-    const float RAGDOLL_FOLLOWS_JOINTS_TIMESCALE = 0.05f;
-    float fraction = glm::clamp(deltaTime / RAGDOLL_FOLLOWS_JOINTS_TIMESCALE, 0.0f, 1.0f);
-    moveShapesTowardJoints(fraction);
+    moveShapesTowardJoints(deltaTime);
 }
 
 float DENSITY_OF_WATER = 1000.0f; // kg/m^3
@@ -676,17 +673,52 @@ void SkeletonModel::buildShapes() {
     enforceRagdollConstraints();
 }
 
-void SkeletonModel::moveShapesTowardJoints(float fraction) {
+void SkeletonModel::moveShapesTowardJoints(float deltaTime) {
     const int numStates = _jointStates.size();
     assert(_jointStates.size() == _ragdollPoints.size());
-    assert(fraction >= 0.0f && fraction <= 1.0f);
-    if (_ragdollPoints.size() == numStates) {
-        float oneMinusFraction = 1.0f - fraction; 
-        int numJoints = _jointStates.size();
-        for (int i = 0; i < numJoints; ++i) {
-            _ragdollPoints[i]._lastPosition = _ragdollPoints[i]._position;
-            _ragdollPoints[i]._position = oneMinusFraction * _ragdollPoints[i]._position + fraction * _jointStates.at(i).getPosition();
+    if (_ragdollPoints.size() != numStates) {
+        return;
+    }
+
+    // fraction = 0 means keep old position, = 1 means slave 100% to target position
+    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; 
+    //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]._lastPosition = _ragdollPoints.at(i)._position;
+
+        int p = state.getParentIndex();
+        if (p == -1) {
+            _ragdollPoints[i]._position = glm::vec3(0.0f);
+            continue;
         }
+        if (state.getDistanceToParent() < EPSILON) {
+            _ragdollPoints[i]._position = _ragdollPoints.at(p)._position;
+            continue;
+        }
+
+        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;
     }
 }
 

interface/src/renderer/JointState.cpp

@@ -20,6 +20,7 @@
 JointState::JointState() :
     _animationPriority(0.0f),
     _positionInParentFrame(0.0f),
+    _distanceToParent(0.0f),
     _fbxJoint(NULL),
     _constraint(NULL) {
 }
@@ -29,6 +30,7 @@ JointState::JointState(const JointState& other) : _constraint(NULL) {
     _rotation = other._rotation;
     _rotationInConstrainedFrame = other._rotationInConstrainedFrame;
     _positionInParentFrame = other._positionInParentFrame;
+    _distanceToParent = other._distanceToParent;
     _animationPriority = other._animationPriority;
     _fbxJoint = other._fbxJoint;
     // DO NOT copy _constraint
@@ -72,6 +74,7 @@ void JointState::copyState(const JointState& state) {
     _rotation = extractRotation(_transform);
     _rotationInConstrainedFrame = state._rotationInConstrainedFrame;
     _positionInParentFrame = state._positionInParentFrame;
+    _distanceToParent = state._distanceToParent;
 
     _visibleTransform = state._visibleTransform;
     _visibleRotation = extractRotation(_visibleTransform);
@@ -82,6 +85,7 @@ void JointState::copyState(const JointState& state) {
 void JointState::initTransform(const glm::mat4& parentTransform) {
     computeTransform(parentTransform);
     _positionInParentFrame = glm::inverse(extractRotation(parentTransform)) * (extractTranslation(_transform) - extractTranslation(parentTransform));
+    _distanceToParent = glm::length(_positionInParentFrame);
 }
 
 void JointState::computeTransform(const glm::mat4& parentTransform) {
@@ -214,6 +218,14 @@ void JointState::setVisibleRotationInConstrainedFrame(const glm::quat& targetRot
     _visibleRotation = parentRotation * _fbxJoint->preRotation * _visibleRotationInConstrainedFrame * _fbxJoint->postRotation;
 }
 
+const bool JointState::rotationIsDefault(const glm::quat& rotation, float tolerance) const {
+    glm::quat defaultRotation = _fbxJoint->rotation;
+    return glm::abs(rotation.x - defaultRotation.x) < tolerance &&
+        glm::abs(rotation.y - defaultRotation.y) < tolerance &&
+        glm::abs(rotation.z - defaultRotation.z) < tolerance &&
+        glm::abs(rotation.w - defaultRotation.w) < tolerance;
+}
+
 const glm::vec3& JointState::getDefaultTranslationInConstrainedFrame() const {
     assert(_fbxJoint != NULL);
     return _fbxJoint->translation;

interface/src/renderer/JointState.h

@@ -51,6 +51,7 @@ public:
     glm::quat getRotationInParentFrame() const;
     glm::quat getVisibleRotationInParentFrame() const;
     const glm::vec3& getPositionInParentFrame() const { return _positionInParentFrame; }
+    float getDistanceToParent() const { return _distanceToParent; }
 
     int getParentIndex() const { return _fbxJoint->parentIndex; }
 
@@ -81,6 +82,9 @@ public:
     void setRotationInConstrainedFrame(const glm::quat& targetRotation);
     void setVisibleRotationInConstrainedFrame(const glm::quat& targetRotation);
     const glm::quat& getRotationInConstrainedFrame() const { return _rotationInConstrainedFrame; }
+    const glm::quat& getVisibleRotationInConstrainedFrame() const { return _visibleRotationInConstrainedFrame; }
+
+    const bool rotationIsDefault(const glm::quat& rotation, float tolerance = EPSILON) const;
 
     const glm::vec3& getDefaultTranslationInConstrainedFrame() const;
 
@@ -104,6 +108,7 @@ private:
     glm::quat _rotation;  // joint- to model-frame
     glm::quat _rotationInConstrainedFrame; // rotation in frame where angular constraints would be applied
     glm::vec3 _positionInParentFrame; // only changes when the Model is scaled
+    float _distanceToParent;
 
     glm::mat4 _visibleTransform;
     glm::quat _visibleRotation;

interface/src/renderer/Model.cpp

@@ -676,12 +676,18 @@ bool Model::getJointState(int index, glm::quat& rotation) const {
     if (index == -1 || index >= _jointStates.size()) {
         return false;
     }
-    rotation = _jointStates.at(index).getRotationInConstrainedFrame();
-    const glm::quat& defaultRotation = _geometry->getFBXGeometry().joints.at(index).rotation;
-    return glm::abs(rotation.x - defaultRotation.x) >= EPSILON ||
-        glm::abs(rotation.y - defaultRotation.y) >= EPSILON ||
-        glm::abs(rotation.z - defaultRotation.z) >= EPSILON ||
-        glm::abs(rotation.w - defaultRotation.w) >= EPSILON;
+    const JointState& state = _jointStates.at(index);
+    rotation = state.getRotationInConstrainedFrame();
+    return !state.rotationIsDefault(rotation);
+}
+
+bool Model::getVisibleJointState(int index, glm::quat& rotation) const {
+    if (index == -1 || index >= _jointStates.size()) {
+        return false;
+    }
+    const JointState& state = _jointStates.at(index);
+    rotation = state.getVisibleRotationInConstrainedFrame();
+    return !state.rotationIsDefault(rotation);
 }
 
 void Model::setJointState(int index, bool valid, const glm::quat& rotation, float priority) {

interface/src/renderer/Model.h

@@ -113,6 +113,10 @@ public:
     /// Fetches the joint state at the specified index.
     /// \return whether or not the joint state is "valid" (that is, non-default)
     bool getJointState(int index, glm::quat& rotation) const;
+
+    /// Fetches the visible joint state at the specified index.
+    /// \return whether or not the joint state is "valid" (that is, non-default)
+    bool getVisibleJointState(int index, glm::quat& rotation) const;
     
     /// Sets the joint state at the specified index.
     void setJointState(int index, bool valid, const glm::quat& rotation = glm::quat(), float priority = 1.0f);

libraries/shared/src/Ragdoll.h

@@ -28,11 +28,7 @@ public:
     void applyAccumulatedDelta();
 
     glm::vec3 getAccumulatedDelta() const { 
-        glm::vec3 foo(0.0f); 
-        if (_numDeltas > 0) { 
-            foo = _accumulatedDelta / (float)_numDeltas; 
-        } 
-        return foo; 
+        return (_numDeltas > 0) ?  _accumulatedDelta / (float)_numDeltas : glm::vec3(0.0f);
     }
 
     glm::vec3 _position;

libraries/shared/src/SharedUtil.cpp

@@ -733,6 +733,13 @@ glm::quat rotationBetween(const glm::vec3& v1, const glm::vec3& v2) {
         }
     } else {
         axis = glm::normalize(glm::cross(v1, v2));
+        // It is possible for axis to be nan even when angle is not less than EPSILON.
+        // For example when angle is small but not tiny but v1 and v2 and have very short lengths.
+        if (glm::isnan(glm::dot(axis, axis))) {
+            // set angle and axis to values that will generate an identity rotation
+            angle = 0.0f;
+            axis = glm::vec3(1.0f, 0.0f, 0.0f);
+        }
     }
     return glm::angleAxis(angle, axis);
 }