merge from upstream

examples/controllers/handControllerGrab.js

@@ -783,8 +783,6 @@ function MyController(hand) {
                 direction: pickRay.direction
             };
 
-            // Messages.sendMessage('Hifi-Light-Overlay-Ray-Check', JSON.stringify(pickRayBacked));
-
             var intersection;
 
             if (USE_BLACKLIST === true && blacklist.length !== 0) {
@@ -1388,11 +1386,6 @@ function MyController(hand) {
             }
         }
 
-        // Messages.sendMessage('Hifi-Object-Manipulation', JSON.stringify({
-        //     action: 'update',
-        //     grabbedEntity: this.grabbedEntity
-        // }))
-
         if (this.actionID && this.actionTimeout - now < ACTION_TTL_REFRESH * MSEC_PER_SEC) {
             // if less than a 5 seconds left, refresh the actions ttl
             var success = Entities.updateAction(this.grabbedEntity, this.actionID, {

examples/light_modifier/README.md

@@ -1,3 +1,5 @@
+*Temporarily Deprecated - needs a better way to know when 'grab beams' intersect with 'light overlays'.  Sending messages containing the ray from the hand grab script to the overlay intersection test doesn't seem to be sustainable. *
+
 This PR demonstrates one way in-world editing of objects might work. 
 
 Running this script will show light overlay icons in-world.  Enter edit mode by running your distance beam through a light overlay.  Exit using the red X.

interface/src/avatar/SkeletonModel.cpp

@@ -46,21 +46,21 @@ void SkeletonModel::initJointStates() {
 
     // Determine the default eye position for avatar scale = 1.0
     int headJointIndex = _geometry->getFBXGeometry().headJointIndex;
-    if (0 <= headJointIndex && headJointIndex < _rig->getJointStateCount()) {
+    if (0 > headJointIndex || headJointIndex >= _rig->getJointStateCount()) {
-
+        qCWarning(interfaceapp) << "Bad head joint! Got:" << headJointIndex << "jointCount:" << _rig->getJointStateCount();
-        glm::vec3 leftEyePosition, rightEyePosition;
-        getEyeModelPositions(leftEyePosition, rightEyePosition);
-        glm::vec3 midEyePosition = (leftEyePosition + rightEyePosition) / 2.0f;
-
-        int rootJointIndex = _geometry->getFBXGeometry().rootJointIndex;
-        glm::vec3 rootModelPosition;
-        getJointPosition(rootJointIndex, rootModelPosition);
-
-        _defaultEyeModelPosition = midEyePosition - rootModelPosition;
-
-        // Skeleton may have already been scaled so unscale it
-        _defaultEyeModelPosition = _defaultEyeModelPosition / _scale;
     }
+    glm::vec3 leftEyePosition, rightEyePosition;
+    getEyeModelPositions(leftEyePosition, rightEyePosition);
+    glm::vec3 midEyePosition = (leftEyePosition + rightEyePosition) / 2.0f;
+
+    int rootJointIndex = _geometry->getFBXGeometry().rootJointIndex;
+    glm::vec3 rootModelPosition;
+    getJointPosition(rootJointIndex, rootModelPosition);
+
+    _defaultEyeModelPosition = midEyePosition - rootModelPosition;
+
+    // Skeleton may have already been scaled so unscale it
+    _defaultEyeModelPosition = _defaultEyeModelPosition / _scale;
 
     computeBoundingShape();
 

libraries/animation/src/AnimInverseKinematics.cpp

@@ -144,168 +144,34 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
 
     int numLoops = 0;
     const int MAX_IK_LOOPS = 4;
-    do {
+    while (numLoops < MAX_IK_LOOPS) {
-        int lowestMovedIndex = (int)_relativePoses.size();
-        for (auto& target: targets) {
-            IKTarget::Type targetType = target.getType();
-            if (targetType == IKTarget::Type::RotationOnly) {
-                // the final rotation will be enforced after the iterations
-                continue;
-            }
-
-            int tipIndex = target.getIndex();
-            int pivotIndex = _skeleton->getParentIndex(tipIndex);
-            if (pivotIndex == -1 || pivotIndex == _hipsIndex) {
-                continue;
-            }
-            int pivotsParentIndex = _skeleton->getParentIndex(pivotIndex);
-            if (pivotsParentIndex == -1) {
-                // TODO?: handle case where tip's parent is root?
-                continue;
-            }
-
-            // cache tip's absolute orientation
-            glm::quat tipOrientation = absolutePoses[tipIndex].rot;
-
-            // also cache tip's parent's absolute orientation so we can recompute 
-            // the tip's parent-relative as we proceed up the chain
-            glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot;
-
-            if (targetType == IKTarget::Type::HmdHead) {
-                // rotate tip directly to target orientation
-                tipOrientation = target.getRotation();
-
-                // enforce tip's constraint
-                RotationConstraint* constraint = getConstraint(tipIndex);
-                if (constraint) {
-                    glm::quat tipRelativeRotation = glm::normalize(tipOrientation * glm::inverse(tipParentOrientation));
-                    bool constrained = constraint->apply(tipRelativeRotation);
-                    if (constrained) {
-                        tipOrientation = glm::normalize(tipRelativeRotation * tipParentOrientation);
-                    }
-                }
-            }
-
-            // cache tip absolute position
-            glm::vec3 tipPosition = absolutePoses[tipIndex].trans;
-
-            // descend toward root, pivoting each joint to get tip closer to target
-            while (pivotIndex != _hipsIndex && pivotsParentIndex != -1) {
-                // compute the two lines that should be aligned
-                glm::vec3 jointPosition = absolutePoses[pivotIndex].trans;
-                glm::vec3 leverArm = tipPosition - jointPosition;
-
-                glm::quat deltaRotation;
-                if (targetType == IKTarget::Type::RotationAndPosition ||
-                        targetType == IKTarget::Type::HipsRelativeRotationAndPosition) {
-                    // compute the swing that would get get tip closer
-                    glm::vec3 targetLine = target.getTranslation() - jointPosition;
-                    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)
-                        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)
-                            const float fraction = 0.5f;
-                            angle *= fraction;
-                            deltaRotation = glm::angleAxis(angle, axis);
-
-                            // The swing will re-orient the tip but there will tend to be be a non-zero delta between the tip's
-                            // new orientation and its target.  This is the final parent-relative orientation that the tip joint have
-                            // make to achieve its target orientation.
-                            glm::quat tipRelativeRotation = glm::inverse(deltaRotation * tipParentOrientation) * target.getRotation();
-
-                            // enforce tip's constraint
-                            RotationConstraint* constraint = getConstraint(tipIndex);
-                            if (constraint) {
-                                bool constrained = constraint->apply(tipRelativeRotation);
-                                if (constrained) {
-                                    // The tip's final parent-relative rotation would violate its constraint
-                                    // so we try to pre-twist this pivot to compensate.
-                                    glm::quat constrainedTipRotation = deltaRotation * tipParentOrientation * tipRelativeRotation;
-                                    glm::quat missingRotation = target.getRotation() * glm::inverse(constrainedTipRotation);
-                                    glm::quat swingPart;
-                                    glm::quat twistPart;
-                                    glm::vec3 axis = glm::normalize(deltaRotation * leverArm);
-                                    swingTwistDecomposition(missingRotation, axis, swingPart, twistPart);
-                                    float dotSign = copysignf(1.0f, twistPart.w);
-                                    deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * twistPart, fraction)) * deltaRotation;
-                                }
-                            }
-                        }
-                    }
-                } else if (targetType == IKTarget::Type::HmdHead) {
-                    // An HmdHead target slaves the orientation of the end-effector by distributing rotation
-                    // deltas up the hierarchy.  Its target position is enforced later by shifting the hips.
-                    deltaRotation = target.getRotation() * glm::inverse(tipOrientation);
-                    float dotSign = copysignf(1.0f, deltaRotation.w);
-                    const float ANGLE_DISTRIBUTION_FACTOR = 0.45f;
-                    deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * deltaRotation, ANGLE_DISTRIBUTION_FACTOR));
-                }
-
-                // compute joint's new parent-relative rotation after swing
-                // Q' = dQ * Q   and   Q = Qp * q   -->   q' = Qp^ * dQ * Q
-                glm::quat newRot = glm::normalize(glm::inverse(
-                        absolutePoses[pivotsParentIndex].rot) *
-                        deltaRotation *
-                        absolutePoses[pivotIndex].rot);
-
-                // enforce pivot's constraint
-                RotationConstraint* constraint = getConstraint(pivotIndex);
-                if (constraint) {
-                    bool constrained = constraint->apply(newRot);
-                    if (constrained) {
-                        // the constraint will modify the local rotation of the tip so we must 
-                        // compute the corresponding model-frame deltaRotation
-                        // Q' = Qp^ * dQ * Q  -->  dQ =   Qp * Q' * Q^
-                        deltaRotation = absolutePoses[pivotsParentIndex].rot *
-                            newRot * glm::inverse(absolutePoses[pivotIndex].rot);
-                    }
-                }
-
-                // store the rotation change in the accumulator
-                _accumulators[pivotIndex].add(newRot, target.getWeight());
-
-                // this joint has been changed so we check to see if it has the lowest index
-                if (pivotIndex < lowestMovedIndex) {
-                    lowestMovedIndex = pivotIndex;
-                }
-
-                // keep track of tip's new transform as we descend towards root
-                tipPosition = jointPosition + deltaRotation * leverArm;
-                tipOrientation = glm::normalize(deltaRotation * tipOrientation);
-                tipParentOrientation = glm::normalize(deltaRotation * tipParentOrientation);
-
-                pivotIndex = pivotsParentIndex;
-                pivotsParentIndex = _skeleton->getParentIndex(pivotIndex);
-            }
-        }
         ++numLoops;
 
