From cdae16e07bd2e8649d310b7c5e0e2a49e5075d20 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 29 Sep 2015 16:11:27 -0700
Subject: [PATCH 1/3] fix bug: IK attenuates fast underpose animations

---
 .../animation/src/AnimInverseKinematics.cpp   | 61 ++++++++++++-------
 .../animation/src/AnimInverseKinematics.h     |  2 +-
 2 files changed, 39 insertions(+), 24 deletions(-)

diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index de226092f1..ff5d2a6a3f 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -89,7 +89,7 @@ static int findRootJointInSkeleton(AnimSkeleton::ConstPointer skeleton, int inde
     return rootIndex;
 }
 
-void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets) {
+void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses) {
     // build a list of valid targets from _targetVarVec and animVars
     _maxTargetIndex = -1;
     bool removeUnfoundJoints = false;
@@ -107,7 +107,7 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::
             }
         } else {
             IKTarget target;
-            AnimPose defaultPose = _skeleton->getAbsolutePose(targetVar.jointIndex, _relativePoses);
+            AnimPose defaultPose = _skeleton->getAbsolutePose(targetVar.jointIndex, underPoses);
             target.pose.trans = animVars.lookup(targetVar.positionVar, defaultPose.trans);
             target.pose.rot = animVars.lookup(targetVar.rotationVar, defaultPose.rot);
             target.setType(animVars.lookup(targetVar.typeVar, QString("")));
@@ -154,7 +154,6 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
     do {
         int lowestMovedIndex = _relativePoses.size();
         for (auto& target: targets) {
-            int tipIndex = target.index;
             if (target.type == IKTarget::Type::RotationOnly) {
                 // the final rotation will be enforced after the iterations
                 continue;
@@ -162,6 +161,7 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
             AnimPose targetPose = target.pose;
 
             // cache tip absolute transform
+            int tipIndex = target.index;
             glm::vec3 tipPosition = absolutePoses[tipIndex].trans;
             glm::quat tipRotation = absolutePoses[tipIndex].rot;
 
@@ -288,6 +288,17 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
         }
     } while (numLoops < MAX_IK_LOOPS);
 
+    /* KEEP: example code for measuring endeffector error of IK solution
+    for (uint32_t i = 0; i < targets.size(); ++i) {
+        auto& target = targets[i];
+        if (target.type == IKTarget::Type::RotationOnly) {
+            continue;
+        }
+        glm::vec3 tipPosition = absolutePoses[target.index].trans;
+        std::cout << i << " IK error = " << glm::distance(tipPosition, target.pose.trans) << std::endl;  // adebug
+    }
+    */
+
     // finally set the relative rotation of each tip to agree with absolute target rotation
     for (auto& target: targets) {
         int tipIndex = target.index;
@@ -312,25 +323,8 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
 
 //virtual
 const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) {
-    if (!_relativePoses.empty()) {
-        // build a list of targets from _targetVarVec
-        std::vector<IKTarget> targets;
-        computeTargets(animVars, targets);
-    
-        if (targets.empty()) {
-            // no IK targets but still need to enforce constraints
-            std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();
-            while (constraintItr != _constraints.end()) {
-                int index = constraintItr->first;
-                glm::quat rotation = _relativePoses[index].rot;
-                constraintItr->second->apply(rotation);
-                _relativePoses[index].rot = rotation;
-                ++constraintItr;
-            }
-        } else {
-            solveWithCyclicCoordinateDescent(targets);
-        }
-    }
+    // don't call this function, call overlay() instead
+    assert(false);
     return _relativePoses;
 }
 
@@ -356,7 +350,28 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
             }
         }
     }
