Merge pull request #10419 from hyperlogic/feature/ik-solver-init-config

More stable IK when hips, feet and chest are controlled via external input

interface/resources/avatar/avatar-animation.json

@@ -49,6 +49,8 @@
                         "id": "ik",
                         "type": "inverseKinematics",
                         "data": {
+                            "solutionSource": "relaxToUnderPoses",
+                            "solutionSourceVar": "solutionSource",
                             "targets": [
                                 {
                                     "jointName": "Hips",

interface/src/Menu.cpp

@@ -523,6 +523,8 @@ Menu::Menu() {
         avatar.get(), SLOT(setEnableDebugDrawSensorToWorldMatrix(bool)));
     addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderIKTargets, 0, false,
         avatar.get(), SLOT(setEnableDebugDrawIKTargets(bool)));
+    addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::RenderIKConstraints, 0, false,
+        avatar.get(), SLOT(setEnableDebugDrawIKConstraints(bool)));
 
     addCheckableActionToQMenuAndActionHash(avatarDebugMenu, MenuOption::ActionMotorControl,
         Qt::CTRL | Qt::SHIFT | Qt::Key_K, true, avatar.get(), SLOT(updateMotionBehaviorFromMenu()),

interface/src/Menu.h

@@ -161,6 +161,7 @@ namespace MenuOption {
     const QString RenderResolutionQuarter = "1/4";
     const QString RenderSensorToWorldMatrix = "Show SensorToWorld Matrix";
     const QString RenderIKTargets = "Show IK Targets";
+    const QString RenderIKConstraints = "Show IK Constraints";
     const QString ResetAvatarSize = "Reset Avatar Size";
     const QString ResetSensors = "Reset Sensors";
     const QString RunningScripts = "Running Scripts...";

interface/src/avatar/MyAvatar.cpp

@@ -38,6 +38,7 @@
 #include <UserActivityLogger.h>
 #include <AnimDebugDraw.h>
 #include <AnimClip.h>
+#include <AnimInverseKinematics.h>
 #include <recording/Deck.h>
 #include <recording/Recorder.h>
 #include <recording/Clip.h>
@@ -504,6 +505,7 @@ void MyAvatar::simulate(float deltaTime) {
 
         if (_rig) {
             _rig->setEnableDebugDrawIKTargets(_enableDebugDrawIKTargets);
+            _rig->setEnableDebugDrawIKConstraints(_enableDebugDrawIKConstraints);
         }
 
         _skeletonModel->simulate(deltaTime);
@@ -927,6 +929,10 @@ void MyAvatar::setEnableDebugDrawIKTargets(bool isEnabled) {
     _enableDebugDrawIKTargets = isEnabled;
 }
 
+void MyAvatar::setEnableDebugDrawIKConstraints(bool isEnabled) {
+    _enableDebugDrawIKConstraints = isEnabled;
+}
+
 void MyAvatar::setEnableMeshVisible(bool isEnabled) {
     _skeletonModel->setVisibleInScene(isEnabled, qApp->getMain3DScene());
 }

interface/src/avatar/MyAvatar.h

@@ -521,6 +521,7 @@ public slots:
     void setEnableDebugDrawHandControllers(bool isEnabled);
     void setEnableDebugDrawSensorToWorldMatrix(bool isEnabled);
     void setEnableDebugDrawIKTargets(bool isEnabled);
+    void setEnableDebugDrawIKConstraints(bool isEnabled);
     bool getEnableMeshVisible() const { return _skeletonModel->isVisible(); }
     void setEnableMeshVisible(bool isEnabled);
     void setUseAnimPreAndPostRotations(bool isEnabled);
@@ -706,6 +707,7 @@ private:
     bool _enableDebugDrawHandControllers { false };
     bool _enableDebugDrawSensorToWorldMatrix { false };
     bool _enableDebugDrawIKTargets { false };
+    bool _enableDebugDrawIKConstraints { false };
 
     AudioListenerMode _audioListenerMode;
     glm::vec3 _customListenPosition;

libraries/animation/src/AnimContext.cpp

@@ -10,7 +10,11 @@
 
 #include "AnimContext.h"
 
-AnimContext::AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix) :
+AnimContext::AnimContext(bool enableDebugDrawIKTargets, bool enableDebugDrawIKConstraints,
+                         const glm::mat4& geometryToRigMatrix, const glm::mat4& rigToWorldMatrix) :
     _enableDebugDrawIKTargets(enableDebugDrawIKTargets),
-    _geometryToRigMatrix(geometryToRigMatrix) {
+    _enableDebugDrawIKConstraints(enableDebugDrawIKConstraints),
+    _geometryToRigMatrix(geometryToRigMatrix),
+    _rigToWorldMatrix(rigToWorldMatrix)
+{
 }

libraries/animation/src/AnimContext.h

@@ -16,15 +16,20 @@
 
 class AnimContext {
 public:
-    AnimContext(bool enableDebugDrawIKTargets, const glm::mat4& geometryToRigMatrix);
+    AnimContext(bool enableDebugDrawIKTargets, bool enableDebugDrawIKConstraints,
+                const glm::mat4& geometryToRigMatrix, const glm::mat4& rigToWorldMatrix);
 
     bool getEnableDebugDrawIKTargets() const { return _enableDebugDrawIKTargets; }
+    bool getEnableDebugDrawIKConstraints() const { return _enableDebugDrawIKConstraints; }
     const glm::mat4& getGeometryToRigMatrix() const { return _geometryToRigMatrix; }
+    const glm::mat4& getRigToWorldMatrix() const { return _rigToWorldMatrix; }
 
 protected:
 
     bool _enableDebugDrawIKTargets { false };
+    bool _enableDebugDrawIKConstraints{ false };
     glm::mat4 _geometryToRigMatrix;
+    glm::mat4 _rigToWorldMatrix;
 };
 
 #endif  // hifi_AnimContext_h

libraries/animation/src/AnimInverseKinematics.cpp

@@ -399,6 +399,13 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
 //virtual
 const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) {
 
+    // allows solutionSource to be overridden by an animVar
+    auto solutionSource = animVars.lookup(_solutionSourceVar, (int)_solutionSource);
+
+    if (context.getEnableDebugDrawIKConstraints()) {
+        debugDrawConstraints(context);
+    }
+
     const float MAX_OVERLAY_DT = 1.0f / 30.0f; // what to clamp delta-time to in AnimInverseKinematics::overlay
     if (dt > MAX_OVERLAY_DT) {
         dt = MAX_OVERLAY_DT;
@@ -410,25 +417,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
 
         PROFILE_RANGE_EX(simulation_animation, "ik/relax", 0xffff00ff, 0);
 
-        // relax toward underPoses
+        initRelativePosesFromSolutionSource((SolutionSource)solutionSource, 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();
-        }
 
         if (!underPoses.empty()) {
             // Sometimes the underpose itself can violate the constraints.  Rather than
@@ -604,9 +593,9 @@ void AnimInverseKinematics::clearIKJointLimitHistory() {
     }
 }
 
-RotationConstraint* AnimInverseKinematics::getConstraint(int index) {
+RotationConstraint* AnimInverseKinematics::getConstraint(int index) const {
     RotationConstraint* constraint = nullptr;
-    std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.find(index);
+    std::map<int, RotationConstraint*>::const_iterator constraintItr = _constraints.find(index);
     if (constraintItr != _constraints.end()) {
         constraint = constraintItr->second;
     }
@@ -622,17 +611,19 @@ void AnimInverseKinematics::clearConstraints() {
     _constraints.clear();
 }
 
-// set up swing limits around a swingTwistConstraint in an ellipse, where lateralSwingTheta is the swing limit for lateral swings (side to side)
+// set up swing limits around a swingTwistConstraint in an ellipse, where lateralSwingPhi is the swing limit for lateral swings (side to side)
-// anteriorSwingTheta is swing limit for forward and backward swings.  (where x-axis of reference rotation is sideways and -z-axis is forward)
+// anteriorSwingPhi is swing limit for forward and backward swings.  (where x-axis of reference rotation is sideways and -z-axis is forward)
-static void setEllipticalSwingLimits(SwingTwistConstraint* stConstraint, float lateralSwingTheta, float anteriorSwingTheta) {
+static void setEllipticalSwingLimits(SwingTwistConstraint* stConstraint, float lateralSwingPhi, float anteriorSwingPhi) {
     assert(stConstraint);
-    const int NUM_SUBDIVISIONS = 8;
+    const int NUM_SUBDIVISIONS = 16;
     std::vector<float> minDots;
     minDots.reserve(NUM_SUBDIVISIONS);
     float dTheta = TWO_PI / NUM_SUBDIVISIONS;
     float theta = 0.0f;
     for (int i = 0; i < NUM_SUBDIVISIONS; i++) {
-        minDots.push_back(cosf(glm::length(glm::vec2(anteriorSwingTheta * cosf(theta), lateralSwingTheta * sinf(theta)))));
+        float theta_prime = atanf((lateralSwingPhi / anteriorSwingPhi) * tanf(theta));
+        float phi = (cosf(2.0f * theta_prime) * ((lateralSwingPhi - anteriorSwingPhi) / 2.0f)) + ((lateralSwingPhi + anteriorSwingPhi) / 2.0f);
+        minDots.push_back(cosf(phi));
         theta += dTheta;
     }
     stConstraint->setSwingLimits(minDots);
@@ -640,7 +631,6 @@ static void setEllipticalSwingLimits(SwingTwistConstraint* stConstraint, float l
 
 void AnimInverseKinematics::initConstraints() {
     if (!_skeleton) {
-        return;
     }
     // We create constraints for the joints shown here
     // (and their Left counterparts if applicable).
@@ -744,30 +734,27 @@ void AnimInverseKinematics::initConstraints() {
             std::vector<glm::vec3> swungDirections;
             float deltaTheta = PI / 4.0f;
             float theta = 0.0f;
-            swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.25f, sinf(theta)));
+            swungDirections.push_back(glm::vec3(mirror * cosf(theta), -0.25f, sinf(theta)));
             theta += deltaTheta;
             swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.0f, sinf(theta)));
             theta += deltaTheta;
-            swungDirections.push_back(glm::vec3(mirror * cosf(theta), -0.25f, sinf(theta))); // posterior
+            swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.25f, sinf(theta))); // posterior
             theta += deltaTheta;
             swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.0f, sinf(theta)));
             theta += deltaTheta;
-            swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.25f, sinf(theta)));
+            swungDirections.push_back(glm::vec3(mirror * cosf(theta), -0.25f, sinf(theta)));
             theta += deltaTheta;
-            swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.5f, sinf(theta)));
+            swungDirections.push_back(glm::vec3(mirror * cosf(theta), -0.5f, sinf(theta)));
             theta += deltaTheta;
