From 48efbba335c93dd6f31f0984ad2b34148d3b3791 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Fri, 11 Mar 2016 14:44:43 -0800
Subject: [PATCH] use dynamic constraints for IK

---
 libraries/animation/src/AnimInverseKinematics.cpp | 11 +++++++++++
 libraries/animation/src/RotationConstraint.h      |  4 ++++
 libraries/animation/src/SwingTwistConstraint.h    |  2 +-
 3 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp
index 46e17c39d8..f4df7ada82 100644
--- a/libraries/animation/src/AnimInverseKinematics.cpp
+++ b/libraries/animation/src/AnimInverseKinematics.cpp
@@ -396,6 +396,17 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
             }
             _relativePoses[i].trans = underPoses[i].trans;
         }
+
+        if (!_relativePoses.empty()) {
+            // Sometimes the underpose itself can violate the constraints.  Rather than
+            // clamp the animation we dynamically expand each constraint to accomodate it.
+            std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();
+            while (constraintItr != _constraints.end()) {
+                int index = constraintItr->first;
+                constraintItr->second->dynamicallyAdjustLimits(_relativePoses[index].rot);
+                ++constraintItr;
+            }
+        }
     }
 
     if (!_relativePoses.empty()) {
diff --git a/libraries/animation/src/RotationConstraint.h b/libraries/animation/src/RotationConstraint.h
index 0745500582..9e34537cab 100644
--- a/libraries/animation/src/RotationConstraint.h
+++ b/libraries/animation/src/RotationConstraint.h
@@ -31,6 +31,10 @@ public:
     /// \return true if this constraint is part of lower spine
     virtual bool isLowerSpine() const { return false; }
 
+    /// \param rotation rotation to allow
+    /// \brief clear previous adjustment and adjust constraint limits to allow rotation
+    virtual void dynamicallyAdjustLimits(const glm::quat& rotation) {}
+
 protected:
     glm::quat _referenceRotation = glm::quat();
 };
diff --git a/libraries/animation/src/SwingTwistConstraint.h b/libraries/animation/src/SwingTwistConstraint.h
index 4734aa8b9d..93d7449d8f 100644
--- a/libraries/animation/src/SwingTwistConstraint.h
+++ b/libraries/animation/src/SwingTwistConstraint.h
@@ -55,7 +55,7 @@ public:
 
     /// \param rotation rotation to allow
     /// \brief clear previous adjustment and adjust constraint limits to allow rotation
-    void dynamicallyAdjustLimits(const glm::quat& rotation);
+    virtual void dynamicallyAdjustLimits(const glm::quat& rotation) override;
 
     // for testing purposes
     const std::vector<float>& getMinDots() { return _swingLimitFunction.getMinDots(); }