-    return evaluate(animVars, dt, triggersOut);
+
+    if (!_relativePoses.empty()) {
+        // build a list of targets from _targetVarVec
+        std::vector<IKTarget> targets;
+        computeTargets(animVars, targets, underPoses);
+    
+        if (targets.empty()) {
+            // no IK targets but still need to enforce constraints
+            std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();
+            while (constraintItr != _constraints.end()) {
+                int index = constraintItr->first;
+                glm::quat rotation = _relativePoses[index].rot;
+                constraintItr->second->apply(rotation);
+                _relativePoses[index].rot = rotation;
+                ++constraintItr;
+            }
+        } else {
+            solveWithCyclicCoordinateDescent(targets);
+        }
+    }
+    return _relativePoses;
+    //return underPoses;
 }
 
 RotationConstraint* AnimInverseKinematics::getConstraint(int index) {
diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h
index 121b35bd9d..2eb0d9b08e 100644
--- a/libraries/animation/src/AnimInverseKinematics.h
+++ b/libraries/animation/src/AnimInverseKinematics.h
@@ -50,7 +50,7 @@ protected:
         void setType(const QString& typeVar) { type = ((typeVar == "RotationOnly") ?  Type::RotationOnly : Type::RotationAndPosition); }
     };
 
-    void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets);
+    void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses);
     void solveWithCyclicCoordinateDescent(const std::vector<IKTarget>& targets);
     virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton);
 

From a052f5e125f7d75fb4d36929c05b2ba2cc6e28df Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 29 Sep 2015 16:21:08 -0700
Subject: [PATCH 2/3] remove commented out debug line

---
 libraries/animation/src/AnimInverseKinematics.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index ff5d2a6a3f..c0adba0ad6 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -371,7 +371,6 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
         }
     }
     return _relativePoses;
-    //return underPoses;
 }
 
 RotationConstraint* AnimInverseKinematics::getConstraint(int index) {

From b192d0a9cdfdada443fc3957804287cfff0d08ab Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 29 Sep 2015 17:39:17 -0700
Subject: [PATCH 3/3] make neck IK target type same as head

---
 .../meshes/defaultAvatar_full/avatar-animation.json          | 5 +++--
 libraries/animation/src/Rig.cpp                              | 4 ++--
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/interface/resources/meshes/defaultAvatar_full/avatar-animation.json b/interface/resources/meshes/defaultAvatar_full/avatar-animation.json
index 72eefaf7e8..dceecddfe0 100644
--- a/interface/resources/meshes/defaultAvatar_full/avatar-animation.json
+++ b/interface/resources/meshes/defaultAvatar_full/avatar-animation.json
@@ -26,13 +26,14 @@
                         {
                             "jointName": "Neck",
                             "positionVar": "neckPosition",
-                            "rotationVar": "neckRotation"
+                            "rotationVar": "neckRotation",
+                            "typeVar": "headAndNeckType"
                         },
                         {
                             "jointName": "Head",
                             "positionVar": "headPosition",
                             "rotationVar": "headRotation",
-                            "typeVar": "headType"
+                            "typeVar": "headAndNeckType"
                         }
                     ]
                 },
diff --git a/libraries/animation/src/Rig.cpp b/libraries/animation/src/Rig.cpp
index 0022749d51..ceef68f85b 100644
--- a/libraries/animation/src/Rig.cpp
+++ b/libraries/animation/src/Rig.cpp
@@ -1079,7 +1079,7 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) {
 
                 _animVars.set("headPosition", headPos);
                 _animVars.set("headRotation", headRot);
-                _animVars.set("headType", QString("RotationAndPosition"));
+                _animVars.set("headAndNeckType", QString("RotationAndPosition"));
                 _animVars.set("neckPosition", neckPos);
                 _animVars.set("neckRotation", neckRot);
 
@@ -1092,7 +1092,7 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) {
 
                 _animVars.unset("headPosition");
                 _animVars.set("headRotation", realLocalHeadOrientation);
-                _animVars.set("headType", QString("RotationOnly"));
+                _animVars.set("headAndNeckType", QString("RotationOnly"));
                 _animVars.unset("neckPosition");
                 _animVars.unset("neckRotation");
             }