+        // solve all targets
+        int lowestMovedIndex = (int)_relativePoses.size();
+        for (auto& target: targets) {
+            int lowIndex = solveTargetWithCCD(target, absolutePoses);
+            if (lowIndex < lowestMovedIndex) {
+                lowestMovedIndex = lowIndex;
+            }
+        }
+
         // harvest accumulated rotations and apply the average
-        const int numJoints = (int)_accumulators.size();
+        for (int i = lowestMovedIndex; i < _maxTargetIndex; ++i) {
-        for (int i = 0; i < numJoints; ++i) {
             if (_accumulators[i].size() > 0) {
                 _relativePoses[i].rot = _accumulators[i].getAverage();
                 _accumulators[i].clear();
             }
         }
 
-        // only update the absolutePoses that need it: those between lowestMovedIndex and _maxTargetIndex
+        // update the absolutePoses that need it (from lowestMovedIndex to _maxTargetIndex)
         for (auto i = lowestMovedIndex; i <= _maxTargetIndex; ++i) {
             auto parentIndex = _skeleton->getParentIndex((int)i);
             if (parentIndex != -1) {
                 absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i];
             }
         }
-    } while (numLoops < MAX_IK_LOOPS);
+    }
 
     // finally set the relative rotation of each tip to agree with absolute target rotation
     for (auto& target: targets) {
@@ -329,6 +195,169 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
     }
 }
 
