From 30248747d6475bed432318f8fee77a7741cd8d46 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Fri, 18 Apr 2014 14:26:34 -0700
Subject: [PATCH 01/12] Provide a means of visualizing the joint constraints.

---
 interface/src/Menu.cpp                 |  1 +
 interface/src/Menu.h                   |  1 +
 interface/src/avatar/MyAvatar.cpp      |  3 ++
 interface/src/avatar/SkeletonModel.cpp | 61 ++++++++++++++++++++++++++
 interface/src/avatar/SkeletonModel.h   |  4 ++
 5 files changed, 70 insertions(+)

diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp
index b05e5c91bc..bb20a469b4 100644
--- a/interface/src/Menu.cpp
+++ b/interface/src/Menu.cpp
@@ -330,6 +330,7 @@ Menu::Menu() :
     addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::DisplayHandTargets, 0, false);
     addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::PlaySlaps, 0, false);
     addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::HandsCollideWithSelf, 0, false);
+    addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::ShowIKConstraints, 0, false);
 
     addDisabledActionAndSeparator(developerMenu, "Testing");
 
diff --git a/interface/src/Menu.h b/interface/src/Menu.h
index c17c9cc507..710cc5d78b 100644
--- a/interface/src/Menu.h
+++ b/interface/src/Menu.h
@@ -348,6 +348,7 @@ namespace MenuOption {
     const QString SettingsImport = "Import Settings";
     const QString Shadows = "Shadows";
     const QString ShowCulledSharedFaces = "Show Culled Shared Voxel Faces";
+    const QString ShowIKConstraints = "Show IK Constraints";
     const QString Stars = "Stars";
     const QString Stats = "Stats";
     const QString StopAllScripts = "Stop All Scripts";
diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 8229611646..a8d6dfa242 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -460,6 +460,9 @@ void MyAvatar::render(const glm::vec3& cameraPosition, RenderMode renderMode) {
         return; // exit early
     }
     Avatar::render(cameraPosition, renderMode);
+    if (Menu::getInstance()->isOptionChecked(MenuOption::ShowIKConstraints)) {
+        _skeletonModel.renderIKConstraints();
+    }
 }
 
 void MyAvatar::renderHeadMouse() const {
diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index b9ac280711..8d593a9c02 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -103,6 +103,11 @@ void SkeletonModel::getBodyShapes(QVector<const Shape*>& shapes) const {
     shapes.push_back(&_boundingShape);
 }
 
+void SkeletonModel::renderIKConstraints() {
+    renderJointConstraints(getRightHandJointIndex());
+    renderJointConstraints(getLeftHandJointIndex());
+}
+
 class IndexValue {
 public:
     int index;
@@ -210,3 +215,59 @@ void SkeletonModel::maybeUpdateLeanRotation(const JointState& parentState, const
         glm::normalize(inverse * axes[0])) * joint.rotation;
 }
 
+void SkeletonModel::renderJointConstraints(int jointIndex) {
+    if (jointIndex == -1) {
+        return;
+    }
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    const float BASE_DIRECTION_SIZE = 300.0f;
+    float directionSize = BASE_DIRECTION_SIZE * extractUniformScale(_scale);
+    glLineWidth(3.0f);
+    do {
+        const FBXJoint& joint = geometry.joints.at(jointIndex);
+        const JointState& jointState = _jointStates.at(jointIndex);
+        glm::vec3 position = extractTranslation(jointState.transform) + _translation;
+        
+        glPushMatrix();
+        glTranslatef(position.x, position.y, position.z);
+        glm::quat parentRotation = (joint.parentIndex == -1) ? _rotation : _jointStates.at(joint.parentIndex).combinedRotation;
+        glm::vec3 rotationAxis = glm::axis(parentRotation);
+        glRotatef(glm::degrees(glm::angle(parentRotation)), rotationAxis.x, rotationAxis.y, rotationAxis.z);
+        float fanScale = directionSize * 0.75f;
+        glScalef(fanScale, fanScale, fanScale);
+        const int AXIS_COUNT = 3;
+        for (int i = 0; i < AXIS_COUNT; i++) {
+            if (joint.rotationMin[i] <= -PI + EPSILON && joint.rotationMax[i] >= PI - EPSILON) {
+                continue; // unconstrained
+            }
+            glm::vec3 axis;
+            axis[i] = 1.0f;
+            
+            glm::vec3 otherAxis;
+            if (i == 0) {
+                otherAxis.y = 1.0f;
+            } else {
+                otherAxis.x = 1.0f;
+            }
+            glColor4f(otherAxis.r, otherAxis.g, otherAxis.b, 0.75f);
+        
+            glBegin(GL_TRIANGLE_FAN);
+            glVertex3f(0.0f, 0.0f, 0.0f);
+            const int FAN_SEGMENTS = 16;
+            for (int j = 0; j < FAN_SEGMENTS; j++) {
+                glm::vec3 rotated = glm::angleAxis(glm::mix(joint.rotationMin[i], joint.rotationMax[i],
+                    (float)j / (FAN_SEGMENTS - 1)), axis) * otherAxis;
+                glVertex3f(rotated.x, rotated.y, rotated.z);
+            }
+            glEnd();
+        }
+        glPopMatrix();
+        
+        renderOrientationDirections(position, jointState.combinedRotation, directionSize);
+        jointIndex = joint.parentIndex;
+        
+    } while (jointIndex != -1 && geometry.joints.at(jointIndex).isFree);
+    
+    glLineWidth(1.0f);
+}
+
diff --git a/interface/src/avatar/SkeletonModel.h b/interface/src/avatar/SkeletonModel.h
index 2020ccf3b2..0a87fcf89d 100644
--- a/interface/src/avatar/SkeletonModel.h
+++ b/interface/src/avatar/SkeletonModel.h
@@ -33,6 +33,8 @@ public:
     ///  \param shapes[out] list of shapes for body collisions
     void getBodyShapes(QVector<const Shape*>& shapes) const;
 
+    void renderIKConstraints();
+
 protected:
 
     void applyHandPosition(int jointIndex, const glm::vec3& position);
@@ -46,6 +48,8 @@ protected:
     virtual void maybeUpdateLeanRotation(const JointState& parentState, const FBXJoint& joint, JointState& state);
     
 private:
+
+    void renderJointConstraints(int jointIndex);
     
     Avatar* _owningAvatar;
 };