-            swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.5f, sinf(theta))); // anterior
+            swungDirections.push_back(glm::vec3(mirror * cosf(theta), -0.5f, sinf(theta))); // anterior
             theta += deltaTheta;
-            swungDirections.push_back(glm::vec3(mirror * cosf(theta), 0.5f, sinf(theta)));
+            swungDirections.push_back(glm::vec3(mirror * cosf(theta), -0.5f, sinf(theta)));
 
-            // rotate directions into joint-frame
+            std::vector<float> minDots;
-            glm::quat invAbsoluteRotation = glm::inverse(absolutePoses[i].rot());
+            for (size_t i = 0; i < swungDirections.size(); i++) {
-            int numDirections = (int)swungDirections.size();
+                minDots.push_back(glm::dot(glm::normalize(swungDirections[i]), Vectors::UNIT_Y));
-            for (int j = 0; j < numDirections; ++j) {
-                swungDirections[j] = invAbsoluteRotation * swungDirections[j];
             }
-            stConstraint->setSwingLimits(swungDirections);
+            stConstraint->setSwingLimits(minDots);
-
             constraint = static_cast<RotationConstraint*>(stConstraint);
         } else if (0 == baseName.compare("Hand", Qt::CaseSensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
@@ -957,6 +944,32 @@ void AnimInverseKinematics::initConstraints() {
     }
 }
 
+void AnimInverseKinematics::initLimitCenterPoses() {
+    assert(_skeleton);
+    _limitCenterPoses.reserve(_skeleton->getNumJoints());
+    for (int i = 0; i < _skeleton->getNumJoints(); i++) {
+        AnimPose pose = _skeleton->getRelativeDefaultPose(i);
+        RotationConstraint* constraint = getConstraint(i);
+        if (constraint) {
+            pose.rot() = constraint->computeCenterRotation();
+        }
+        _limitCenterPoses.push_back(pose);
+    }
+
+    // The limit center rotations for the LeftArm and RightArm form a t-pose.
+    // In order for the elbows to look more natural, we rotate them down by the avatar's sides
+    const float UPPER_ARM_THETA = PI / 3.0f;  // 60 deg
+    int leftArmIndex = _skeleton->nameToJointIndex("LeftArm");
+    const glm::quat armRot = glm::angleAxis(UPPER_ARM_THETA, Vectors::UNIT_X);
+    if (leftArmIndex >= 0 && leftArmIndex < (int)_limitCenterPoses.size()) {
+        _limitCenterPoses[leftArmIndex].rot() = _limitCenterPoses[leftArmIndex].rot() * armRot;
+    }
+    int rightArmIndex = _skeleton->nameToJointIndex("RightArm");
+    if (rightArmIndex >= 0 && rightArmIndex < (int)_limitCenterPoses.size()) {
+        _limitCenterPoses[rightArmIndex].rot() = _limitCenterPoses[rightArmIndex].rot() * armRot;
+    }
+}
+
 void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) {
     AnimNode::setSkeletonInternal(skeleton);
 
@@ -973,6 +986,7 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele
 
     if (skeleton) {
         initConstraints();
+        initLimitCenterPoses();
         _headIndex = _skeleton->nameToJointIndex("Head");
         _hipsIndex = _skeleton->nameToJointIndex("Hips");
 
@@ -989,3 +1003,170 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele
         _hipsParentIndex = -1;
     }
 }