+int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVec& absolutePoses) {
+    int lowestMovedIndex = (int)_relativePoses.size();
+    IKTarget::Type targetType = target.getType();
+    if (targetType == IKTarget::Type::RotationOnly) {
+        // the final rotation will be enforced after the iterations
+        // TODO: solve this correctly
+        return lowestMovedIndex;
+    }
+
+    int tipIndex = target.getIndex();
+    int pivotIndex = _skeleton->getParentIndex(tipIndex);
+    if (pivotIndex == -1 || pivotIndex == _hipsIndex) {
+        return lowestMovedIndex;
+    }
+    int pivotsParentIndex = _skeleton->getParentIndex(pivotIndex);
+    if (pivotsParentIndex == -1) {
+        // TODO?: handle case where tip's parent is root?
+        return lowestMovedIndex;
+    }
+
+    // cache tip's absolute orientation
+    glm::quat tipOrientation = absolutePoses[tipIndex].rot;
+
+    // also cache tip's parent's absolute orientation so we can recompute
+    // the tip's parent-relative as we proceed up the chain
+    glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot;
+
+    if (targetType == IKTarget::Type::HmdHead) {
+        // rotate tip directly to target orientation
+        tipOrientation = target.getRotation();
+        glm::quat tipRelativeRotation = glm::normalize(tipOrientation * glm::inverse(tipParentOrientation));
+
+        // enforce tip's constraint
+        RotationConstraint* constraint = getConstraint(tipIndex);
+        if (constraint) {
+            bool constrained = constraint->apply(tipRelativeRotation);
+            if (constrained) {
+                tipOrientation = glm::normalize(tipRelativeRotation * tipParentOrientation);
+                tipRelativeRotation = glm::normalize(tipOrientation * glm::inverse(tipParentOrientation));
+            }
+        }
+        // store the relative rotation change in the accumulator
+        _accumulators[tipIndex].add(tipRelativeRotation, target.getWeight());
+    }
+
+    // cache tip absolute position
+    glm::vec3 tipPosition = absolutePoses[tipIndex].trans;
+
+    // descend toward root, pivoting each joint to get tip closer to target position
+    while (pivotIndex != _hipsIndex && pivotsParentIndex != -1) {
+        // compute the two lines that should be aligned
+        glm::vec3 jointPosition = absolutePoses[pivotIndex].trans;
+        glm::vec3 leverArm = tipPosition - jointPosition;
+
+        glm::quat deltaRotation;
+        if (targetType == IKTarget::Type::RotationAndPosition ||
+                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);
+            if (axisLength > MIN_AXIS_LENGTH) {
+                // 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)));
+
+                const float MIN_ADJUSTMENT_ANGLE = 1.0e-4f;
+                if (angle > MIN_ADJUSTMENT_ANGLE) {
+                    // reduce angle by a fraction (for stability)
+                    const float fraction = 0.5f;
+                    angle *= fraction;
+                    deltaRotation = glm::angleAxis(angle, axis);
+
+                    // The swing will re-orient the tip but there will tend to be be a non-zero delta between the tip's
+                    // new orientation and its target.  This is the final parent-relative orientation that the tip joint have
+                    // make to achieve its target orientation.
+                    glm::quat tipRelativeRotation = glm::inverse(deltaRotation * tipParentOrientation) * target.getRotation();
+
+                    // enforce tip's constraint
+                    RotationConstraint* constraint = getConstraint(tipIndex);
+                    if (constraint) {
+                        bool constrained = constraint->apply(tipRelativeRotation);
+                        if (constrained) {
+                            // The tip's final parent-relative rotation would violate its constraint
+                            // so we try to pre-twist this pivot to compensate.
+                            glm::quat constrainedTipRotation = deltaRotation * tipParentOrientation * tipRelativeRotation;
+                            glm::quat missingRotation = target.getRotation() * glm::inverse(constrainedTipRotation);
+                            glm::quat swingPart;
+                            glm::quat twistPart;
+                            glm::vec3 axis = glm::normalize(deltaRotation * leverArm);
+                            swingTwistDecomposition(missingRotation, axis, swingPart, twistPart);
+                            float dotSign = copysignf(1.0f, twistPart.w);
+                            deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * twistPart, fraction)) * deltaRotation;
+                        }
+                    }
+                }
+            }
+        } else if (targetType == IKTarget::Type::HmdHead) {
+            // An HmdHead target slaves the orientation of the end-effector by distributing rotation
+            // deltas up the hierarchy.  Its target position is enforced later (by shifting the hips).
+            deltaRotation = target.getRotation() * glm::inverse(tipOrientation);
+            float dotSign = copysignf(1.0f, deltaRotation.w);
+            const float ANGLE_DISTRIBUTION_FACTOR = 0.45f;
+            deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * deltaRotation, ANGLE_DISTRIBUTION_FACTOR));
+        }
+
+        // compute joint's new parent-relative rotation after swing
+        // Q' = dQ * Q   and   Q = Qp * q   -->   q' = Qp^ * dQ * Q
+        glm::quat newRot = glm::normalize(glm::inverse(
+                absolutePoses[pivotsParentIndex].rot) *
+                deltaRotation *
+                absolutePoses[pivotIndex].rot);
+
+        // enforce pivot's constraint
+        RotationConstraint* constraint = getConstraint(pivotIndex);
+        if (constraint) {
+            bool constrained = constraint->apply(newRot);
+            if (constrained) {
+                // the constraint will modify the local rotation of the tip so we must
+                // compute the corresponding model-frame deltaRotation
+                // Q' = Qp^ * dQ * Q  -->  dQ =   Qp * Q' * Q^
+                deltaRotation = absolutePoses[pivotsParentIndex].rot * newRot * glm::inverse(absolutePoses[pivotIndex].rot);
+            }
+        }
+
+        // store the relative rotation change in the accumulator
+        _accumulators[pivotIndex].add(newRot, target.getWeight());
+
+        // this joint has been changed so we check to see if it has the lowest index
+        if (pivotIndex < lowestMovedIndex) {
+            lowestMovedIndex = pivotIndex;
+        }
+
+        // keep track of tip's new transform as we descend towards root
+        tipPosition = jointPosition + deltaRotation * leverArm;
+        tipOrientation = glm::normalize(deltaRotation * tipOrientation);
+        tipParentOrientation = glm::normalize(deltaRotation * tipParentOrientation);
+
+        pivotIndex = pivotsParentIndex;
+        pivotsParentIndex = _skeleton->getParentIndex(pivotIndex);
+    }
+    return lowestMovedIndex;
+}
+
 //virtual
 const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) {
     // don't call this function, call overlay() instead
@@ -341,7 +370,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
     if (_relativePoses.size() != underPoses.size()) {
         loadPoses(underPoses);
     } else {
-        // relax toward underpose
+        // 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
@@ -352,8 +381,10 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
         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;
@@ -376,7 +407,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
                 ++constraintItr;
             }
         } else {
-            // shift the everything according to the _hipsOffset from the previous frame
+            // shift hips according to the _hipsOffset from the previous frame
             float offsetLength = glm::length(_hipsOffset);
             const float MIN_HIPS_OFFSET_LENGTH = 0.03f;
             if (offsetLength > MIN_HIPS_OFFSET_LENGTH) {
@@ -393,14 +424,14 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
                         hipsFrameRotation *= _relativePoses[index].rot;
                         index = _skeleton->getParentIndex(index);
                     }
-                    _relativePoses[_hipsIndex].trans = underPoses[_hipsIndex].trans 
+                    _relativePoses[_hipsIndex].trans = underPoses[_hipsIndex].trans
                         + glm::inverse(glm::normalize(hipsFrameRotation)) * (scaleFactor * _hipsOffset);
                 }
             }
 
             solveWithCyclicCoordinateDescent(targets);
 
