Merge pull request #1226 from ey6es/master

New IK method with "gravity" for more relaxed positions, Leap integration with new skeleton.

interface/src/Menu.cpp

@@ -355,6 +355,7 @@ Menu::Menu() :
     QMenu* raveGloveOptionsMenu = developerMenu->addMenu("Rave Glove Options");
 
     addCheckableActionToQMenuAndActionHash(raveGloveOptionsMenu, MenuOption::SimulateLeapHand);
+    addCheckableActionToQMenuAndActionHash(raveGloveOptionsMenu, MenuOption::DisplayLeapHands, 0, true);
     addCheckableActionToQMenuAndActionHash(raveGloveOptionsMenu, MenuOption::TestRaveGlove);
 
     QMenu* trackingOptionsMenu = developerMenu->addMenu("Tracking Options");

interface/src/Menu.h

@@ -160,6 +160,7 @@ namespace MenuOption {
     const QString DisableConstantCulling = "Disable Constant Culling";
     const QString DisableFastVoxelPipeline = "Disable Fast Voxel Pipeline";
     const QString DisplayFrustum = "Display Frustum";
+    const QString DisplayLeapHands = "Display Leap Hands";
     const QString DontRenderVoxels = "Don't call _voxels.render()";
     const QString DontCallOpenGLForVoxels = "Don't call glDrawRangeElementsEXT() for Voxels";
     const QString EchoAudio = "Echo Audio";

interface/src/Util.cpp

@@ -162,6 +162,12 @@ glm::vec3 extractTranslation(const glm::mat4& matrix) {
     return glm::vec3(matrix[3][0], matrix[3][1], matrix[3][2]);
 }
 
+void setTranslation(glm::mat4& matrix, const glm::vec3& translation) {
+    matrix[3][0] = translation.x;
+    matrix[3][1] = translation.y;
+    matrix[3][2] = translation.z;
+}
+
 glm::quat extractRotation(const glm::mat4& matrix, bool assumeOrthogonal) {
     // uses the iterative polar decomposition algorithm described by Ken Shoemake at
     // http://www.cs.wisc.edu/graphics/Courses/838-s2002/Papers/polar-decomp.pdf

interface/src/Util.h

@@ -57,6 +57,8 @@ glm::quat safeMix(const glm::quat& q1, const glm::quat& q2, float alpha);
 
 glm::vec3 extractTranslation(const glm::mat4& matrix);
 
+void setTranslation(glm::mat4& matrix, const glm::vec3& translation);
+
 glm::quat extractRotation(const glm::mat4& matrix, bool assumeOrthogonal = false);
 
 double diffclock(timeval *clock1,timeval *clock2);

interface/src/avatar/Avatar.cpp

@@ -665,12 +665,14 @@ void Avatar::updateArmIKAndConstraints(float deltaTime, AvatarJointID fingerTipJ
     float distance = glm::length(armVector);
     
     // don't let right hand get dragged beyond maximum arm length...
-    if (distance > _maxArmLength) {
+    const float ARM_RETRACTION = 0.75f;
+    float armLength = _maxArmLength * ARM_RETRACTION;
+    if (distance > armLength) {
         // reset right hand to be constrained to maximum arm length
         fingerJoint.position = shoulderJoint.position;
         glm::vec3 armNormal = armVector / distance;
-        armVector = armNormal * _maxArmLength;
+        armVector = armNormal * armLength;
-        distance = _maxArmLength;
+        distance = armLength;
         glm::vec3 constrainedPosition = shoulderJoint.position;
         constrainedPosition += armVector;
         fingerJoint.position = constrainedPosition;

interface/src/avatar/Hand.cpp

@@ -11,11 +11,10 @@
 #include "Application.h"
 #include "Avatar.h"
 #include "Hand.h"
+#include "Menu.h"
 #include "Util.h"
 #include "renderer/ProgramObject.h"
 
-const bool SHOW_LEAP_HAND = true;
-
 using namespace std;
 
 Hand::Hand(Avatar* owningAvatar) :
@@ -139,7 +138,7 @@ void Hand::render(bool lookingInMirror) {
     
     calculateGeometry();
 
-    if ( SHOW_LEAP_HAND ) {
+    if (Menu::getInstance()->isOptionChecked(MenuOption::DisplayLeapHands)) {
         if (!isRaveGloveActive()) {
             renderLeapFingerTrails();
         }

interface/src/avatar/MyAvatar.cpp

@@ -603,13 +603,6 @@ float MyAvatar::getBallRenderAlpha(int ball, bool lookingInMirror) const {
 
 void MyAvatar::renderBody(bool lookingInMirror, bool renderAvatarBalls) {
 
-    if (Application::getInstance()->getCamera()->getMode() == CAMERA_MODE_FIRST_PERSON && !lookingInMirror) {
-        // Dont display body, only the hand        
-        _hand.render(lookingInMirror);
-        
-        return;
-    }
-    
     if (_head.getVideoFace().isFullFrame()) {
         //  Render the full-frame video
         float alpha = getBallRenderAlpha(BODY_BALL_HEAD_BASE, lookingInMirror);
@@ -684,11 +677,11 @@ void MyAvatar::renderBody(bool lookingInMirror, bool renderAvatarBalls) {
         }
     } else {
         //  Render the body's voxels and head
+        if (!_skeletonModel.render(1.0f)) {
+            _voxels.render(false);
+        }
         float alpha = getBallRenderAlpha(BODY_BALL_HEAD_BASE, lookingInMirror);
         if (alpha > 0.0f) {
-            if (!_skeletonModel.render(alpha)) {
-                _voxels.render(false);
-            }
             _head.render(alpha, true);
         }
     }

interface/src/avatar/SkeletonModel.cpp

@@ -27,12 +27,50 @@ void SkeletonModel::simulate(float deltaTime) {
     setScale(glm::vec3(1.0f, 1.0f, 1.0f) * _owningAvatar->getScale() * MODEL_SCALE);
     
     Model::simulate(deltaTime);
+
+    // find the left and rightmost active Leap palms
+    HandData& hand = _owningAvatar->getHand();
+    int leftPalmIndex = -1;
+    float leftPalmX = FLT_MAX;
+    int rightPalmIndex = -1;    
+    float rightPalmX = -FLT_MAX;
+    for (int i = 0; i < hand.getNumPalms(); i++) {
+        if (hand.getPalms()[i].isActive()) {
+            float x = hand.getPalms()[i].getRawPosition().x;
+            if (x < leftPalmX) {
+                leftPalmIndex = i;
+                leftPalmX = x;
+            }
+            if (x > rightPalmX) {
+                rightPalmIndex = i;
+                rightPalmX = x;
+            }
+        }
+    }
     
-    if (_owningAvatar->getHandState() == HAND_STATE_NULL) {
+    const float HAND_RESTORATION_RATE = 0.25f;
-        const float HAND_RESTORATION_RATE = 0.25f;
+    
-        restoreRightHandPosition(HAND_RESTORATION_RATE);
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    if (leftPalmIndex == -1) {
+        // no Leap data; set hands from mouse
+        if (_owningAvatar->getHandState() == HAND_STATE_NULL) {
+            restoreRightHandPosition(HAND_RESTORATION_RATE);
+        } else {
+            setRightHandPosition(_owningAvatar->getHandPosition());
+        }
+        restoreLeftHandPosition(HAND_RESTORATION_RATE);
+    
+    } else if (leftPalmIndex == rightPalmIndex) {
+        // right hand only
+        applyPalmData(geometry.rightHandJointIndex, geometry.rightFingerJointIndices, geometry.rightFingertipJointIndices, 
+            hand.getPalms()[leftPalmIndex]);
+        restoreLeftHandPosition(HAND_RESTORATION_RATE);
+        
     } else {
-        setRightHandPosition(_owningAvatar->getHandPosition());
+        applyPalmData(geometry.leftHandJointIndex, geometry.leftFingerJointIndices, geometry.leftFingertipJointIndices,
+            hand.getPalms()[leftPalmIndex]);
+        applyPalmData(geometry.rightHandJointIndex, geometry.rightFingerJointIndices, geometry.rightFingertipJointIndices,
+            hand.getPalms()[rightPalmIndex]);
     }
 }
 
@@ -86,6 +124,67 @@ bool SkeletonModel::render(float alpha) {
     return true;
 }
 
+class IndexValue {
+public:
+    int index;
+    float value;
+};
+
+bool operator<(const IndexValue& firstIndex, const IndexValue& secondIndex) {
+    return firstIndex.value < secondIndex.value;
+}
+
+void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJointIndices,
+        const QVector<int>& fingertipJointIndices, PalmData& palm) {
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    setJointPosition(jointIndex, palm.getPosition());
+    float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
+    glm::quat palmRotation = rotationBetween(_rotation * IDENTITY_UP, -palm.getNormal()) * _rotation *
+        glm::angleAxis(90.0f, 0.0f, sign, 0.0f); // ninety degree rotation to face fingers forward from bind pose
+    
+    // sort the finger indices by raw x, get the average direction
+    QVector<IndexValue> fingerIndices;
+    glm::vec3 direction;
+    for (int i = 0; i < palm.getNumFingers(); i++) {
+        glm::vec3 fingerVector = palm.getFingers()[i].getTipPosition() - palm.getPosition();
+        float length = glm::length(fingerVector);
+        if (length > EPSILON) {
+            direction += fingerVector / length;
+        }
+        fingerVector = glm::inverse(palmRotation) * fingerVector * -sign;
+        IndexValue indexValue = { i, atan2f(fingerVector.z, fingerVector.x) };
+        fingerIndices.append(indexValue);
+    }
+    qSort(fingerIndices.begin(), fingerIndices.end());
+    
+    // rotate palm according to average finger direction
+    float directionLength = glm::length(direction);
+    if (directionLength > EPSILON) {
+        palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation;
+    }
+    setJointRotation(jointIndex, palmRotation, true);
+    
+    // no point in continuing if there are no fingers
+    if (palm.getNumFingers() == 0 || fingerJointIndices.isEmpty()) {
+        return;
+    }
+     
+    // match them up as best we can
+    float proportion = fingerIndices.size() / (float)fingerJointIndices.size();
+    for (int i = 0; i < fingerJointIndices.size(); i++) {
+        int fingerIndex = fingerIndices.at(roundf(i * proportion)).index;
+        glm::vec3 fingerVector = palm.getFingers()[fingerIndex].getTipPosition() -
+            palm.getFingers()[fingerIndex].getRootPosition();
+        
+        int fingerJointIndex = fingerJointIndices.at(i);
+        int fingertipJointIndex = fingertipJointIndices.at(i);
+        glm::vec3 jointVector = extractTranslation(geometry.joints.at(fingertipJointIndex).bindTransform) -
+            extractTranslation(geometry.joints.at(fingerJointIndex).bindTransform);
+        
+        setJointRotation(fingerJointIndex, rotationBetween(palmRotation * jointVector, fingerVector) * palmRotation, true);
+    }
+}
+
 void SkeletonModel::updateJointState(int index) {
     Model::updateJointState(index);
     

interface/src/avatar/SkeletonModel.h

@@ -9,6 +9,8 @@
 #ifndef __interface__SkeletonModel__
 #define __interface__SkeletonModel__
 
+#include <HandData.h>
+
 #include "renderer/Model.h"
 
 class Avatar;
@@ -26,6 +28,9 @@ public:
     
 protected:
     
+    void applyPalmData(int jointIndex, const QVector<int>& fingerJointIndices,
+        const QVector<int>& fingertipJointIndices, PalmData& palm);
+    
     /// Updates the state of the joint at the specified index.
     virtual void updateJointState(int index);   
     

interface/src/devices/LeapManager.cpp

@@ -198,8 +198,8 @@ void LeapManager::nextFrame(Avatar& avatar) {
             // There's no real Leap data and we need to fake it.
             for (size_t i = 0; i < hand.getNumPalms(); ++i) {
                 static const glm::vec3 fakeHandOffsets[] = {
-                    glm::vec3( -500.0f, 50.0f, 50.0f),
+                    glm::vec3( -250.0f, 50.0f, 50.0f),
-                    glm::vec3(    0.0f, 50.0f, 50.0f)
+                    glm::vec3(  250.0f, 50.0f, 50.0f)
                 };
                 static const glm::vec3 fakeHandFingerMirrors[] = {
                     glm::vec3( -1.0f, 1.0f, 1.0f),
@@ -218,7 +218,7 @@ void LeapManager::nextFrame(Avatar& avatar) {
                 // Simulated data
                 
                 palm.setRawPosition(glm::vec3( 0.0f, 0.0f, 0.0f) + fakeHandOffsets[i]);
-                palm.setRawNormal(glm::vec3(0.0f, 1.0f, 0.0f));
+                palm.setRawNormal(glm::vec3(0.0f, -1.0f, 0.0f));
 
                 for (size_t f = 0; f < palm.getNumFingers(); ++f) {
                     FingerData& finger = palm.getFingers()[f];

interface/src/renderer/FBXReader.cpp

@@ -726,6 +726,17 @@ void setTangents(FBXMesh& mesh, int firstIndex, int secondIndex) {
         -glm::degrees(atan2f(-texCoordDelta.t, texCoordDelta.s)), normal) * glm::normalize(bitangent), normal);
 }
 
+QVector<int> getIndices(const QVector<QString> ids, QVector<QString> modelIDs) {
+    QVector<int> indices;
+    foreach (const QString& id, ids) {
+        int index = modelIDs.indexOf(id);
+        if (index != -1) {
+            indices.append(index);
+        }
+    }
+    return indices;
+}
+
 FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping) {
     QHash<QString, ExtractedMesh> meshes;
     QVector<ExtractedBlendshape> blendshapes;
@@ -747,6 +758,10 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
     QString jointHeadName = processID(joints.value("jointHead", "jointHead").toString());
     QString jointLeftHandName = processID(joints.value("jointLeftHand", "jointLeftHand").toString());
     QString jointRightHandName = processID(joints.value("jointRightHand", "jointRightHand").toString());
+    QVariantList jointLeftFingerNames = joints.values("jointLeftFinger");
+    QVariantList jointRightFingerNames = joints.values("jointRightFinger");
+    QVariantList jointLeftFingertipNames = joints.values("jointLeftFingertip");
+    QVariantList jointRightFingertipNames = joints.values("jointRightFingertip");
     QString jointEyeLeftID;
     QString jointEyeRightID;
     QString jointNeckID;
@@ -755,6 +770,10 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
     QString jointHeadID;
     QString jointLeftHandID;
     QString jointRightHandID;
+    QVector<QString> jointLeftFingerIDs(jointLeftFingerNames.size());
+    QVector<QString> jointRightFingerIDs(jointRightFingerNames.size());
+    QVector<QString> jointLeftFingertipIDs(jointLeftFingertipNames.size());
+    QVector<QString> jointRightFingertipIDs(jointRightFingertipNames.size());
     
     QVariantHash blendshapeMappings = mapping.value("bs").toHash();
     QHash<QByteArray, QPair<int, float> > blendshapeIndices;
@@ -811,6 +830,7 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                     } else {
                         name = getID(object.properties);
                     }
+                    int index;
                     if (name == jointEyeLeftName || name == "EyeL" || name == "joint_Leye") {
                         jointEyeLeftID = getID(object.properties);
                         
@@ -834,6 +854,18 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                         
                     } else if (name == jointRightHandName) {
                         jointRightHandID = getID(object.properties);
+                        
+                    } else if ((index = jointLeftFingerNames.indexOf(name)) != -1) {
+                        jointLeftFingerIDs[index] = getID(object.properties);
+                        
+                    } else if ((index = jointRightFingerNames.indexOf(name)) != -1) {
+                        jointRightFingerIDs[index] = getID(object.properties);
+                    
+                    } else if ((index = jointLeftFingertipNames.indexOf(name)) != -1) {
+                        jointLeftFingertipIDs[index] = getID(object.properties);
+                        
+                    } else if ((index = jointRightFingertipNames.indexOf(name)) != -1) {
+                        jointRightFingertipIDs[index] = getID(object.properties);
                     }
                     glm::vec3 translation;
                     glm::vec3 rotationOffset;
@@ -1063,14 +1095,18 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
         glm::quat combinedRotation = model.preRotation * model.rotation * model.postRotation;
         if (joint.parentIndex == -1) {    
             joint.transform = geometry.offset * model.preTransform * glm::mat4_cast(combinedRotation) * model.postTransform;
-            joint.inverseBindRotation = glm::inverse(combinedRotation);
+            joint.inverseDefaultRotation = glm::inverse(combinedRotation);
+            joint.distanceToParent = 0.0f;
             
         } else {
             const FBXJoint& parentJoint = geometry.joints.at(joint.parentIndex);
             joint.transform = parentJoint.transform *
                 model.preTransform * glm::mat4_cast(combinedRotation) * model.postTransform;
-            joint.inverseBindRotation = glm::inverse(combinedRotation) * parentJoint.inverseBindRotation;
+            joint.inverseDefaultRotation = glm::inverse(combinedRotation) * parentJoint.inverseDefaultRotation;
+            joint.distanceToParent = glm::distance(extractTranslation(parentJoint.transform),
+                extractTranslation(joint.transform));
         }
+        joint.inverseBindRotation = joint.inverseDefaultRotation;
         geometry.joints.append(joint);
         geometry.jointIndices.insert(model.name, geometry.joints.size() - 1);  
     }
@@ -1084,6 +1120,10 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
     geometry.headJointIndex = modelIDs.indexOf(jointHeadID);
     geometry.leftHandJointIndex = modelIDs.indexOf(jointLeftHandID);
     geometry.rightHandJointIndex = modelIDs.indexOf(jointRightHandID);
+    geometry.leftFingerJointIndices = getIndices(jointLeftFingerIDs, modelIDs);
+    geometry.rightFingerJointIndices = getIndices(jointRightFingerIDs, modelIDs);
+    geometry.leftFingertipJointIndices = getIndices(jointLeftFingertipIDs, modelIDs);
+    geometry.rightFingertipJointIndices = getIndices(jointRightFingertipIDs, modelIDs);
     
     // extract the translation component of the neck transform
     if (geometry.neckJointIndex != -1) {
@@ -1182,6 +1222,11 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                 }
                 fbxCluster.inverseBindMatrix = glm::inverse(cluster.transformLink) * modelTransform;
                 extracted.mesh.clusters.append(fbxCluster);
+                
+                // override the bind rotation with the transform link
+                FBXJoint& joint = geometry.joints[fbxCluster.jointIndex];
+                joint.inverseBindRotation = glm::inverse(extractRotation(cluster.transformLink));
+                joint.bindTransform = cluster.transformLink;
             }
         }
         

interface/src/renderer/FBXReader.h

@@ -46,13 +46,16 @@ public:
     bool isFree;
     QVector<int> freeLineage;
     int parentIndex;
+    float distanceToParent;
     glm::mat4 preTransform;
     glm::quat preRotation;
     glm::quat rotation;
     glm::quat postRotation;
     glm::mat4 postTransform;
     glm::mat4 transform;
+    glm::quat inverseDefaultRotation;
     glm::quat inverseBindRotation;
+    glm::mat4 bindTransform;
 };
 
 /// A single binding to a joint in an FBX document.
@@ -134,6 +137,12 @@ public:
     int leftHandJointIndex;
     int rightHandJointIndex;
     
+    QVector<int> leftFingerJointIndices;
+    QVector<int> rightFingerJointIndices;
+    
+    QVector<int> leftFingertipJointIndices;
+    QVector<int> rightFingertipJointIndices;
+    
     glm::vec3 neckPivot;
     
     QVector<FBXAttachment> attachments;

interface/src/renderer/Model.cpp

@@ -485,6 +485,10 @@ bool Model::setLeftHandRotation(const glm::quat& rotation) {
     return isActive() && setJointRotation(_geometry->getFBXGeometry().leftHandJointIndex, rotation);
 }
 
+float Model::getLeftArmLength() const {
+    return isActive() ? getLimbLength(_geometry->getFBXGeometry().leftHandJointIndex) : 0.0f;
+}
+
 bool Model::setRightHandPosition(const glm::vec3& position) {
     return isActive() && setJointPosition(_geometry->getFBXGeometry().rightHandJointIndex, position);
 }
@@ -497,6 +501,10 @@ bool Model::setRightHandRotation(const glm::quat& rotation) {
     return isActive() && setJointRotation(_geometry->getFBXGeometry().rightHandJointIndex, rotation);
 }
 
+float Model::getRightArmLength() const {
+    return isActive() ? getLimbLength(_geometry->getFBXGeometry().rightHandJointIndex) : 0.0f;
+}
+
 void Model::setURL(const QUrl& url) {
     // don't recreate the geometry if it's the same URL
     if (_url == url) {
@@ -566,47 +574,96 @@ bool Model::getJointPosition(int jointIndex, glm::vec3& position) const {
     return true;
 }
 
-bool Model::getJointRotation(int jointIndex, glm::quat& rotation) const {
+bool Model::getJointRotation(int jointIndex, glm::quat& rotation, bool fromBind) const {
     if (jointIndex == -1 || _jointStates.isEmpty()) {
         return false;
     }
     rotation = _jointStates[jointIndex].combinedRotation *
-        _geometry->getFBXGeometry().joints[jointIndex].inverseBindRotation;
+        (fromBind ? _geometry->getFBXGeometry().joints[jointIndex].inverseBindRotation :
+            _geometry->getFBXGeometry().joints[jointIndex].inverseDefaultRotation);
     return true;
 }
 
-bool Model::setJointPosition(int jointIndex, const glm::vec3& position) {
+void Model::setJointTranslation(int jointIndex, int parentIndex, int childIndex, const glm::vec3& translation) {
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    JointState& state = _jointStates[jointIndex];
+    if (childIndex != -1 && geometry.joints.at(jointIndex).isFree) {
+        // if there's a child, then I must adjust *my* rotation
+        glm::vec3 childTranslation = extractTranslation(_jointStates.at(childIndex).transform);
+        glm::quat delta = rotationBetween(childTranslation - extractTranslation(state.transform),
+            childTranslation - translation);
+        state.rotation = state.rotation * glm::inverse(state.combinedRotation) * delta * state.combinedRotation;
+        state.combinedRotation = delta * state.combinedRotation;
+    }
+    if (parentIndex != -1 && geometry.joints.at(parentIndex).isFree) {
+        // if there's a parent, then I must adjust *its* rotation
+        JointState& parent = _jointStates[parentIndex];
+        glm::vec3 parentTranslation = extractTranslation(parent.transform);
+        glm::quat delta = rotationBetween(extractTranslation(state.transform) - parentTranslation, 
+            translation - parentTranslation);
+        parent.rotation = parent.rotation * glm::inverse(parent.combinedRotation) * delta * parent.combinedRotation;
+        parent.combinedRotation = delta * parent.combinedRotation;
+    }
+    ::setTranslation(state.transform, translation);
+}
+
+bool Model::setJointPosition(int jointIndex, const glm::vec3& position, int lastFreeIndex) {
     if (jointIndex == -1 || _jointStates.isEmpty()) {
         return false;
     }
     glm::vec3 relativePosition = position - _translation;
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     const QVector<int>& freeLineage = geometry.joints.at(jointIndex).freeLineage;
+    if (lastFreeIndex == -1) {
+        lastFreeIndex = freeLineage.last();
+    }
     
-    // this is a cyclic coordinate descent algorithm: see
+    // this is a constraint relaxation algorithm: see
-    // http://www.ryanjuckett.com/programming/animation/21-cyclic-coordinate-descent-in-2d
+    // http://www.ryanjuckett.com/programming/animation/22-constraint-relaxation-ik-in-2d
-    const int ITERATION_COUNT = 1;
+    
+    // the influence of gravity; lowers the potential energy of our configurations
+    glm::vec3 gravity = _rotation * IDENTITY_UP * -0.01f;
+    
+    // over one or more iterations, apply the length constraints and update the rotations accordingly
+    float uniformScale = (_scale.x + _scale.y + _scale.z) / 3.0f;
+    const int ITERATION_COUNT = 3;
     for (int i = 0; i < ITERATION_COUNT; i++) {
-        // first, we go from the joint upwards, rotating the end as close as possible to the target
+        // start by optimistically setting the position of the end joint to our target
-        glm::vec3 endPosition = extractTranslation(_jointStates[jointIndex].transform);
+        setJointTranslation(jointIndex, freeLineage.at(1), -1, relativePosition);
-        for (int j = 1; j < freeLineage.size(); j++) {
+    
-            int index = freeLineage.at(j);
+        for (int j = 1; freeLineage.at(j - 1) != lastFreeIndex; j++) {
-            if (glm::distance(endPosition, relativePosition) < EPSILON) {
+            int sourceIndex = freeLineage.at(j);
-                return true; // close enough to target position
+            int destIndex = freeLineage.at(j - 1);
-            }
+            JointState& sourceState = _jointStates[sourceIndex];
-            const FBXJoint& joint = geometry.joints.at(index);
+            JointState& destState = _jointStates[destIndex];
-            if (!joint.isFree) {
+            glm::vec3 sourceTranslation = extractTranslation(sourceState.transform);
+            glm::vec3 destTranslation = extractTranslation(destState.transform);
+            glm::vec3 boneVector = destTranslation - sourceTranslation;
+            float boneLength = glm::length(boneVector);
+            if (boneLength < EPSILON) {
                 continue;
             }
-            JointState& state = _jointStates[index];
+            float extension = geometry.joints.at(destIndex).distanceToParent * uniformScale / boneLength - 1.0f;
-            glm::vec3 jointPosition = extractTranslation(state.transform);
+            if (fabs(extension) < EPSILON) {
-            glm::vec3 jointVector = endPosition - jointPosition;
+                continue;
-            glm::quat deltaRotation = rotationBetween(jointVector, relativePosition - jointPosition);
+            }
-            state.rotation = state.rotation * glm::inverse(state.combinedRotation) * deltaRotation * state.combinedRotation;
+            if (j == 1) {
-            endPosition = deltaRotation * jointVector + jointPosition;
+                setJointTranslation(sourceIndex, freeLineage.at(j + 1), -1,
+                    sourceTranslation - boneVector * extension + gravity);
+            
+            } else if (sourceIndex == lastFreeIndex) {
+                setJointTranslation(destIndex, -1, freeLineage.at(j - 2),
+                    destTranslation + boneVector * extension + gravity);
+
+            } else {
+                setJointTranslation(sourceIndex, freeLineage.at(j + 1), -1,
+                    sourceTranslation - boneVector * extension * 0.5f + gravity);
+                setJointTranslation(destIndex, -1, freeLineage.at(j - 2),
+                    destTranslation + boneVector * extension * 0.5f + gravity);
+            }
         }
         
-        // then, from the first free joint downwards, update the transforms again
+        // now update the joint states from the top
         for (int j = freeLineage.size() - 1; j >= 0; j--) {
             updateJointState(freeLineage.at(j));
         }
@@ -615,13 +672,14 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position) {
     return true;
 }
 
-bool Model::setJointRotation(int jointIndex, const glm::quat& rotation) {
+bool Model::setJointRotation(int jointIndex, const glm::quat& rotation, bool fromBind) {
     if (jointIndex == -1 || _jointStates.isEmpty()) {
         return false;
     }
     JointState& state = _jointStates[jointIndex];
     state.rotation = state.rotation * glm::inverse(state.combinedRotation) * rotation *
-        glm::inverse(_geometry->getFBXGeometry().joints.at(jointIndex).inverseBindRotation);
+        glm::inverse(fromBind ? _geometry->getFBXGeometry().joints.at(jointIndex).inverseBindRotation :
+            _geometry->getFBXGeometry().joints.at(jointIndex).inverseDefaultRotation);
     return true;
 }
 
@@ -638,6 +696,19 @@ bool Model::restoreJointPosition(int jointIndex, float percent) {
     return true;
 }
 
+float Model::getLimbLength(int jointIndex) const {
+    if (jointIndex == -1 || _jointStates.isEmpty()) {
+        return 0.0f;
+    }
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    const QVector<int>& freeLineage = geometry.joints.at(jointIndex).freeLineage;
+    int length = 0.0f;
+    for (int i = freeLineage.size() - 2; i >= 0; i--) {
+        length += geometry.joints.at(freeLineage.at(i)).distanceToParent;
+    }
+    return length;
+}
+
 void Model::deleteGeometry() {
     foreach (Model* attachment, _attachments) {
         delete attachment;

interface/src/renderer/Model.h

@@ -83,6 +83,9 @@ public:
     /// \return whether or not the left hand joint was found
     bool setLeftHandRotation(const glm::quat& rotation);
     
+    /// Returns the extended length from the left hand to its first free ancestor.
+    float getLeftArmLength() const;
+    
     /// Sets the position of the right hand using inverse kinematics.
     /// \return whether or not the right hand joint was found
     bool setRightHandPosition(const glm::vec3& position);
@@ -96,6 +99,9 @@ public:
     /// \return whether or not the right hand joint was found
     bool setRightHandRotation(const glm::quat& rotation);
     
+    /// Returns the extended length from the right hand to its first free ancestor.
+    float getRightArmLength() const;
+    
     /// Returns the average color of all meshes in the geometry.
     glm::vec4 computeAverageColor() const;
 
@@ -135,10 +141,10 @@ protected:
     virtual void maybeUpdateEyeRotation(const JointState& parentState, const FBXJoint& joint, JointState& state);
     
     bool getJointPosition(int jointIndex, glm::vec3& position) const;
-    bool getJointRotation(int jointIndex, glm::quat& rotation) const;
+    bool getJointRotation(int jointIndex, glm::quat& rotation, bool fromBind = false) const;
     
-    bool setJointPosition(int jointIndex, const glm::vec3& position);
+    bool setJointPosition(int jointIndex, const glm::vec3& position, int lastFreeIndex = -1);
-    bool setJointRotation(int jointIndex, const glm::quat& rotation);
+    bool setJointRotation(int jointIndex, const glm::quat& rotation, bool fromBind = false);
     
     /// Restores the indexed joint to its default position.
     /// \param percent the percentage of the default position to apply (i.e., 0.25f to slerp one fourth of the way to
@@ -146,8 +152,14 @@ protected:
     /// \return true if the joint was found
     bool restoreJointPosition(int jointIndex, float percent = 1.0f);
     
+    /// Computes and returns the extended length of the limb terminating at the specified joint and starting at the joint's
+    /// first free ancestor.
+    float getLimbLength(int jointIndex) const;
+    
 private:
     
+    void setJointTranslation(int jointIndex, int parentIndex, int childIndex, const glm::vec3& translation);
+    
     void deleteGeometry();
     
     float _pupilDilation;