+
+static glm::vec3 sphericalToCartesian(float phi, float theta) {
+    float cos_phi = cosf(phi);
+    float sin_phi = sinf(phi);
+    return glm::vec3(sin_phi * cosf(theta), cos_phi, -sin_phi * sinf(theta));
+}
+
+void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) const {
+    if (_skeleton) {
+        const vec4 RED(1.0f, 0.0f, 0.0f, 1.0f);
+        const vec4 GREEN(0.0f, 1.0f, 0.0f, 1.0f);
+        const vec4 BLUE(0.0f, 0.0f, 1.0f, 1.0f);
+        const vec4 PURPLE(0.5f, 0.0f, 1.0f, 1.0f);
+        const vec4 CYAN(0.0f, 1.0f, 1.0f, 1.0f);
+        const vec4 GRAY(0.2f, 0.2f, 0.2f, 1.0f);
+        const vec4 MAGENTA(1.0f, 0.0f, 1.0f, 1.0f);
+        const float AXIS_LENGTH = 2.0f; // cm
+        const float TWIST_LENGTH = 4.0f; // cm
+        const float HINGE_LENGTH = 6.0f; // cm
+        const float SWING_LENGTH = 5.0f; // cm
+
+        AnimPoseVec poses = _skeleton->getRelativeDefaultPoses();
+
+        // copy reference rotations into the relative poses
+        for (int i = 0; i < (int)poses.size(); i++) {
+            const RotationConstraint* constraint = getConstraint(i);
+            if (constraint) {
+                poses[i].rot() = constraint->getReferenceRotation();
+            }
+        }
+
+        // convert relative poses to absolute
+        _skeleton->convertRelativePosesToAbsolute(poses);
+
+        mat4 geomToWorldMatrix = context.getRigToWorldMatrix() * context.getGeometryToRigMatrix();
+
+        // draw each pose and constraint
+        for (int i = 0; i < (int)poses.size(); i++) {
+            // transform local axes into world space.
+            auto pose = poses[i];
+            glm::vec3 xAxis = transformVectorFast(geomToWorldMatrix, pose.rot() * Vectors::UNIT_X);
+            glm::vec3 yAxis = transformVectorFast(geomToWorldMatrix, pose.rot() * Vectors::UNIT_Y);
+            glm::vec3 zAxis = transformVectorFast(geomToWorldMatrix, pose.rot() * Vectors::UNIT_Z);
+            glm::vec3 pos = transformPoint(geomToWorldMatrix, pose.trans());
+            DebugDraw::getInstance().drawRay(pos, pos + AXIS_LENGTH * xAxis, RED);
+            DebugDraw::getInstance().drawRay(pos, pos + AXIS_LENGTH * yAxis, GREEN);
+            DebugDraw::getInstance().drawRay(pos, pos + AXIS_LENGTH * zAxis, BLUE);
+
+            // draw line to parent
+            int parentIndex = _skeleton->getParentIndex(i);
+            if (parentIndex != -1) {
+                glm::vec3 parentPos = transformPoint(geomToWorldMatrix, poses[parentIndex].trans());
+                DebugDraw::getInstance().drawRay(pos, parentPos, GRAY);
+            }
+
+            glm::quat parentAbsRot;
+            if (parentIndex != -1) {
+                parentAbsRot = poses[parentIndex].rot();
+            }
+
+            const RotationConstraint* constraint = getConstraint(i);
+            if (constraint) {
+                glm::quat refRot = constraint->getReferenceRotation();
+                const ElbowConstraint* elbowConstraint = dynamic_cast<const ElbowConstraint*>(constraint);
+                if (elbowConstraint) {
+                    glm::vec3 hingeAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * elbowConstraint->getHingeAxis());
+                    DebugDraw::getInstance().drawRay(pos, pos + HINGE_LENGTH * hingeAxis, MAGENTA);
+
+                    // draw elbow constraints
+                    glm::quat minRot = glm::angleAxis(elbowConstraint->getMinAngle(), elbowConstraint->getHingeAxis());
+                    glm::quat maxRot = glm::angleAxis(elbowConstraint->getMaxAngle(), elbowConstraint->getHingeAxis());
+
+                    const int NUM_SWING_STEPS = 10;
+                    for (int i = 0; i < NUM_SWING_STEPS + 1; i++) {
+                        glm::quat rot = glm::normalize(glm::lerp(minRot, maxRot, i * (1.0f / NUM_SWING_STEPS)));
+                        glm::vec3 axis = transformVectorFast(geomToWorldMatrix, parentAbsRot * rot * refRot * Vectors::UNIT_Y);
+                        DebugDraw::getInstance().drawRay(pos, pos + TWIST_LENGTH * axis, CYAN);
+                    }
+
+                } else {
+                    const SwingTwistConstraint* swingTwistConstraint = dynamic_cast<const SwingTwistConstraint*>(constraint);
+                    if (swingTwistConstraint) {
+                        // twist constraints
+
+                        glm::vec3 hingeAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * Vectors::UNIT_Y);
+                        DebugDraw::getInstance().drawRay(pos, pos + HINGE_LENGTH * hingeAxis, MAGENTA);
+
+                        glm::quat minRot = glm::angleAxis(swingTwistConstraint->getMinTwist(), Vectors::UNIT_Y);
+                        glm::quat maxRot = glm::angleAxis(swingTwistConstraint->getMaxTwist(), Vectors::UNIT_Y);
+
+                        const int NUM_SWING_STEPS = 10;
+                        for (int i = 0; i < NUM_SWING_STEPS + 1; i++) {
+                            glm::quat rot = glm::normalize(glm::lerp(minRot, maxRot, i * (1.0f / NUM_SWING_STEPS)));
+                            glm::vec3 axis = transformVectorFast(geomToWorldMatrix, parentAbsRot * rot * refRot * Vectors::UNIT_X);
+                            DebugDraw::getInstance().drawRay(pos, pos + TWIST_LENGTH * axis, CYAN);
+                        }
+
+                        // draw swing constraints.
+                        const size_t NUM_MIN_DOTS = swingTwistConstraint->getMinDots().size();
+                        const float D_THETA = TWO_PI / (NUM_MIN_DOTS - 1);
+                        float theta = 0.0f;
+                        for (size_t i = 0, j = NUM_MIN_DOTS - 2; i < NUM_MIN_DOTS - 1; j = i, i++, theta += D_THETA) {
+                            // compute swing rotation from theta and phi angles.
+                            float phi = acosf(swingTwistConstraint->getMinDots()[i]);
+                            glm::vec3 swungAxis = sphericalToCartesian(phi, theta);
+                            glm::vec3 worldSwungAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * swungAxis);
+                            glm::vec3 swingTip = pos + SWING_LENGTH * worldSwungAxis;
+
+                            float prevPhi = acos(swingTwistConstraint->getMinDots()[j]);
+                            float prevTheta = theta - D_THETA;
+                            glm::vec3 prevSwungAxis = sphericalToCartesian(prevPhi, prevTheta);
+                            glm::vec3 prevWorldSwungAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * prevSwungAxis);
+                            glm::vec3 prevSwingTip = pos + SWING_LENGTH * prevWorldSwungAxis;
+
+                            DebugDraw::getInstance().drawRay(pos, swingTip, PURPLE);
+                            DebugDraw::getInstance().drawRay(prevSwingTip, swingTip, PURPLE);
+                        }
+                    }
+                }
+                pose.rot() = constraint->computeCenterRotation();
+            }
+        }
+    }
+}
+
+// for bones under IK, blend between previous solution (_relativePoses) to targetPoses
+// for bones NOT under IK, copy directly from underPoses.
+// mutates _relativePoses.
+void AnimInverseKinematics::blendToPoses(const AnimPoseVec& targetPoses, const AnimPoseVec& underPoses, float blendFactor) {
+    // relax toward poses
+    int numJoints = (int)_relativePoses.size();
+    for (int i = 0; i < numJoints; ++i) {
+        float dotSign = copysignf(1.0f, glm::dot(_relativePoses[i].rot(), targetPoses[i].rot()));
+        if (_accumulators[i].isDirty()) {
+            // this joint is affected by IK --> blend toward the targetPoses rotation
+            _relativePoses[i].rot() = glm::normalize(glm::lerp(_relativePoses[i].rot(), dotSign * targetPoses[i].rot(), blendFactor));
+        } else {
+            // this joint is NOT affected by IK --> slam to underPoses rotation
+            _relativePoses[i].rot() = underPoses[i].rot();
+        }
+        _relativePoses[i].trans() = underPoses[i].trans();
+    }
+}
+
+void AnimInverseKinematics::initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPoses) {
+    const float RELAX_BLEND_FACTOR = (1.0f / 16.0f);
+    const float COPY_BLEND_FACTOR = 1.0f;
+    switch (solutionSource) {
+    default:
+    case SolutionSource::RelaxToUnderPoses:
+        blendToPoses(underPoses, underPoses, RELAX_BLEND_FACTOR);
+        break;
+    case SolutionSource::RelaxToLimitCenterPoses:
+        blendToPoses(_limitCenterPoses, underPoses, RELAX_BLEND_FACTOR);
+        break;
+    case SolutionSource::PreviousSolution:
+        // do nothing... _relativePoses is already the previous solution
+        break;
+    case SolutionSource::UnderPoses:
+        _relativePoses = underPoses;
+        break;
+    case SolutionSource::LimitCenterPoses:
+        // essentially copy limitCenterPoses over to _relativePoses.
+        blendToPoses(_limitCenterPoses, underPoses, COPY_BLEND_FACTOR);
+        break;
+    }
+}