From 6add0dfb4271360d1d3dff82fbf4f61a67041934 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Fri, 18 Apr 2014 15:04:42 -0700
Subject: [PATCH 02/12] Tweaked Hydra "IK" to propagate delta rotations up the
 hierarchy while preserving joint constraints.

---
 interface/src/avatar/SkeletonModel.cpp |  7 ++++---
 interface/src/renderer/Model.cpp       | 13 ++++++++++---
 interface/src/renderer/Model.h         |  2 +-
 3 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 8d593a9c02..1c6a568754 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -138,7 +138,7 @@ void SkeletonModel::applyHandPosition(int jointIndex, const glm::vec3& position)
 
     // align hand with forearm
     float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
-    applyRotationDelta(jointIndex, rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector), false);
+    applyRotationDelta(jointIndex, rotationBetween(handRotation * glm::vec3(-sign, 0.0f, 0.0f), forearmVector));
 }
 
 void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJointIndices,
@@ -156,7 +156,7 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
     // rotate forearm to align with palm direction
     glm::quat palmRotation;
     getJointRotation(parentJointIndex, palmRotation, true);
-    applyRotationDelta(parentJointIndex, rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()), false);
+    applyRotationDelta(parentJointIndex, rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()), true, true);
     getJointRotation(parentJointIndex, palmRotation, true);
 
     // sort the finger indices by raw x, get the average direction
@@ -178,7 +178,8 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
     float directionLength = glm::length(direction);
     const unsigned int MIN_ROTATION_FINGERS = 3;
     if (directionLength > EPSILON && palm.getNumFingers() >= MIN_ROTATION_FINGERS) {
-        applyRotationDelta(parentJointIndex, rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction), false);
+        applyRotationDelta(parentJointIndex, rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction),
+            true, true);
         getJointRotation(parentJointIndex, palmRotation, true);
     }
 
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index f8de7210ea..36088dec70 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -997,9 +997,10 @@ float Model::getLimbLength(int jointIndex) const {
     return length;
 }
 
-void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain) {
+void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain, bool propagate) {
     JointState& state = _jointStates[jointIndex];
-    const FBXJoint& joint = _geometry->getFBXGeometry().joints[jointIndex];
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    const FBXJoint& joint = geometry.joints[jointIndex];
     if (!constrain || (joint.rotationMin == glm::vec3(-PI, -PI, -PI) &&
             joint.rotationMax == glm::vec3(PI, PI, PI))) {
         // no constraints
@@ -1007,10 +1008,16 @@ void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool cons
         state.combinedRotation = delta * state.combinedRotation;
         return;
     }
+    glm::quat targetRotation = delta * state.combinedRotation;
     glm::quat newRotation = glm::quat(glm::clamp(safeEulerAngles(state.rotation *
-        glm::inverse(state.combinedRotation) * delta * state.combinedRotation), joint.rotationMin, joint.rotationMax));
+        glm::inverse(state.combinedRotation) * targetRotation), joint.rotationMin, joint.rotationMax));
     state.combinedRotation = state.combinedRotation * glm::inverse(state.rotation) * newRotation;
     state.rotation = newRotation;
