reduce hand IK coupling to hip position

libraries/animation/src/AnimInverseKinematics.cpp

@@ -163,7 +163,6 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
         }
 
         // harvest accumulated rotations and apply the average
-        const int numJoints = (int)_accumulators.size();
         for (int i = lowestMovedIndex; i < _maxTargetIndex; ++i) {
             if (_accumulators[i].size() > 0) {
                 _relativePoses[i].rot = _accumulators[i].getAverage();
@@ -261,16 +260,32 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe
                 targetType == IKTarget::Type::HipsRelativeRotationAndPosition) {
             // compute the swing that would get get tip closer
             glm::vec3 targetLine = target.getTranslation() - jointPosition;
+
+            const float MIN_AXIS_LENGTH = 1.0e-4f;
+            RotationConstraint* constraint = getConstraint(pivotIndex);
+            if (constraint && constraint->isLowerSpine()) {
+                // for these types of targets we only allow twist at the lower-spine
+                // (this prevents the hand targets from bending the spine too much and thereby driving the hips too far)
+                glm::vec3 twistAxis = absolutePoses[pivotIndex].trans - absolutePoses[pivotsParentIndex].trans;
+                float twistAxisLength = glm::length(twistAxis);
+                if (twistAxisLength > MIN_AXIS_LENGTH) {
+                    // project leverArm and targetLine to the plane
+                    twistAxis /= twistAxisLength;
+                    leverArm -= glm::dot(leverArm, twistAxis) * twistAxis;
+                    targetLine -= glm::dot(targetLine, twistAxis) * twistAxis;
+                } else {
+                    leverArm = Vectors::ZERO;
+                    targetLine = Vectors::ZERO;
+                }
+            }
+
             glm::vec3 axis = glm::cross(leverArm, targetLine);
             float axisLength = glm::length(axis);
-            const float MIN_AXIS_LENGTH = 1.0e-4f;
             if (axisLength > MIN_AXIS_LENGTH) {
-                // compute deltaRotation for alignment (swings tip closer to target)
+                // compute angle of rotation that brings tip closer to target
                 axis /= axisLength;
                 float angle = acosf(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine)));
 
-                // NOTE: even when axisLength is not zero (e.g. lever-arm and pivot-arm are not quite aligned) it is
-                // still possible for the angle to be zero so we also check that to avoid unnecessary calculations.
                 const float MIN_ADJUSTMENT_ANGLE = 1.0e-4f;
                 if (angle > MIN_ADJUSTMENT_ANGLE) {
                     // reduce angle by a fraction (for stability)
@@ -663,6 +678,10 @@ void AnimInverseKinematics::initConstraints() {
             const float MAX_SPINE_SWING = PI / 14.0f;
             minDots.push_back(cosf(MAX_SPINE_SWING));
             stConstraint->setSwingLimits(minDots);
+            if (0 == baseName.compare("Spine1", Qt::CaseInsensitive)
+                    || 0 == baseName.compare("Spine", Qt::CaseInsensitive)) {
+                stConstraint->setLowerSpine(true);
+            }
 
             constraint = static_cast<RotationConstraint*>(stConstraint);
         } else if (baseName.startsWith("Hips2", Qt::CaseInsensitive)) {

libraries/animation/src/RotationConstraint.h

@@ -28,6 +28,9 @@ public:
     /// \return true if rotation is clamped
     virtual bool apply(glm::quat& rotation) const = 0;
 
+    /// \return true if this constraint is part of lower spine
+    virtual bool isLowerSpine() const { return false; }
+
 protected:
     glm::quat _referenceRotation = glm::quat();
 };

libraries/animation/src/SwingTwistConstraint.cpp

@@ -123,7 +123,7 @@ void SwingTwistConstraint::setSwingLimits(const std::vector<glm::vec3>& swungDir
 
         // sort limits by theta
         std::sort(limits.begin(), limits.end());
-    
+
         // extrapolate evenly distributed limits for fast lookup table
         float deltaTheta = TWO_PI / (float)(numLimits);
         uint32_t rightIndex = 0;
@@ -219,7 +219,7 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const {
     } else {
         _lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;
     }
-    
+
     // clamp the swing
     // The swingAxis is always perpendicular to the reference axis (yAxis in the constraint's frame).
     glm::vec3 swungY = swingRotation * yAxis;
@@ -232,7 +232,7 @@ bool SwingTwistConstraint::apply(glm::quat& rotation) const {
         float theta = atan2f(-swingAxis.z, swingAxis.x);
         float minDot = _swingLimitFunction.getMinDot(theta);
         if (glm::dot(swungY, yAxis) < minDot) {
-            // The swing limits are violated so we extract the angle from midDot and 
+            // The swing limits are violated so we extract the angle from midDot and
             // use it to supply a new rotation.
             swingAxis /= axisLength;
             swingRotation = glm::angleAxis(acosf(minDot), swingAxis);

libraries/animation/src/SwingTwistConstraint.h

@@ -18,20 +18,20 @@
 
 class SwingTwistConstraint : public RotationConstraint {
 public:
-    // The SwingTwistConstraint starts in the "referenceRotation" and then measures an initial twist 
+    // The SwingTwistConstraint starts in the "referenceRotation" and then measures an initial twist
     // about the yAxis followed by a swing about some axis that lies in the XZ plane, such that the twist
-    // and swing combine to produce the rotation.  Each partial rotation is constrained within limits 
+    // and swing combine to produce the rotation.  Each partial rotation is constrained within limits
     // then used to construct the new final rotation.
 
     SwingTwistConstraint();
 
     /// \param minDots vector of minimum dot products between the twist and swung axes
-    /// \brief The values are minimum dot-products between the twist axis and the swung axes 
+    /// \brief The values are minimum dot-products between the twist axis and the swung axes
     ///  that correspond to swing axes equally spaced around the XZ plane.  Another way to
-    ///  think about it is that the dot-products correspond to correspond to angles (theta) 
+    ///  think about it is that the dot-products correspond to correspond to angles (theta)
-    ///  about the twist axis ranging from 0 to 2PI-deltaTheta (Note: the cyclic boundary 
+    ///  about the twist axis ranging from 0 to 2PI-deltaTheta (Note: the cyclic boundary
     ///  conditions are handled internally, so don't duplicate the dot-product at 2PI).
-    ///  See the paper by Quang Liu and Edmond C. Prakash mentioned below for a more detailed 
+    ///  See the paper by Quang Liu and Edmond C. Prakash mentioned below for a more detailed
     ///  description of how this works.
     void setSwingLimits(std::vector<float> minDots);
 
@@ -50,21 +50,24 @@ public:
     /// \return true if rotation is changed
     virtual bool apply(glm::quat& rotation) const override;
 
+    void setLowerSpine(bool lowerSpine) { _lowerSpine = lowerSpine; }
+    virtual bool isLowerSpine() const { return _lowerSpine; }
+
     // SwingLimitFunction is an implementation of the constraint check described in the paper:
     // "The Parameterization of Joint Rotation with the Unit Quaternion" by Quang Liu and Edmond C. Prakash
     class SwingLimitFunction {
     public:
         SwingLimitFunction();
-    
+
         /// \brief use a uniform conical swing limit
         void setCone(float maxAngle);
-    
+
         /// \brief use a vector of lookup values for swing limits
         void setMinDots(const std::vector<float>& minDots);
-    
+
         /// \return minimum dotProduct between reference and swung axes
         float getMinDot(float theta) const;
-    
+
     protected:
         // the limits are stored in a lookup table with cyclic boundary conditions
         std::vector<float> _minDots;
@@ -84,6 +87,7 @@ protected:
     // We want to remember the LAST clamped boundary, so we an use it even when the far boundary is closer.
     // This reduces "pops" when the input twist angle goes far beyond and wraps around toward the far boundary.
     mutable int _lastTwistBoundary;
+    bool _lowerSpine { false };
 };
 
 #endif // hifi_SwingTwistConstraint_h