libraries/animation/src/AnimInverseKinematics.h

@@ -43,18 +43,34 @@ public:
 
     float getMaxErrorOnLastSolve() { return _maxErrorOnLastSolve; }
 
+    enum class SolutionSource {
+        RelaxToUnderPoses = 0,
+        RelaxToLimitCenterPoses,
+        PreviousSolution,
+        UnderPoses,
+        LimitCenterPoses,
+        NumSolutionSources,
+    };
+
+    void setSolutionSource(SolutionSource solutionSource) { _solutionSource = solutionSource; }
+    void setSolutionSourceVar(const QString& solutionSourceVar) { _solutionSourceVar = solutionSourceVar; }
+
 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(SolutionSource solutionSource, const AnimPoseVec& underPose);
+    void blendToPoses(const AnimPoseVec& targetPoses, const AnimPoseVec& underPose, float blendFactor);
 
     // for AnimDebugDraw rendering
     virtual const AnimPoseVec& getPosesInternal() const override { return _relativePoses; }
 
-    RotationConstraint* getConstraint(int index);
+    RotationConstraint* getConstraint(int index) const;
     void clearConstraints();
     void initConstraints();
+    void initLimitCenterPoses();
     void computeHipsOffset(const std::vector<IKTarget>& targets, const AnimPoseVec& underPoses, float dt);
 
     // no copies