+    
+    if (propagate && targetRotation != state.combinedRotation &&
+            joint.parentIndex != -1 && geometry.joints[joint.parentIndex].isFree) {
+        applyRotationDelta(joint.parentIndex, targetRotation * glm::inverse(state.combinedRotation), true, true);
+    }
 }
 
 const int BALL_SUBDIVISIONS = 10;
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index 65b79fffdd..b472e17b20 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -256,7 +256,7 @@ protected:
     /// first free ancestor.
     float getLimbLength(int jointIndex) const;
 
-    void applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain = true);
+    void applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain = true, bool propagate = false);
     
 private:
     

From 743034c986fffd65bd564176239b3de019836367 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Fri, 18 Apr 2014 15:09:37 -0700
Subject: [PATCH 03/12] Need to transfer parent rotation to child.

---
 interface/src/renderer/Model.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 36088dec70..17d3dd783c 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -1017,6 +1017,7 @@ void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool cons
     if (propagate && targetRotation != state.combinedRotation &&
             joint.parentIndex != -1 && geometry.joints[joint.parentIndex].isFree) {
         applyRotationDelta(joint.parentIndex, targetRotation * glm::inverse(state.combinedRotation), true, true);
+        state.combinedRotation = _jointStates.at(joint.parentIndex).combinedRotation * state.rotation;
     }
 }
 

From e2558bdac088908ed755fcdb3d3175932567d9a6 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Fri, 18 Apr 2014 17:16:20 -0700
Subject: [PATCH 04/12] Experimenting with tweaking the Euler angles so that z,
 rather than y, is in [-pi/2, pi/2].

---
 interface/src/renderer/Model.cpp    | 11 ++++++---
 libraries/shared/src/SharedUtil.cpp | 38 ++++++++++++++++++++++++++---
 2 files changed, 43 insertions(+), 6 deletions(-)

diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 17d3dd783c..2ff8a0fee9 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -892,6 +892,11 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position, int last
         lastFreeIndex = freeLineage.last();
     }
 
+    // now update the joint states from the top
+    for (int j = freeLineage.size() - 1; j >= 0; j--) {
+        updateJointState(freeLineage.at(j));
+    }
+    
     // this is a cyclic coordinate descent algorithm: see
     // http://www.ryanjuckett.com/programming/animation/21-cyclic-coordinate-descent-in-2d
     const int ITERATION_COUNT = 1;
@@ -1009,13 +1014,13 @@ void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool cons
         return;
     }
     glm::quat targetRotation = delta * state.combinedRotation;
-    glm::quat newRotation = glm::quat(glm::clamp(safeEulerAngles(state.rotation *
-        glm::inverse(state.combinedRotation) * targetRotation), joint.rotationMin, joint.rotationMax));
+    glm::vec3 eulers = safeEulerAngles(state.rotation * glm::inverse(state.combinedRotation) * targetRotation);
+    glm::quat newRotation = glm::quat(glm::clamp(eulers, joint.rotationMin, joint.rotationMax));
     state.combinedRotation = state.combinedRotation * glm::inverse(state.rotation) * newRotation;
     state.rotation = newRotation;
     
     if (propagate && targetRotation != state.combinedRotation &&
-            joint.parentIndex != -1 && geometry.joints[joint.parentIndex].isFree) {
+            joint.parentIndex != -1 && geometry.joints.at(joint.parentIndex).isFree) {
         applyRotationDelta(joint.parentIndex, targetRotation * glm::inverse(state.combinedRotation), true, true);
         state.combinedRotation = _jointStates.at(joint.parentIndex).combinedRotation * state.rotation;
     }