-            // compute the new target hips offset (for next frame)
+            // measure new _hipsOffset for next frame
             // by looking for discrepancies between where a targeted endEffector is
             // and where it wants to be (after IK solutions are done)
             glm::vec3 newHipsOffset = Vectors::ZERO;
@@ -409,7 +440,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
                 if (targetIndex == _headIndex && _headIndex != -1) {
                     // special handling for headTarget
                     if (target.getType() == IKTarget::Type::RotationOnly) {
-                        // we want to shift the hips to bring the underpose closer
+                        // we want to shift the hips to bring the underPose closer
                         // to where the head happens to be (overpose)
                         glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans;
                         glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
@@ -511,16 +542,16 @@ void AnimInverseKinematics::initConstraints() {
     for (int i = 0; i < numJoints; ++i) {
         // compute the joint's baseName and remember whether its prefix was "Left" or not
         QString baseName = _skeleton->getJointName(i);
-        bool isLeft = baseName.startsWith("Left", Qt::CaseInsensitive);
+        bool isLeft = baseName.startsWith("Left", Qt::CaseSensitive);
         float mirror = isLeft ? -1.0f : 1.0f;
         if (isLeft) {
             baseName.remove(0, 4);
-        } else if (baseName.startsWith("Right", Qt::CaseInsensitive)) {
+        } else if (baseName.startsWith("Right", Qt::CaseSensitive)) {
             baseName.remove(0, 5);
         }
 
         RotationConstraint* constraint = nullptr;
-        if (0 == baseName.compare("Arm", Qt::CaseInsensitive)) {
+        if (0 == baseName.compare("Arm", Qt::CaseSensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
             stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f);
@@ -554,7 +585,7 @@ void AnimInverseKinematics::initConstraints() {
             stConstraint->setSwingLimits(minDots);
 
             constraint = static_cast<RotationConstraint*>(stConstraint);
-        } else if (0 == baseName.compare("UpLeg", Qt::CaseInsensitive)) {
+        } else if (0 == baseName.compare("UpLeg", Qt::CaseSensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
             stConstraint->setTwistLimits(-PI / 4.0f, PI / 4.0f);
@@ -580,7 +611,7 @@ void AnimInverseKinematics::initConstraints() {
             stConstraint->setSwingLimits(swungDirections);
 
             constraint = static_cast<RotationConstraint*>(stConstraint);
-        } else if (0 == baseName.compare("Hand", Qt::CaseInsensitive)) {
+        } else if (0 == baseName.compare("Hand", Qt::CaseSensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
             const float MAX_HAND_TWIST = 3.0f * PI / 5.0f;
@@ -619,7 +650,7 @@ void AnimInverseKinematics::initConstraints() {
             stConstraint->setSwingLimits(minDots);
 
             constraint = static_cast<RotationConstraint*>(stConstraint);
-        } else if (baseName.startsWith("Shoulder", Qt::CaseInsensitive)) {
+        } else if (baseName.startsWith("Shoulder", Qt::CaseSensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
             const float MAX_SHOULDER_TWIST = PI / 20.0f;
@@ -631,7 +662,7 @@ void AnimInverseKinematics::initConstraints() {
             stConstraint->setSwingLimits(minDots);
 
             constraint = static_cast<RotationConstraint*>(stConstraint);
-        } else if (baseName.startsWith("Spine", Qt::CaseInsensitive)) {
+        } else if (baseName.startsWith("Spine", Qt::CaseSensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
             const float MAX_SPINE_TWIST = PI / 12.0f;
@@ -641,9 +672,13 @@ 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::CaseSensitive)
+                    || 0 == baseName.compare("Spine", Qt::CaseSensitive)) {
+                stConstraint->setLowerSpine(true);
+            }
 
             constraint = static_cast<RotationConstraint*>(stConstraint);
-        } else if (baseName.startsWith("Hips2", Qt::CaseInsensitive)) {
+        } else if (baseName.startsWith("Hips2", Qt::CaseSensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
             const float MAX_SPINE_TWIST = PI / 8.0f;
@@ -655,7 +690,7 @@ void AnimInverseKinematics::initConstraints() {
             stConstraint->setSwingLimits(minDots);
 
             constraint = static_cast<RotationConstraint*>(stConstraint);
-        } else if (0 == baseName.compare("Neck", Qt::CaseInsensitive)) {
+        } else if (0 == baseName.compare("Neck", Qt::CaseSensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
             const float MAX_NECK_TWIST = PI / 9.0f;
@@ -667,7 +702,7 @@ void AnimInverseKinematics::initConstraints() {
             stConstraint->setSwingLimits(minDots);
 
             constraint = static_cast<RotationConstraint*>(stConstraint);
-        } else if (0 == baseName.compare("Head", Qt::CaseInsensitive)) {
+        } else if (0 == baseName.compare("Head", Qt::CaseSensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
             const float MAX_HEAD_TWIST = PI / 9.0f;
@@ -679,7 +714,7 @@ void AnimInverseKinematics::initConstraints() {
             stConstraint->setSwingLimits(minDots);
 
             constraint = static_cast<RotationConstraint*>(stConstraint);
-        } else if (0 == baseName.compare("ForeArm", Qt::CaseInsensitive)) {
+        } else if (0 == baseName.compare("ForeArm", Qt::CaseSensitive)) {
             // The elbow joint rotates about the parent-frame's zAxis (-zAxis) for the Right (Left) arm.
             ElbowConstraint* eConstraint = new ElbowConstraint();
             glm::quat referenceRotation = _defaultRelativePoses[i].rot;
@@ -710,7 +745,7 @@ void AnimInverseKinematics::initConstraints() {
             eConstraint->setAngleLimits(minAngle, maxAngle);
 
             constraint = static_cast<RotationConstraint*>(eConstraint);
-        } else if (0 == baseName.compare("Leg", Qt::CaseInsensitive)) {
+        } else if (0 == baseName.compare("Leg", Qt::CaseSensitive)) {
             // The knee joint rotates about the parent-frame's -xAxis.
             ElbowConstraint* eConstraint = new ElbowConstraint();
             glm::quat referenceRotation = _defaultRelativePoses[i].rot;
@@ -741,7 +776,7 @@ void AnimInverseKinematics::initConstraints() {
             eConstraint->setAngleLimits(minAngle, maxAngle);
 
             constraint = static_cast<RotationConstraint*>(eConstraint);
-        } else if (0 == baseName.compare("Foot", Qt::CaseInsensitive)) {
+        } else if (0 == baseName.compare("Foot", Qt::CaseSensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot);
             stConstraint->setTwistLimits(-PI / 4.0f, PI / 4.0f);

libraries/animation/src/AnimInverseKinematics.h

@@ -40,6 +40,7 @@ public:
 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;
 
     // for AnimDebugDraw rendering

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