@@ -85,6 +101,7 @@ protected:
     std::vector<IKTargetVar> _targetVarVec;
     AnimPoseVec _defaultRelativePoses; // poses of the relaxed state
     AnimPoseVec _relativePoses; // current relative poses
+    AnimPoseVec _limitCenterPoses;  // relative
 
     // experimental data for moving hips during IK
     glm::vec3 _hipsOffset { Vectors::ZERO };
@@ -100,6 +117,8 @@ protected:
 
     float _maxErrorOnLastSolve { FLT_MAX };
     bool _previousEnableDebugIKTargets { false };
+    SolutionSource _solutionSource { SolutionSource::RelaxToUnderPoses };
+    QString _solutionSourceVar;
 };
 
 #endif // hifi_AnimInverseKinematics_h

libraries/animation/src/AnimNodeLoader.cpp

@@ -352,6 +352,23 @@ static AnimOverlay::BoneSet stringToBoneSetEnum(const QString& str) {
     return AnimOverlay::NumBoneSets;
 }
 
+static const char* solutionSourceStrings[(int)AnimInverseKinematics::SolutionSource::NumSolutionSources] = {
+    "relaxToUnderPoses",
+    "relaxToLimitCenterPoses",
+    "previousSolution",
+    "underPoses",
+    "limitCenterPoses"
+};
+
+static AnimInverseKinematics::SolutionSource stringToSolutionSourceEnum(const QString& str) {
+    for (int i = 0; i < (int)AnimInverseKinematics::SolutionSource::NumSolutionSources; i++) {
+        if (str == solutionSourceStrings[i]) {
+            return (AnimInverseKinematics::SolutionSource)i;
+        }
+    }
+    return AnimInverseKinematics::SolutionSource::NumSolutionSources;
+}
+
 static AnimNode::Pointer loadOverlayNode(const QJsonObject& jsonObj, const QString& id, const QUrl& jsonUrl) {
 
     READ_STRING(boneSet, jsonObj, id, jsonUrl, nullptr);
@@ -457,6 +474,23 @@ AnimNode::Pointer loadInverseKinematicsNode(const QJsonObject& jsonObj, const QS
         node->setTargetVars(jointName, positionVar, rotationVar, typeVar);
     };
 
+    READ_OPTIONAL_STRING(solutionSource, jsonObj);
+
+    if (!solutionSource.isEmpty()) {
+        AnimInverseKinematics::SolutionSource solutionSourceType = stringToSolutionSourceEnum(solutionSource);
+        if (solutionSourceType != AnimInverseKinematics::SolutionSource::NumSolutionSources) {
+            node->setSolutionSource(solutionSourceType);
+        } else {
+            qCWarning(animation) << "AnimNodeLoader, bad solutionSourceType in \"solutionSource\", id = " << id << ", url = " << jsonUrl.toDisplayString();
+        }
+    }
+
+    READ_OPTIONAL_STRING(solutionSourceVar, jsonObj);
+
+    if (!solutionSourceVar.isEmpty()) {
+        node->setSolutionSourceVar(solutionSourceVar);
+    }
+
     return node;
 }
 