diff --git a/libraries/shared/src/SharedUtil.cpp b/libraries/shared/src/SharedUtil.cpp
index f4e4b28f93..69381a07ae 100644
--- a/libraries/shared/src/SharedUtil.cpp
+++ b/libraries/shared/src/SharedUtil.cpp
@@ -640,27 +640,59 @@ void debug::checkDeadBeef(void* memoryVoid, int size) {
 // https://github.com/threerings/clyde/blob/master/src/main/java/com/threerings/math/Quaternion.java)
 glm::vec3 safeEulerAngles(const glm::quat& q) {
     float sy = 2.0f * (q.y * q.w - q.x * q.z);
+    glm::vec3 eulers;
     if (sy < 1.0f - EPSILON) {
         if (sy > -1.0f + EPSILON) {
-            return glm::vec3(
+            eulers = glm::vec3(
                 atan2f(q.y * q.z + q.x * q.w, 0.5f - (q.x * q.x + q.y * q.y)),
                 asinf(sy),
                 atan2f(q.x * q.y + q.z * q.w, 0.5f - (q.y * q.y + q.z * q.z)));
 
         } else {
             // not a unique solution; x + z = atan2(-m21, m11)
-            return glm::vec3(
+            eulers = glm::vec3(
                 0.0f,
                 - PI_OVER_TWO,
                 atan2f(q.x * q.w - q.y * q.z, 0.5f - (q.x * q.x + q.z * q.z)));
         }
     } else {
         // not a unique solution; x - z = atan2(-m21, m11)
-        return glm::vec3(
+        eulers = glm::vec3(
             0.0f,
             PI_OVER_TWO,
             -atan2f(q.x * q.w - q.y * q.z, 0.5f - (q.x * q.x + q.z * q.z)));
     }
+    
+    // adjust so that z, rather than y, is in [-pi/2, pi/2]
+    if (eulers.z < -PI_OVER_TWO) {
+        if (eulers.x < 0.0f) {
+            eulers.x += PI;
+        } else {
+            eulers.x -= PI;
+        }
+        eulers.y = -eulers.y;
+        if (eulers.y < 0.0f) {
+            eulers.y += PI;
+        } else {
+            eulers.y -= PI;
+        }
+        eulers.z += PI;
+        
+    } else if (eulers.z > PI_OVER_TWO) {
+        if (eulers.x < 0.0f) {
+            eulers.x += PI;
+        } else {
+            eulers.x -= PI;
+        }
+        eulers.y = -eulers.y;
+        if (eulers.y < 0.0f) {
+            eulers.y += PI;
+        } else {
+            eulers.y -= PI;
+        }
+        eulers.z -= PI;
+    }
+    return eulers;
 }
 
 //  Helper function returns the positive angle (in radians) between two 3D vectors

From f7bc1ae62c310de94375629d63bce7cbad257561 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Wed, 23 Apr 2014 10:47:24 -0700
Subject: [PATCH 05/12] Working on integrating rotation into IK.

---
 interface/src/avatar/SkeletonModel.cpp | 25 +++++-------------
 interface/src/renderer/Model.cpp       | 36 +++++++++++++++++---------
 interface/src/renderer/Model.h         |  5 ++--
 3 files changed, 34 insertions(+), 32 deletions(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 1c6a568754..6581bc1841 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -148,17 +148,12 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
     }
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
-    int parentJointIndex = geometry.joints.at(jointIndex).parentIndex;
-    if (parentJointIndex == -1) {
-        return;
-    }
     
-    // rotate forearm to align with palm direction
+    // rotate palm to align with palm direction
     glm::quat palmRotation;
-    getJointRotation(parentJointIndex, palmRotation, true);
-    applyRotationDelta(parentJointIndex, rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()), true, true);
-    getJointRotation(parentJointIndex, palmRotation, true);
-
+    getJointRotation(jointIndex, palmRotation, true);
+    palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()) * palmRotation;
+    
     // sort the finger indices by raw x, get the average direction
     QVector<IndexValue> fingerIndices;
     glm::vec3 direction;
@@ -178,18 +173,12 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
     float directionLength = glm::length(direction);
     const unsigned int MIN_ROTATION_FINGERS = 3;
     if (directionLength > EPSILON && palm.getNumFingers() >= MIN_ROTATION_FINGERS) {
-        applyRotationDelta(parentJointIndex, rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction),
-            true, true);
-        getJointRotation(parentJointIndex, palmRotation, true);
+        palmRotation = rotationBetween(palmRotation * glm::vec3(-sign, 0.0f, 0.0f), direction) * palmRotation;
     }
 
-    // let wrist inherit forearm rotation
-    _jointStates[jointIndex].rotation = glm::quat();
-
-    // set elbow position from wrist position
+    // set hand position, rotation
     glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f);
-    setJointPosition(parentJointIndex, palm.getPosition() + forearmVector *
-        geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale));
+    setJointPosition(jointIndex, palm.getPosition(), palmRotation);
 }
 
 void SkeletonModel::updateJointState(int index) {
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index b373ffa545..9aa5bd8227 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -882,12 +882,12 @@ bool Model::getJointRotation(int jointIndex, glm::quat& rotation, bool fromBind)
     return true;
 }
 
