From 2166d8c159e15e20a9ff03e62486b426d511268d Mon Sep 17 00:00:00 2001
From: "Anthony J. Thibault" <tony@highfidelity.io>
Date: Mon, 8 May 2017 14:10:16 -0700
Subject: [PATCH] Added setSolutionSource to AnimInverseKinematics node.

---
 .../animation/src/AnimInverseKinematics.cpp   | 58 +++++++++++++------
 .../animation/src/AnimInverseKinematics.h     | 13 +++++
 2 files changed, 52 insertions(+), 19 deletions(-)

diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index 5dcbfcafad..bf05d9358c 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -414,25 +414,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
 
         PROFILE_RANGE_EX(simulation_animation, "ik/relax", 0xffff00ff, 0);
 
-        // relax toward underPoses
-        // HACK: this relaxation needs to be constant per-frame rather than per-realtime
-        // in order to prevent IK "flutter" for bad FPS.  The bad news is that the good parts
-        // of this relaxation will be FPS dependent (low FPS will make the limbs align slower
-        // in real-time), however most people will not notice this and this problem is less
-        // annoying than the flutter.
-        const float blend = (1.0f / 60.0f) / (0.25f); // effectively: dt / RELAXATION_TIMESCALE
-        int numJoints = (int)_relativePoses.size();
-        for (int i = 0; i < numJoints; ++i) {
-            float dotSign = copysignf(1.0f, glm::dot(_relativePoses[i].rot(), underPoses[i].rot()));
-            if (_accumulators[i].isDirty()) {
-                // this joint is affected by IK --> blend toward underPose rotation
-                _relativePoses[i].rot() = glm::normalize(glm::lerp(_relativePoses[i].rot(), dotSign * underPoses[i].rot(), blend));
-            } else {
-                // this joint is NOT affected by IK --> slam to underPose rotation
-                _relativePoses[i].rot() = underPoses[i].rot();
-            }
-            _relativePoses[i].trans() = underPoses[i].trans();
-        }
+        initRelativePosesFromSolutionSource(underPoses);
 
         if (!underPoses.empty()) {
             // Sometimes the underpose itself can violate the constraints.  Rather than
@@ -1135,3 +1117,41 @@ void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) con
         }
     }
 }
+
+void AnimInverseKinematics::relaxToPoses(const AnimPoseVec& poses) {
+    // relax toward poses
+    const float blend = (1.0f / 60.0f) / (0.25f);
+    int numJoints = (int)_relativePoses.size();
+    for (int i = 0; i < numJoints; ++i) {
+        float dotSign = copysignf(1.0f, glm::dot(_relativePoses[i].rot(), poses[i].rot()));
+        if (_accumulators[i].isDirty()) {
+            // this joint is affected by IK --> blend toward each pose rotation
+            _relativePoses[i].rot() = glm::normalize(glm::lerp(_relativePoses[i].rot(), dotSign * poses[i].rot(), blend));
+        } else {
+            // this joint is NOT affected by IK --> slam to underPose rotation
+            _relativePoses[i].rot() = poses[i].rot();
+        }
+        _relativePoses[i].trans() = poses[i].trans();
+    }
+}
+
+void AnimInverseKinematics::initRelativePosesFromSolutionSource(const AnimPoseVec& underPoses) {
+    switch (_solutionSource) {
+    default:
+    case SolutionSource::RelaxToUnderPoses:
+        relaxToPoses(underPoses);
+        break;
+    case SolutionSource::RelaxToLimitCenterPoses:
+        relaxToPoses(_limitCenterPoses);
+        break;
+    case SolutionSource::PreviousSolution:
+        // do nothing... _relativePoses is already the previous solution
+        break;
+    case SolutionSource::UnderPoses:
+        _relativePoses = underPoses;
+        break;
+    case SolutionSource::LimitCenterPoses:
+        _relativePoses = _limitCenterPoses;
+        break;
+    }
+}
diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h
index b40f025ecd..9b7c095e6b 100644
--- a/libraries/animation/src/AnimInverseKinematics.h
+++ b/libraries/animation/src/AnimInverseKinematics.h
@@ -43,12 +43,24 @@ public:
 
     float getMaxErrorOnLastSolve() { return _maxErrorOnLastSolve; }
 
+    enum class SolutionSource {
+        RelaxToUnderPoses = 0,
+        RelaxToLimitCenterPoses,
+        PreviousSolution,
+        UnderPoses,
+        LimitCenterPoses
+    };
+
+    void setSolutionSource(SolutionSource solutionSource) { _solutionSource = solutionSource; }
+
 protected:
     void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses);
     void solveWithCyclicCoordinateDescent(const std::vector<IKTarget>& targets);
     int solveTargetWithCCD(const IKTarget& target, AnimPoseVec& absolutePoses);
     virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override;
     void debugDrawConstraints(const AnimContext& context) const;
+    void initRelativePosesFromSolutionSource(const AnimPoseVec& underPose);
+    void relaxToPoses(const AnimPoseVec& poses);
 
     // for AnimDebugDraw rendering
     virtual const AnimPoseVec& getPosesInternal() const override { return _relativePoses; }
@@ -103,6 +115,7 @@ protected:
 
     float _maxErrorOnLastSolve { FLT_MAX };
     bool _previousEnableDebugIKTargets { false };
+    SolutionSource _solutionSource { SolutionSource::RelaxToUnderPoses };
 };
 
 #endif // hifi_AnimInverseKinematics_h