libraries/animation/src/AnimPose.cpp

@@ -39,7 +39,7 @@ glm::vec3 AnimPose::xformPoint(const glm::vec3& rhs) const {
     return *this * rhs;
 }
 
-// really slow
+// really slow, but accurate for transforms with non-uniform scale
 glm::vec3 AnimPose::xformVector(const glm::vec3& rhs) const {
     glm::vec3 xAxis = _rot * glm::vec3(_scale.x, 0.0f, 0.0f);
     glm::vec3 yAxis = _rot * glm::vec3(0.0f, _scale.y, 0.0f);
@@ -49,6 +49,11 @@ glm::vec3 AnimPose::xformVector(const glm::vec3& rhs) const {
     return transInvMat * rhs;
 }
 
+// faster, but does not handle non-uniform scale correctly.
+glm::vec3 AnimPose::xformVectorFast(const glm::vec3& rhs) const {
+    return _rot * (_scale * rhs);
+}
+
 AnimPose AnimPose::operator*(const AnimPose& rhs) const {
     glm::mat4 result;
     glm_mat4u_mul(*this, rhs, result);

libraries/animation/src/AnimPose.h

@@ -25,7 +25,8 @@ public:
     static const AnimPose identity;
 
     glm::vec3 xformPoint(const glm::vec3& rhs) const;
-    glm::vec3 xformVector(const glm::vec3& rhs) const;  // really slow
+    glm::vec3 xformVector(const glm::vec3& rhs) const;  // really slow, but accurate for transforms with non-uniform scale
+    glm::vec3 xformVectorFast(const glm::vec3& rhs) const;  // faster, but does not handle non-uniform scale correctly.
 
     glm::vec3 operator*(const glm::vec3& rhs) const; // same as xformPoint
     AnimPose operator*(const AnimPose& rhs) const;

libraries/animation/src/AnimUtil.cpp

@@ -33,6 +33,23 @@ void blend(size_t numPoses, const AnimPose* a, const AnimPose* b, float alpha, A
     }
 }
 
+glm::quat averageQuats(size_t numQuats, const glm::quat* quats) {
+    if (numQuats == 0) {
+        return glm::quat();
+    }
+    glm::quat accum = quats[0];
+    glm::quat firstRot = quats[0];
+    for (size_t i = 1; i < numQuats; i++) {
+        glm::quat rot = quats[i];
+        float dot = glm::dot(firstRot, rot);
+        if (dot < 0.0f) {
+            rot = -rot;
+        }
+        accum += rot;
+    }
+    return glm::normalize(accum);
+}
+
 float accumulateTime(float startFrame, float endFrame, float timeScale, float currentFrame, float dt, bool loopFlag,
                      const QString& id, AnimNode::Triggers& triggersOut) {
 

libraries/animation/src/AnimUtil.h

@@ -16,9 +16,9 @@
 // this is where the magic happens
 void blend(size_t numPoses, const AnimPose* a, const AnimPose* b, float alpha, AnimPose* result);
 
+glm::quat averageQuats(size_t numQuats, const glm::quat* quats);
+
 float accumulateTime(float startFrame, float endFrame, float timeScale, float currentFrame, float dt, bool loopFlag,
                      const QString& id, AnimNode::Triggers& triggersOut);
 
 #endif
-
-

libraries/animation/src/ElbowConstraint.cpp

@@ -13,6 +13,7 @@
 
 #include <GeometryUtil.h>
 #include <NumericalConstants.h>
+#include "AnimUtil.h"
 
 ElbowConstraint::ElbowConstraint() :
         _minAngle(-PI),
@@ -77,3 +78,10 @@ bool ElbowConstraint::apply(glm::quat& rotation) const {
     return false;
 }
 
+glm::quat ElbowConstraint::computeCenterRotation() const {
+    const size_t NUM_LIMITS = 2;
+    glm::quat limits[NUM_LIMITS];
+    limits[0] = glm::angleAxis(_minAngle, _axis) * _referenceRotation;
+    limits[1] = glm::angleAxis(_maxAngle, _axis) * _referenceRotation;
+    return averageQuats(NUM_LIMITS, limits);
+}

libraries/animation/src/ElbowConstraint.h

@@ -18,6 +18,12 @@ public:
     void setHingeAxis(const glm::vec3& axis);
     void setAngleLimits(float minAngle, float maxAngle);
     virtual bool apply(glm::quat& rotation) const override;
+    virtual glm::quat computeCenterRotation() const override;
+
+    glm::vec3 getHingeAxis() const { return _axis; }
+    float getMinAngle() const { return _minAngle; }
+    float getMaxAngle() const { return _maxAngle; }
+
 protected:
     glm::vec3 _axis;
     glm::vec3 _perpAxis;

libraries/animation/src/Rig.cpp

@@ -305,30 +305,35 @@ void Rig::clearJointAnimationPriority(int index) {
     }
 }
 
-void Rig::clearIKJointLimitHistory() {
+std::shared_ptr<AnimInverseKinematics> Rig::getAnimInverseKinematicsNode() const {
+    std::shared_ptr<AnimInverseKinematics> result;
     if (_animNode) {
         _animNode->traverse([&](AnimNode::Pointer node) {
             // only report clip nodes as valid roles.
             auto ikNode = std::dynamic_pointer_cast<AnimInverseKinematics>(node);
             if (ikNode) {
-                ikNode->clearIKJointLimitHistory();
+                result = ikNode;
+                return false;
+            } else {
+                return true;
             }
-            return true;
         });
     }
+    return result;
+}
+
+void Rig::clearIKJointLimitHistory() {
+    auto ikNode = getAnimInverseKinematicsNode();
+    if (ikNode) {
+        ikNode->clearIKJointLimitHistory();
+    }
 }
 
 void Rig::setMaxHipsOffsetLength(float maxLength) {
     _maxHipsOffsetLength = maxLength;
-
+    auto ikNode = getAnimInverseKinematicsNode();
-    if (_animNode) {
+    if (ikNode) {
-        _animNode->traverse([&](AnimNode::Pointer node) {
+        ikNode->setMaxHipsOffsetLength(_maxHipsOffsetLength);
-            auto ikNode = std::dynamic_pointer_cast<AnimInverseKinematics>(node);
-            if (ikNode) {
-                ikNode->setMaxHipsOffsetLength(_maxHipsOffsetLength);
-            }
-            return true;
-        });
     }
 }
 
@@ -936,7 +941,7 @@ void Rig::updateAnimationStateHandlers() { // called on avatar update thread (wh
     }
 }
 
-void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) {
+void Rig::updateAnimations(float deltaTime, const glm::mat4& rootTransform, const glm::mat4& rigToWorldTransform) {
 
     PROFILE_RANGE_EX(simulation_animation_detail, __FUNCTION__, 0xffff00ff, 0);
     PerformanceTimer perfTimer("updateAnimations");
@@ -949,7 +954,8 @@ void Rig::updateAnimations(float deltaTime, glm::mat4 rootTransform) {
         updateAnimationStateHandlers();
         _animVars.setRigToGeometryTransform(_rigToGeometryTransform);
 
-        AnimContext context(_enableDebugDrawIKTargets, getGeometryToRigTransform());
+        AnimContext context(_enableDebugDrawIKTargets, _enableDebugDrawIKConstraints,
+                            getGeometryToRigTransform(), rigToWorldTransform);
 
         // evaluate the animation
         AnimNode::Triggers triggersOut;
@@ -1025,10 +1031,12 @@ void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) {
     _animVars.set("notIsTalking", !params.isTalking);
 
     if (params.hipsEnabled) {
+        _animVars.set("solutionSource", (int)AnimInverseKinematics::SolutionSource::RelaxToLimitCenterPoses);
         _animVars.set("hipsType", (int)IKTarget::Type::RotationAndPosition);
         _animVars.set("hipsPosition", extractTranslation(params.hipsMatrix));
         _animVars.set("hipsRotation", glmExtractRotation(params.hipsMatrix));
     } else {
+        _animVars.set("solutionSource", (int)AnimInverseKinematics::SolutionSource::RelaxToUnderPoses);
         _animVars.set("hipsType", (int)IKTarget::Type::Unknown);
     }
 
@@ -1440,7 +1448,7 @@ void Rig::computeAvatarBoundingCapsule(
 
     // call overlay twice: once to verify AnimPoseVec joints and again to do the IK
     AnimNode::Triggers triggersOut;
-    AnimContext context(false, glm::mat4());
+    AnimContext context(false, false, glm::mat4(), glm::mat4());
     float dt = 1.0f; // the value of this does not matter
     ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses());
     AnimPoseVec finalPoses =  ikNode.overlay(animVars, context, dt, triggersOut, _animSkeleton->getRelativeBindPoses());

libraries/animation/src/Rig.h

@@ -26,6 +26,7 @@
 #include "SimpleMovingAverage.h"
 
 class Rig;
+class AnimInverseKinematics;
 typedef std::shared_ptr<Rig> RigPointer;
 
 // Rig instances are reentrant.
@@ -111,6 +112,8 @@ public:
     void clearJointStates();
     void clearJointAnimationPriority(int index);
 
+    std::shared_ptr<AnimInverseKinematics> getAnimInverseKinematicsNode() const;
+
     void clearIKJointLimitHistory();
     void setMaxHipsOffsetLength(float maxLength);
     float getMaxHipsOffsetLength() const;
@@ -159,7 +162,7 @@ public:
     void computeMotionAnimationState(float deltaTime, const glm::vec3& worldPosition, const glm::vec3& worldVelocity, const glm::quat& worldRotation, CharacterControllerState ccState);
 
     // Regardless of who started the animations or how many, update the joints.
-    void updateAnimations(float deltaTime, glm::mat4 rootTransform);
+    void updateAnimations(float deltaTime, const glm::mat4& rootTransform, const glm::mat4& rigToWorldTransform);
 
     // legacy
     void inverseKinematics(int endIndex, glm::vec3 targetPosition, const glm::quat& targetRotation, float priority,
@@ -228,6 +231,7 @@ public:
     const glm::mat4& getGeometryToRigTransform() const { return _geometryToRigTransform; }
 
     void setEnableDebugDrawIKTargets(bool enableDebugDrawIKTargets) { _enableDebugDrawIKTargets = enableDebugDrawIKTargets; }
+    void setEnableDebugDrawIKConstraints(bool enableDebugDrawIKConstraints) { _enableDebugDrawIKConstraints = enableDebugDrawIKConstraints; }
 
     // input assumed to be in rig space
     void computeHeadFromHMD(const AnimPose& hmdPose, glm::vec3& headPositionOut, glm::quat& headOrientationOut) const;
@@ -338,6 +342,7 @@ protected:
     float _maxHipsOffsetLength { 1.0f };
 
     bool _enableDebugDrawIKTargets { false };
+    bool _enableDebugDrawIKConstraints { false };
 
 private:
     QMap<int, StateHandler> _stateHandlers;

libraries/animation/src/RotationConstraint.h

@@ -38,6 +38,9 @@ public:
     /// \brief reset any remembered joint limit history
     virtual void clearHistory() {};
 
+    /// \brief return the rotation that lies at the "center" of all the joint limits.
+    virtual glm::quat computeCenterRotation() const = 0;
+
 protected:
     glm::quat _referenceRotation = glm::quat();
 };

libraries/animation/src/SwingTwistConstraint.cpp

@@ -15,6 +15,7 @@
 #include <GeometryUtil.h>
 #include <GLMHelpers.h>
 #include <NumericalConstants.h>
+#include "AnimUtil.h"
 
 
 const float MIN_MINDOT = -0.999f;
@@ -430,3 +431,33 @@ void SwingTwistConstraint::dynamicallyAdjustLimits(const glm::quat& rotation) {
 void SwingTwistConstraint::clearHistory() {
     _lastTwistBoundary = LAST_CLAMP_NO_BOUNDARY;
 }
+
+glm::quat SwingTwistConstraint::computeCenterRotation() const {
+    const size_t NUM_TWIST_LIMITS = 2;
+    const size_t NUM_MIN_DOTS = getMinDots().size();
+    std::vector<glm::quat> swingLimits;
+    swingLimits.reserve(NUM_MIN_DOTS);
+
+    glm::quat twistLimits[NUM_TWIST_LIMITS];
+    if (_minTwist != _maxTwist) {
+        // to ensure that twists do not flip the center rotation, we devide twist angle by 2.
+        twistLimits[0] = glm::angleAxis(_minTwist / 2.0f, _referenceRotation * Vectors::UNIT_Y);
+        twistLimits[1] = glm::angleAxis(_maxTwist / 2.0f, _referenceRotation * Vectors::UNIT_Y);
+    }
+    const float D_THETA = TWO_PI / (NUM_MIN_DOTS - 1);
+    float theta = 0.0f;
+    for (size_t i = 0; i < NUM_MIN_DOTS - 1; i++, theta += D_THETA) {
+        // compute swing rotation from theta and phi angles.
+        float phi = acos(getMinDots()[i]);
+        float cos_phi = getMinDots()[i];
+        float sin_phi = sinf(phi);
+        glm::vec3 swungAxis(sin_phi * cosf(theta), cos_phi, -sin_phi * sinf(theta));
+
+        // to ensure that swings > 90 degrees do not flip the center rotation, we devide phi / 2
+        glm::quat swing = glm::angleAxis(phi / 2, glm::normalize(glm::cross(Vectors::UNIT_Y, swungAxis)));
+        swingLimits.push_back(swing);
+    }
+    glm::quat averageSwing = averageQuats(swingLimits.size(), &swingLimits[0]);
+    glm::quat averageTwist = averageQuats(2, twistLimits);
+    return averageSwing * averageTwist * _referenceRotation;
+}

libraries/animation/src/SwingTwistConstraint.h

@@ -58,7 +58,7 @@ public:
     virtual void dynamicallyAdjustLimits(const glm::quat& rotation) override;
 
     // for testing purposes
-    const std::vector<float>& getMinDots() { return _swingLimitFunction.getMinDots(); }
+    const std::vector<float>& getMinDots() const { return _swingLimitFunction.getMinDots(); }
 
     // 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
@@ -81,7 +81,7 @@ public:
         float getMinDot(float theta) const;
 
         // for testing purposes
-        const std::vector<float>& getMinDots() { return _minDots; }
+        const std::vector<float>& getMinDots() const { return _minDots; }
 
     private:
         // the limits are stored in a lookup table with cyclic boundary conditions
@@ -99,6 +99,11 @@ public:
 
     void clearHistory() override;
 
+    virtual glm::quat computeCenterRotation() const override;
+
+    float getMinTwist() const { return _minTwist; }
+    float getMaxTwist() const { return _maxTwist; }
+
 private:
     float handleTwistBoundaryConditions(float twistAngle) const;
 

libraries/render-utils/src/Model.cpp

@@ -1046,7 +1046,8 @@ void Model::simulate(float deltaTime, bool fullUpdate) {
 //virtual
 void Model::updateRig(float deltaTime, glm::mat4 parentTransform) {
     _needsUpdateClusterMatrices = true;
-    _rig->updateAnimations(deltaTime, parentTransform);
+    glm::mat4 rigToWorldTransform = createMatFromQuatAndPos(getRotation(), getTranslation());
+    _rig->updateAnimations(deltaTime, parentTransform, rigToWorldTransform);
 }
 
 void Model::computeMeshPartLocalBounds() {