-bool Model::setJointPosition(int jointIndex, const glm::vec3& position, int lastFreeIndex,
-        bool allIntermediatesFree, const glm::vec3& alignment) {
+bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const glm::quat& rotation, bool useRotation,
+       int lastFreeIndex, bool allIntermediatesFree, const glm::vec3& alignment) {
     if (jointIndex == -1 || _jointStates.isEmpty()) {
         return false;
     }
-    glm::vec3 relativePosition = position - _translation;
+    glm::vec3 relativePosition = translation - _translation;
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     const QVector<int>& freeLineage = geometry.joints.at(jointIndex).freeLineage;
     if (freeLineage.isEmpty()) {
@@ -897,17 +897,20 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position, int last
         lastFreeIndex = freeLineage.last();
     }
 
-    // now update the joint states from the top
-    for (int j = freeLineage.size() - 1; j >= 0; j--) {
-        updateJointState(freeLineage.at(j));
-    }
-    
     // this is a cyclic coordinate descent algorithm: see
     // http://www.ryanjuckett.com/programming/animation/21-cyclic-coordinate-descent-in-2d
     const int ITERATION_COUNT = 1;
     glm::vec3 worldAlignment = _rotation * alignment;
     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
+        // first, try to rotate the end effector as close as possible to the target rotation, if any
+        glm::quat endRotation;
+        if (useRotation) {
+            getJointRotation(jointIndex, endRotation, true);
+            applyRotationDelta(jointIndex, rotation * glm::inverse(endRotation));
+            getJointRotation(jointIndex, endRotation, true);
+        }    
+        
+        // then, we go from the joint upwards, rotating the end as close as possible to the target
         glm::vec3 endPosition = extractTranslation(_jointStates[jointIndex].transform);
         for (int j = 1; freeLineage.at(j - 1) != lastFreeIndex; j++) {
             int index = freeLineage.at(j);
@@ -919,8 +922,17 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& position, int last
             glm::vec3 jointPosition = extractTranslation(state.transform);
             glm::vec3 jointVector = endPosition - jointPosition;
             glm::quat oldCombinedRotation = state.combinedRotation;
-            applyRotationDelta(index, rotationBetween(jointVector, relativePosition - jointPosition));
-            endPosition = state.combinedRotation * glm::inverse(oldCombinedRotation) * jointVector + jointPosition;
+            if (useRotation) {
+                applyRotationDelta(index, safeMix(rotationBetween(jointVector, relativePosition - jointPosition),
+                    rotation * glm::inverse(endRotation), 0.5f));
+                glm::quat actualDelta = state.combinedRotation * glm::inverse(oldCombinedRotation);   
+                endRotation = actualDelta * endRotation;
+                endPosition = actualDelta * jointVector + jointPosition;
+                
+            } else {
+                applyRotationDelta(index, rotationBetween(jointVector, relativePosition - jointPosition));
+                endPosition = state.combinedRotation * glm::inverse(oldCombinedRotation) * jointVector + jointPosition;
+            }
             if (alignment != glm::vec3() && j > 1) {
                 jointVector = endPosition - jointPosition;
                 glm::vec3 positionSum;
@@ -1154,7 +1166,7 @@ void Model::applyCollision(CollisionInfo& collision) {
                 getJointPosition(jointIndex, end);
                 glm::vec3 newEnd = start + glm::angleAxis(angle, axis) * (end - start);
                 // try to move it
-                setJointPosition(jointIndex, newEnd, -1, true);
+                setJointPosition(jointIndex, newEnd, glm::quat(), false, -1, true);
             }
         }
     }
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index b472e17b20..10e9e2a66a 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -240,8 +240,9 @@ protected:
     bool getJointPosition(int jointIndex, glm::vec3& position) const;
     bool getJointRotation(int jointIndex, glm::quat& rotation, bool fromBind = false) const;
     
-    bool setJointPosition(int jointIndex, const glm::vec3& position, int lastFreeIndex = -1,
-        bool allIntermediatesFree = false, const glm::vec3& alignment = glm::vec3(0.0f, -1.0f, 0.0f));
+    bool setJointPosition(int jointIndex, const glm::vec3& translation, const glm::quat& rotation = glm::quat(),
+        bool useRotation = false, int lastFreeIndex = -1, bool allIntermediatesFree = false,
+        const glm::vec3& alignment = glm::vec3(0.0f, -1.0f, 0.0f));
     bool setJointRotation(int jointIndex, const glm::quat& rotation, bool fromBind = false);
     
     void setJointTranslation(int jointIndex, const glm::vec3& translation);

From 511fc8367f425eb0c6c9f4f55221cf0747555d7b Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Wed, 23 Apr 2014 11:34:34 -0700
Subject: [PATCH 06/12] More work on IK.

---
 interface/src/avatar/SkeletonModel.cpp |  3 +--
 interface/src/renderer/Model.cpp       | 26 +++++++++++++++-----------
 2 files changed, 16 insertions(+), 13 deletions(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 6581bc1841..f52858b4ff 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -177,8 +177,7 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
     }
 
     // set hand position, rotation
-    glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f);
-    setJointPosition(jointIndex, palm.getPosition(), palmRotation);
+    setJointPosition(jointIndex, palm.getPosition(), palmRotation, true);
 }
 
 void SkeletonModel::updateJointState(int index) {
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 9aa5bd8227..77d9743cd5 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -896,6 +896,7 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const
     if (lastFreeIndex == -1) {
         lastFreeIndex = freeLineage.last();
     }
+    float baseWeight = 1.0f / (freeLineage.indexOf(lastFreeIndex) + (useRotation ? 1 : 0));
 
     // this is a cyclic coordinate descent algorithm: see
     // http://www.ryanjuckett.com/programming/animation/21-cyclic-coordinate-descent-in-2d
@@ -906,7 +907,7 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const
         glm::quat endRotation;
         if (useRotation) {
             getJointRotation(jointIndex, endRotation, true);
-            applyRotationDelta(jointIndex, rotation * glm::inverse(endRotation));
+            applyRotationDelta(jointIndex, safeMix(glm::quat(), rotation * glm::inverse(endRotation), baseWeight));
             getJointRotation(jointIndex, endRotation, true);
         }    
         
@@ -922,17 +923,13 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const
             glm::vec3 jointPosition = extractTranslation(state.transform);
             glm::vec3 jointVector = endPosition - jointPosition;
             glm::quat oldCombinedRotation = state.combinedRotation;
+            glm::quat combinedDelta;
+            float combinedWeight = 0.0f;
             if (useRotation) {
-                applyRotationDelta(index, safeMix(rotationBetween(jointVector, relativePosition - jointPosition),
-                    rotation * glm::inverse(endRotation), 0.5f));
-                glm::quat actualDelta = state.combinedRotation * glm::inverse(oldCombinedRotation);   
-                endRotation = actualDelta * endRotation;
-                endPosition = actualDelta * jointVector + jointPosition;
-                
-            } else {
-                applyRotationDelta(index, rotationBetween(jointVector, relativePosition - jointPosition));
-                endPosition = state.combinedRotation * glm::inverse(oldCombinedRotation) * jointVector + jointPosition;
+                combinedDelta = safeMix(combinedDelta, rotation * glm::inverse(endRotation), baseWeight / (combinedWeight += 1.0f));
             }
+            combinedDelta = safeMix(combinedDelta, rotationBetween(jointVector, relativePosition - jointPosition),
+                baseWeight / (combinedWeight += 1.0f));
             if (alignment != glm::vec3() && j > 1) {
                 jointVector = endPosition - jointPosition;
                 glm::vec3 positionSum;
@@ -946,9 +943,16 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const
                 glm::vec3 projectedAlignment = glm::cross(jointVector, glm::cross(worldAlignment, jointVector));
                 const float LENGTH_EPSILON = 0.001f;
                 if (glm::length(projectedCenterOfMass) > LENGTH_EPSILON && glm::length(projectedAlignment) > LENGTH_EPSILON) {
-                    applyRotationDelta(index, rotationBetween(projectedCenterOfMass, projectedAlignment));
+                    combinedDelta = safeMix(combinedDelta, rotationBetween(projectedCenterOfMass, projectedAlignment),
+                        baseWeight / (combinedWeight += 1.0f));
                 }
             }
+            applyRotationDelta(index, combinedDelta);
+            glm::quat actualDelta = state.combinedRotation * glm::inverse(oldCombinedRotation);
+            endPosition = actualDelta * jointVector + jointPosition;
+            if (useRotation) {
+                endRotation = actualDelta * endRotation;
+            }
         }       
     }
      

