From 559188caced5c9cb3c2bb3eeb993588c1e018605 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Wed, 23 Jul 2014 17:00:17 -0700
Subject: [PATCH] parent-relative linear slaving of verlet shapes

---
 interface/src/avatar/SkeletonModel.cpp | 57 ++++++++++++++++++++------
 1 file changed, 44 insertions(+), 13 deletions(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 59f39834e3..1a68b86a14 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/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,51 @@ 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;
+    }
+
+    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();
+    //}
+    // 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;
+        }
+
+        const JointState& parentState = _jointStates.at(p);
+        glm::vec3 targetBone = state.getPosition() - parentState.getPosition();
+        glm::vec3 bone = _ragdollPoints.at(i)._lastPosition - _ragdollPoints.at(p)._lastPosition;
+
+        glm::vec3 newBone = (1.0f - fraction) * bone + fraction * targetBone;
+        float boneLength = glm::length(newBone);
+        if (boneLength > EPSILON) {
+            // slam newBone's lenght to that of the joint, which helps maintain distance constraints
+            newBone *= state.getDistanceToParent() / boneLength;
+        }
+        // we set the new position relative to parent
+        _ragdollPoints[i]._position = _ragdollPoints.at(p)._position + newBone;
     }
 }