Raised max iterations for IK to 16 from 4

* IK now returns early if solution is "good enough"
* IK now has nsight markers for profiling

libraries/animation/CMakeLists.txt

@@ -1,3 +1,5 @@
 set(TARGET_NAME animation)
 setup_hifi_library(Network Script)
 link_hifi_libraries(shared model fbx)
+
+target_nsight()

libraries/animation/src/AnimInverseKinematics.cpp

@@ -13,6 +13,7 @@
 #include <GLMHelpers.h>
 #include <NumericalConstants.h>
 #include <SharedUtil.h>
+#include <shared/NsightHelpers.h>
 
 #include "ElbowConstraint.h"
 #include "SwingTwistConstraint.h"
@@ -144,9 +145,12 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
         accumulator.clearAndClean();
     }
 
+    float maxError = FLT_MAX;
+    int HACK_MAX_ERROR_JOINT = -1;
     int numLoops = 0;
-    const int MAX_IK_LOOPS = 4;
+    const int MAX_IK_LOOPS = 16;
-    while (numLoops < MAX_IK_LOOPS) {
+    const float MAX_ERROR_TOLERANCE = 0.1f; // cm
+    while (maxError > MAX_ERROR_TOLERANCE && numLoops < MAX_IK_LOOPS) {
         ++numLoops;
 
         // solve all targets
@@ -173,6 +177,19 @@ void AnimInverseKinematics::solveWithCyclicCoordinateDescent(const std::vector<I
                 absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i];
             }
         }
+
+        // compute maxError
+        maxError = 0.0f;
+        for (int i = 0; i < targets.size(); i++) {
+            if (targets[i].getType() == IKTarget::Type::RotationAndPosition || targets[i].getType() == IKTarget::Type::HmdHead ||
+                targets[i].getType() == IKTarget::Type::HipsRelativeRotationAndPosition) {
+                float error = glm::length(absolutePoses[targets[i].getIndex()].trans - targets[i].getTranslation());
+                if (error > maxError) {
+                    maxError = error;
+                    HACK_MAX_ERROR_JOINT = targets[i].getIndex();
+                }
+            }
+        }
     }
 
     // finally set the relative rotation of each tip to agree with absolute target rotation
@@ -285,8 +302,8 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe
                 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;
+                    const float FRACTION = 0.5f;
-                    angle *= fraction;
+                    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
@@ -308,7 +325,7 @@ int AnimInverseKinematics::solveTargetWithCCD(const IKTarget& target, AnimPoseVe
                             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;
+                            deltaRotation = glm::normalize(glm::lerp(glm::quat(), dotSign * twistPart, FRACTION)) * deltaRotation;
                         }
                     }
                 }