From 523498ee5b918b4cbc5a0930522fddd09dc51274 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Wed, 23 Apr 2014 11:52:22 -0700
Subject: [PATCH 07/12] Revert to using weight that doesn't depend on number of
 free joints.

---
 interface/src/renderer/Model.cpp | 18 ++++++++++--------
 1 file changed, 10 insertions(+), 8 deletions(-)

diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 77d9743cd5..143b6a8109 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -896,8 +896,7 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const
     if (lastFreeIndex == -1) {
         lastFreeIndex = freeLineage.last();
     }
-    float baseWeight = 1.0f / (freeLineage.indexOf(lastFreeIndex) + (useRotation ? 1 : 0));
-
+    
     // this is a cyclic coordinate descent algorithm: see
     // http://www.ryanjuckett.com/programming/animation/21-cyclic-coordinate-descent-in-2d
     const int ITERATION_COUNT = 1;
@@ -907,7 +906,7 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const
         glm::quat endRotation;
         if (useRotation) {
             getJointRotation(jointIndex, endRotation, true);
-            applyRotationDelta(jointIndex, safeMix(glm::quat(), rotation * glm::inverse(endRotation), baseWeight));
+            applyRotationDelta(jointIndex, rotation * glm::inverse(endRotation));
             getJointRotation(jointIndex, endRotation, true);
         }    
         
@@ -924,12 +923,15 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const
             glm::vec3 jointVector = endPosition - jointPosition;
             glm::quat oldCombinedRotation = state.combinedRotation;
             glm::quat combinedDelta;
-            float combinedWeight = 0.0f;
+            float combinedWeight;
             if (useRotation) {
-                combinedDelta = safeMix(combinedDelta, rotation * glm::inverse(endRotation), baseWeight / (combinedWeight += 1.0f));
+                combinedDelta = rotation * glm::inverse(endRotation);
+                combinedWeight = 1.0f;
+            } else {
+                combinedDelta = safeMix(rotation * glm::inverse(endRotation),
+                    rotationBetween(jointVector, relativePosition - jointPosition), 0.5f);
+                combinedWeight = 2.0f;
             }
-            combinedDelta = safeMix(combinedDelta, rotationBetween(jointVector, relativePosition - jointPosition),
-                baseWeight / (combinedWeight += 1.0f));
             if (alignment != glm::vec3() && j > 1) {
                 jointVector = endPosition - jointPosition;
                 glm::vec3 positionSum;
@@ -944,7 +946,7 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const
                 const float LENGTH_EPSILON = 0.001f;
                 if (glm::length(projectedCenterOfMass) > LENGTH_EPSILON && glm::length(projectedAlignment) > LENGTH_EPSILON) {
                     combinedDelta = safeMix(combinedDelta, rotationBetween(projectedCenterOfMass, projectedAlignment),
-                        baseWeight / (combinedWeight += 1.0f));
+                        1.0f / (combinedWeight + 1.0f));
                 }
             }
             applyRotationDelta(index, combinedDelta);

From 09bb51261a14b471804611403c9228a56880de7d Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Wed, 23 Apr 2014 11:55:16 -0700
Subject: [PATCH 08/12] Got that backwards.

---
 interface/src/renderer/Model.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 143b6a8109..26e59635bb 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -925,12 +925,13 @@ bool Model::setJointPosition(int jointIndex, const glm::vec3& translation, const
             glm::quat combinedDelta;
             float combinedWeight;
             if (useRotation) {
-                combinedDelta = rotation * glm::inverse(endRotation);
-                combinedWeight = 1.0f;
-            } else {
                 combinedDelta = safeMix(rotation * glm::inverse(endRotation),
                     rotationBetween(jointVector, relativePosition - jointPosition), 0.5f);
                 combinedWeight = 2.0f;
+                
+            } else {
+                combinedDelta = rotationBetween(jointVector, relativePosition - jointPosition);
+                combinedWeight = 1.0f;
             }
             if (alignment != glm::vec3() && j > 1) {
                 jointVector = endPosition - jointPosition;

From 0386dec2a67e0f8d450d7f49c8643bb82c9427ba Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Wed, 23 Apr 2014 12:09:54 -0700
Subject: [PATCH 09/12] Provide option to align wrists with forearms.

---
 interface/src/Menu.cpp                 |  1 +
 interface/src/Menu.h                   |  1 +
 interface/src/avatar/SkeletonModel.cpp | 20 ++++++++++++++++++--
 3 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/interface/src/Menu.cpp b/interface/src/Menu.cpp
index 0a0bd599eb..a7012d838d 100644
--- a/interface/src/Menu.cpp
+++ b/interface/src/Menu.cpp
@@ -336,6 +336,7 @@ Menu::Menu() :
     addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::PlaySlaps, 0, false);
     addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::HandsCollideWithSelf, 0, false);
     addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::ShowIKConstraints, 0, false);
+    addCheckableActionToQMenuAndActionHash(handOptionsMenu, MenuOption::AlignForearmsWithWrists, 0, true);
 
     addDisabledActionAndSeparator(developerMenu, "Testing");
 
diff --git a/interface/src/Menu.h b/interface/src/Menu.h
index f9f97be379..09b5fabfc8 100644
--- a/interface/src/Menu.h
+++ b/interface/src/Menu.h
@@ -256,6 +256,7 @@ private:
 
 namespace MenuOption {
     const QString AboutApp = "About Interface";
+    const QString AlignForearmsWithWrists = "Align Forearms with Wrists";
     const QString AmbientOcclusion = "Ambient Occlusion";
     const QString Atmosphere = "Atmosphere";
     const QString AudioNoiseReduction = "Audio Noise Reduction";
diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index f52858b4ff..4bb7814300 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -148,10 +148,18 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
     }
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     float sign = (jointIndex == geometry.rightHandJointIndex) ? 1.0f : -1.0f;
+    int parentJointIndex = geometry.joints.at(jointIndex).parentIndex;
+    if (parentJointIndex == -1) {
+        return;
+    }
     
     // rotate palm to align with palm direction
     glm::quat palmRotation;
-    getJointRotation(jointIndex, palmRotation, true);
+    if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
+        getJointRotation(parentJointIndex, palmRotation, true);
+    } else {
+        getJointRotation(jointIndex, palmRotation, true);
+    }
     palmRotation = rotationBetween(palmRotation * geometry.palmDirection, palm.getNormal()) * palmRotation;
     
     // sort the finger indices by raw x, get the average direction