@@ -369,6 +386,7 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
 
 //virtual
 const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) {
+
     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;
@@ -377,6 +395,9 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
     if (_relativePoses.size() != underPoses.size()) {
         loadPoses(underPoses);
     } else {
+
+        PROFILE_RANGE_EX("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
@@ -410,9 +431,13 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
     }
 
     if (!_relativePoses.empty()) {
+
         // build a list of targets from _targetVarVec
         std::vector<IKTarget> targets;
-        computeTargets(animVars, targets, underPoses);
+        {
+            PROFILE_RANGE_EX("ik/computeTargets", 0xffff00ff, 0);
+            computeTargets(animVars, targets, underPoses);
+        }
 
         if (targets.empty()) {
             // no IK targets but still need to enforce constraints
@@ -425,64 +450,76 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
                 ++constraintItr;
             }
         } else {
-            // shift hips according to the _hipsOffset from the previous frame
+
-            float offsetLength = glm::length(_hipsOffset);
+            {
-            const float MIN_HIPS_OFFSET_LENGTH = 0.03f;
+                PROFILE_RANGE_EX("ik/shiftHips", 0xffff00ff, 0);
-            if (offsetLength > MIN_HIPS_OFFSET_LENGTH && _hipsIndex >= 0) {
+
-                // but only if offset is long enough
+                // shift hips according to the _hipsOffset from the previous frame
-                float scaleFactor = ((offsetLength - MIN_HIPS_OFFSET_LENGTH) / offsetLength);
+                float offsetLength = glm::length(_hipsOffset);
-                if (_hipsParentIndex == -1) {
+                const float MIN_HIPS_OFFSET_LENGTH = 0.03f;
-                    // the hips are the root so _hipsOffset is in the correct frame
+                if (offsetLength > MIN_HIPS_OFFSET_LENGTH && _hipsIndex >= 0) {
-                    _relativePoses[_hipsIndex].trans = underPoses[_hipsIndex].trans + scaleFactor * _hipsOffset;
+                    // but only if offset is long enough
-                } else {
+                    float scaleFactor = ((offsetLength - MIN_HIPS_OFFSET_LENGTH) / offsetLength);
-                    // the hips are NOT the root so we need to transform _hipsOffset into hips local-frame
+                    if (_hipsParentIndex == -1) {
-                    glm::quat hipsFrameRotation = _relativePoses[_hipsParentIndex].rot;
+                        // the hips are the root so _hipsOffset is in the correct frame
-                    int index = _skeleton->getParentIndex(_hipsParentIndex);
+                        _relativePoses[_hipsIndex].trans = underPoses[_hipsIndex].trans + scaleFactor * _hipsOffset;
-                    while (index != -1) {
+                    } else {
-                        hipsFrameRotation *= _relativePoses[index].rot;
+                        // the hips are NOT the root so we need to transform _hipsOffset into hips local-frame
-                        index = _skeleton->getParentIndex(index);
+                        glm::quat hipsFrameRotation = _relativePoses[_hipsParentIndex].rot;
+                        int index = _skeleton->getParentIndex(_hipsParentIndex);
+                        while (index != -1) {
+                            hipsFrameRotation *= _relativePoses[index].rot;
+                            index = _skeleton->getParentIndex(index);
+                        }
+                        _relativePoses[_hipsIndex].trans = underPoses[_hipsIndex].trans
+                            + glm::inverse(glm::normalize(hipsFrameRotation)) * (scaleFactor * _hipsOffset);
                     }
-                    _relativePoses[_hipsIndex].trans = underPoses[_hipsIndex].trans
-                        + glm::inverse(glm::normalize(hipsFrameRotation)) * (scaleFactor * _hipsOffset);
                 }
             }
 
-            solveWithCyclicCoordinateDescent(targets);
+            {
-
+                PROFILE_RANGE_EX("ik/ccd", 0xffff00ff, 0);
-            // measure new _hipsOffset for next frame
+                solveWithCyclicCoordinateDescent(targets);
-            // 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;
-            for (auto& target: targets) {
-                int targetIndex = target.getIndex();
-                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
-                        // to where the head happens to be (overpose)
-                        glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans;
-                        glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
-                        const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f;
-                        newHipsOffset += HEAD_OFFSET_SLAVE_FACTOR * (actual - under);
-                    } else if (target.getType() == IKTarget::Type::HmdHead) {
-                        // we want to shift the hips to bring the head to its designated position
-                        glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
-                        _hipsOffset += target.getTranslation() - actual;
-                        // and ignore all other targets
-                        newHipsOffset = _hipsOffset;
-                        break;
-                    }
-                } else if (target.getType() == IKTarget::Type::RotationAndPosition) {
-                    glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans;
-                    glm::vec3 targetPosition = target.getTranslation();
-                    newHipsOffset += targetPosition - actualPosition;
-                }
             }
 
-            // smooth transitions by relaxing _hipsOffset toward the new value
+            {
-            const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.15f;
+                PROFILE_RANGE_EX("ik/measureHipsOffset", 0xffff00ff, 0);
-            float tau = dt < HIPS_OFFSET_SLAVE_TIMESCALE ?  dt / HIPS_OFFSET_SLAVE_TIMESCALE : 1.0f;
+
-            _hipsOffset += (newHipsOffset - _hipsOffset) * tau;
+                // 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;
+                for (auto& target: targets) {
+                    int targetIndex = target.getIndex();
+                    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
+                            // to where the head happens to be (overpose)
+                            glm::vec3 under = _skeleton->getAbsolutePose(_headIndex, underPoses).trans;
+                            glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
+                            const float HEAD_OFFSET_SLAVE_FACTOR = 0.65f;
+                            newHipsOffset += HEAD_OFFSET_SLAVE_FACTOR * (actual - under);
+                        } else if (target.getType() == IKTarget::Type::HmdHead) {
+                            // we want to shift the hips to bring the head to its designated position
+                            glm::vec3 actual = _skeleton->getAbsolutePose(_headIndex, _relativePoses).trans;
+                            _hipsOffset += target.getTranslation() - actual;
+                            // and ignore all other targets
+                            newHipsOffset = _hipsOffset;
+                            break;
+                        }
+                    } else if (target.getType() == IKTarget::Type::RotationAndPosition) {
+                        glm::vec3 actualPosition = _skeleton->getAbsolutePose(targetIndex, _relativePoses).trans;
+                        glm::vec3 targetPosition = target.getTranslation();
+                        newHipsOffset += targetPosition - actualPosition;
+                    }
+                }
+
+                // smooth transitions by relaxing _hipsOffset toward the new value
+                const float HIPS_OFFSET_SLAVE_TIMESCALE = 0.15f;
+                float tau = dt < HIPS_OFFSET_SLAVE_TIMESCALE ?  dt / HIPS_OFFSET_SLAVE_TIMESCALE : 1.0f;
+                _hipsOffset += (newHipsOffset - _hipsOffset) * tau;
+            }
         }
     }
     return _relativePoses;