@@ -177,7 +185,15 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
     }
 
     // set hand position, rotation
-    setJointPosition(jointIndex, palm.getPosition(), palmRotation, true);
+    if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
+        glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f);
+        setJointPosition(parentJointIndex, palm.getPosition() + forearmVector *
+            geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale), palmRotation, true);
+        _jointStates[jointIndex].rotation = glm::quat();
+        
+    } else {
+        setJointPosition(jointIndex, palm.getPosition(), palmRotation, true);
+    }
 }
 
 void SkeletonModel::updateJointState(int index) {

From 141250d51c0a2153461a3f3ce1156c11123ce21e Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Wed, 23 Apr 2014 12:22:24 -0700
Subject: [PATCH 10/12] Don't constrain elbow when rotating forearm with wrist.

---
 interface/src/avatar/SkeletonModel.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/interface/src/avatar/SkeletonModel.cpp b/interface/src/avatar/SkeletonModel.cpp
index 4bb7814300..f8ebba676f 100644
--- a/interface/src/avatar/SkeletonModel.cpp
+++ b/interface/src/avatar/SkeletonModel.cpp
@@ -188,7 +188,8 @@ void SkeletonModel::applyPalmData(int jointIndex, const QVector<int>& fingerJoin
     if (Menu::getInstance()->isOptionChecked(MenuOption::AlignForearmsWithWrists)) {
         glm::vec3 forearmVector = palmRotation * glm::vec3(sign, 0.0f, 0.0f);
         setJointPosition(parentJointIndex, palm.getPosition() + forearmVector *
-            geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale), palmRotation, true);
+            geometry.joints.at(jointIndex).distanceToParent * extractUniformScale(_scale));
+        setJointRotation(parentJointIndex, palmRotation, true);
         _jointStates[jointIndex].rotation = glm::quat();
         
     } else {

From c8c3cf3664ced00de93a0a23d059b7cbbe0c697d Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Wed, 23 Apr 2014 12:29:01 -0700
Subject: [PATCH 11/12] Remove unused propagate option for applyRotationDelta.

---
 interface/src/renderer/Model.cpp | 8 +-------
 interface/src/renderer/Model.h   | 2 +-
 2 files changed, 2 insertions(+), 8 deletions(-)

diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 26e59635bb..9c7dc9a6a1 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -1026,7 +1026,7 @@ float Model::getLimbLength(int jointIndex) const {
     return length;
 }
 
-void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain, bool propagate) {
+void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain) {
     JointState& state = _jointStates[jointIndex];
     const FBXGeometry& geometry = _geometry->getFBXGeometry();
     const FBXJoint& joint = geometry.joints[jointIndex];
@@ -1042,12 +1042,6 @@ void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool cons
     glm::quat newRotation = glm::quat(glm::clamp(eulers, joint.rotationMin, joint.rotationMax));
     state.combinedRotation = state.combinedRotation * glm::inverse(state.rotation) * newRotation;
     state.rotation = newRotation;
-    
-    if (propagate && targetRotation != state.combinedRotation &&
-            joint.parentIndex != -1 && geometry.joints.at(joint.parentIndex).isFree) {
-        applyRotationDelta(joint.parentIndex, targetRotation * glm::inverse(state.combinedRotation), true, true);
-        state.combinedRotation = _jointStates.at(joint.parentIndex).combinedRotation * state.rotation;
-    }
 }
 
 const int BALL_SUBDIVISIONS = 10;
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index 10e9e2a66a..335071d669 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -257,7 +257,7 @@ protected:
     /// first free ancestor.
     float getLimbLength(int jointIndex) const;
 
-    void applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain = true, bool propagate = false);
+    void applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain = true);
     
 private:
     

From 2bac8024079ce096462acaf8538c67aaf3bc9815 Mon Sep 17 00:00:00 2001
From: Andrzej Kapolka <drzej.k@gmail.com>
Date: Wed, 23 Apr 2014 12:30:39 -0700
Subject: [PATCH 12/12] One more unused variable.

---
 interface/src/renderer/Model.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index 9c7dc9a6a1..3484ac5fc8 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -1028,8 +1028,7 @@ float Model::getLimbLength(int jointIndex) const {
 
 void Model::applyRotationDelta(int jointIndex, const glm::quat& delta, bool constrain) {
     JointState& state = _jointStates[jointIndex];
-    const FBXGeometry& geometry = _geometry->getFBXGeometry();
-    const FBXJoint& joint = geometry.joints[jointIndex];
+    const FBXJoint& joint = _geometry->getFBXGeometry().joints[jointIndex];
     if (!constrain || (joint.rotationMin == glm::vec3(-PI, -PI, -PI) &&
             joint.rotationMax == glm::vec3(PI, PI, PI))) {
         // no constraints