split AnimIK::evaluate() into sub-functions

also IK targets now in model-frame instead of root-frame

libraries/animation/src/AnimInverseKinematics.cpp

@@ -82,22 +82,8 @@ static int findRootJointInSkeleton(AnimSkeleton::ConstPointer skeleton, int inde
     return rootIndex;
 }
 
-struct IKTarget {
+void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets) {
-    AnimPose pose;
+    // build a list of valid targets from _targetVarVec and animVars
-    int index;
-    int rootIndex;
-};
-
-//virtual
-const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) {
-
-    // NOTE: we assume that _relativePoses are up to date (e.g. loadPoses() was just called)
-    if (_relativePoses.empty()) {
-        return _relativePoses;
-    }
-
-    // build a list of targets from _targetVarVec
-    std::vector<IKTarget> targets;
     bool removeUnfoundJoints = false;
     for (auto& targetVar : _targetVarVec) {
         if (targetVar.jointIndex == -1) {
@@ -141,154 +127,160 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
             }
         }
     }
+}
 
-    if (targets.empty()) {
+void AnimInverseKinematics::solveWithCyclicCoordinateDescent(std::vector<IKTarget>& targets) {
-        // no IK targets but still need to enforce constraints
+    // compute absolute poses that correspond to relative target poses
-        std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();
+    AnimPoseVec absolutePoses;
-        while (constraintItr != _constraints.end()) {
+    computeAbsolutePoses(absolutePoses);
-            int index = constraintItr->first;
-            glm::quat rotation = _relativePoses[index].rot;
-            constraintItr->second->apply(rotation);
-            _relativePoses[index].rot = rotation;
-            ++constraintItr;
-        }
-    } else {
-        // clear the accumulators before we start the IK solver
-        for (auto& accumulatorPair: _accumulators) {
-            accumulatorPair.second.clear();
-        }
 
-        // compute absolute poses that correspond to relative target poses
+    // clear the accumulators before we start the IK solver
-        AnimPoseVec absolutePoses;
+    for (auto& accumulatorPair: _accumulators) {
-        computeAbsolutePoses(absolutePoses);
+        accumulatorPair.second.clear();
+    }
 
-        float largestError = 0.0f;
+    float largestError = 0.0f;
-        const float ACCEPTABLE_RELATIVE_ERROR = 1.0e-3f;
+    const float ACCEPTABLE_RELATIVE_ERROR = 1.0e-3f;
-        int numLoops = 0;
+    int numLoops = 0;
-        const int MAX_IK_LOOPS = 16;
+    const int MAX_IK_LOOPS = 16;
-        const quint64 MAX_IK_TIME = 10 * USECS_PER_MSEC;
+    const quint64 MAX_IK_TIME = 10 * USECS_PER_MSEC;
-        quint64 expiry = usecTimestampNow() + MAX_IK_TIME;
+    quint64 expiry = usecTimestampNow() + MAX_IK_TIME;
-        do {
+    do {
-            largestError = 0.0f;
+        largestError = 0.0f;
-            int lowestMovedIndex = _relativePoses.size();
+        int lowestMovedIndex = _relativePoses.size();
-            for (auto& target: targets) {
-                int tipIndex = target.index;
-                AnimPose targetPose = target.pose;
-                int rootIndex = target.rootIndex;
-                if (rootIndex != -1) {
-                    // transform targetPose into skeleton's absolute frame
-                    AnimPose& rootPose = _relativePoses[rootIndex];
-                    targetPose.trans = rootPose.trans + rootPose.rot * targetPose.trans;
-                    targetPose.rot = rootPose.rot * targetPose.rot;
-                }
-
-                glm::vec3 tip = absolutePoses[tipIndex].trans;
-                float error = glm::length(targetPose.trans - tip);
-
-                // descend toward root, pivoting each joint to get tip closer to target
-                int pivotIndex = _skeleton->getParentIndex(tipIndex);
-                while (pivotIndex != -1 && error > ACCEPTABLE_RELATIVE_ERROR) {
-                    // compute the two lines that should be aligned
-                    glm::vec3 jointPosition = absolutePoses[pivotIndex].trans;
-                    glm::vec3 leverArm = tip - jointPosition;
-                    glm::vec3 targetLine = targetPose.trans - jointPosition;
-
-                    // compute the axis of the rotation that would align them
-                    glm::vec3 axis = glm::cross(leverArm, targetLine);
-                    float axisLength = glm::length(axis);
-                    if (axisLength > EPSILON) {
-                        // compute deltaRotation for alignment (brings tip closer to target)
-                        axis /= axisLength;
-                        float angle = acosf(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine)));
-
-                        // NOTE: even when axisLength is not zero (e.g. lever-arm and pivot-arm are not quite aligned) it is
-                        // still possible for the angle to be zero so we also check that to avoid unnecessary calculations.
-                        if (angle > EPSILON) {
-                            // reduce angle by half: slows convergence but adds stability to IK solution
-                            angle = 0.5f * angle;
-                            glm::quat deltaRotation = glm::angleAxis(angle, axis);
-
-                            int parentIndex = _skeleton->getParentIndex(pivotIndex);
-                            if (parentIndex == -1) {
-                                // TODO? apply constraints to root?
-                                // TODO? harvest the root's transform as movement of entire skeleton?
-                            } else {
-                                // compute joint's new parent-relative rotation
-                                // Q' = dQ * Q   and   Q = Qp * q   -->   q' = Qp^ * dQ * Q
-                                glm::quat newRot = glm::normalize(glm::inverse(
-                                        absolutePoses[parentIndex].rot) *
-                                        deltaRotation *
-                                        absolutePoses[pivotIndex].rot);
-                                RotationConstraint* constraint = getConstraint(pivotIndex);
-                                if (constraint) {
-                                    bool constrained = constraint->apply(newRot);
-                                    if (constrained) {
-                                        // the constraint will modify the movement of the tip so we have to compute the modified
-                                        // model-frame deltaRotation
-                                        // Q' = Qp^ * dQ * Q  -->  dQ =   Qp * Q' * Q^
-                                        deltaRotation = absolutePoses[parentIndex].rot *
-                                            newRot *
-                                            glm::inverse(absolutePoses[pivotIndex].rot);
-                                    }
-                                }
-                                // store the rotation change in the accumulator
-                                _accumulators[pivotIndex].add(newRot);
-                            }
-                            // 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 position as we descend towards root
-                            tip = jointPosition + deltaRotation * leverArm;
-                            error = glm::length(targetPose.trans - tip);
-                        }
-                    }
-                    pivotIndex = _skeleton->getParentIndex(pivotIndex);
-                }
-                if (largestError < error) {
-                    largestError = error;
-                }
-            }
-            ++numLoops;
-
-            // harvest accumulated rotations and apply the average
-            for (auto& accumulatorPair: _accumulators) {
-                RotationAccumulator& accumulator = accumulatorPair.second;
-                if (accumulator.size() > 0) {
-                    _relativePoses[accumulatorPair.first].rot = accumulator.getAverage();
-                    accumulator.clear();
-                }
-            }
-
-            // only update the absolutePoses that need it: those between lowestMovedIndex and _maxTargetIndex
-            for (int i = lowestMovedIndex; i <= _maxTargetIndex; ++i) {
-                int parentIndex = _skeleton->getParentIndex(i);
-                if (parentIndex != -1) {
-                    absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i];
-                }
-            }
-        } while (largestError > ACCEPTABLE_RELATIVE_ERROR && numLoops < MAX_IK_LOOPS && usecTimestampNow() < expiry);
-
-        // finally set the relative rotation of each tip to agree with absolute target rotation
         for (auto& target: targets) {
             int tipIndex = target.index;
-            int parentIndex = _skeleton->getParentIndex(tipIndex);
+            AnimPose targetPose = target.pose;
-            if (parentIndex != -1) {
+
-                AnimPose targetPose = target.pose;
+            glm::vec3 tip = absolutePoses[tipIndex].trans;
-                // compute tip's new parent-relative rotation
+            float error = glm::length(targetPose.trans - tip);
-                // Q = Qp * q   -->   q' = Qp^ * Q
+
-                glm::quat newRelativeRotation = glm::inverse(absolutePoses[parentIndex].rot) * targetPose.rot;
+            // descend toward root, pivoting each joint to get tip closer to target
-                RotationConstraint* constraint = getConstraint(tipIndex);
+            int pivotIndex = _skeleton->getParentIndex(tipIndex);
-                if (constraint) {
+            while (pivotIndex != -1 && error > ACCEPTABLE_RELATIVE_ERROR) {
-                    constraint->apply(newRelativeRotation);
+                // compute the two lines that should be aligned
-                    // TODO: ATM the final rotation target just fails but we need to provide
+                glm::vec3 jointPosition = absolutePoses[pivotIndex].trans;
-                    // feedback to the IK system so that it can adjust the bones up the skeleton
+                glm::vec3 leverArm = tip - jointPosition;
-                    // to help this rotation target get met.
+                glm::vec3 targetLine = targetPose.trans - jointPosition;
+
+                // compute the axis of the rotation that would align them
+                glm::vec3 axis = glm::cross(leverArm, targetLine);
+                float axisLength = glm::length(axis);
+                if (axisLength > EPSILON) {
+                    // compute deltaRotation for alignment (brings tip closer to target)
+                    axis /= axisLength;
+                    float angle = acosf(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine)));
+
+                    // NOTE: even when axisLength is not zero (e.g. lever-arm and pivot-arm are not quite aligned) it is
+                    // still possible for the angle to be zero so we also check that to avoid unnecessary calculations.
+                    if (angle > EPSILON) {
+                        // reduce angle by half: slows convergence but adds stability to IK solution
+                        angle = 0.5f * angle;
+                        glm::quat deltaRotation = glm::angleAxis(angle, axis);
+
+                        int parentIndex = _skeleton->getParentIndex(pivotIndex);
+                        if (parentIndex == -1) {
+                            // TODO? apply constraints to root?
+                            // TODO? harvest the root's transform as movement of entire skeleton?
+                        } else {
+                            // compute joint's new parent-relative rotation
+                            // Q' = dQ * Q   and   Q = Qp * q   -->   q' = Qp^ * dQ * Q
+                            glm::quat newRot = glm::normalize(glm::inverse(
+                                    absolutePoses[parentIndex].rot) *
+                                    deltaRotation *
+                                    absolutePoses[pivotIndex].rot);
+                            RotationConstraint* constraint = getConstraint(pivotIndex);
+                            if (constraint) {
+                                bool constrained = constraint->apply(newRot);
+                                if (constrained) {
+                                    // the constraint will modify the movement of the tip so we have to compute the modified
+                                    // model-frame deltaRotation
+                                    // Q' = Qp^ * dQ * Q  -->  dQ =   Qp * Q' * Q^
+                                    deltaRotation = absolutePoses[parentIndex].rot *
+                                        newRot *
+                                        glm::inverse(absolutePoses[pivotIndex].rot);
+                                }
+                            }
+                            // store the rotation change in the accumulator
+                            _accumulators[pivotIndex].add(newRot);
+                        }
+                        // 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 position as we descend towards root
+                        tip = jointPosition + deltaRotation * leverArm;
+                        error = glm::length(targetPose.trans - tip);
+                    }
                 }
-                _relativePoses[tipIndex].rot = newRelativeRotation;
+                pivotIndex = _skeleton->getParentIndex(pivotIndex);
-                absolutePoses[tipIndex].rot = targetPose.rot;
             }
+            if (largestError < error) {
+                largestError = error;
+            }
+        }
+        ++numLoops;
+
+        // harvest accumulated rotations and apply the average
+        for (auto& accumulatorPair: _accumulators) {
+            RotationAccumulator& accumulator = accumulatorPair.second;
+            if (accumulator.size() > 0) {
+                _relativePoses[accumulatorPair.first].rot = accumulator.getAverage();
+                accumulator.clear();
+            }
+        }
+
+        // only update the absolutePoses that need it: those between lowestMovedIndex and _maxTargetIndex
+        for (int i = lowestMovedIndex; i <= _maxTargetIndex; ++i) {
+            int parentIndex = _skeleton->getParentIndex(i);
+            if (parentIndex != -1) {
+                absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i];
+            }
+        }
+    } while (largestError > ACCEPTABLE_RELATIVE_ERROR && numLoops < MAX_IK_LOOPS && usecTimestampNow() < expiry);
+
+    // finally set the relative rotation of each tip to agree with absolute target rotation
+    for (auto& target: targets) {
+        int tipIndex = target.index;
+        int parentIndex = _skeleton->getParentIndex(tipIndex);
+        if (parentIndex != -1) {
+            AnimPose targetPose = target.pose;
+            // compute tip's new parent-relative rotation
+            // Q = Qp * q   -->   q' = Qp^ * Q
+            glm::quat newRelativeRotation = glm::inverse(absolutePoses[parentIndex].rot) * targetPose.rot;
+            RotationConstraint* constraint = getConstraint(tipIndex);
+            if (constraint) {
+                constraint->apply(newRelativeRotation);
+                // TODO: ATM the final rotation target just fails but we need to provide
+                // feedback to the IK system so that it can adjust the bones up the skeleton
+                // to help this rotation target get met.
+            }
+            _relativePoses[tipIndex].rot = newRelativeRotation;
+            absolutePoses[tipIndex].rot = targetPose.rot;
+        }
+    }
+}
+
+//virtual
+const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVars, float dt, AnimNode::Triggers& triggersOut) {
+    if (!_relativePoses.empty()) {
+        // build a list of targets from _targetVarVec
+        std::vector<IKTarget> targets;
+        computeTargets(animVars, targets);
+    
+        if (targets.empty()) {
+            // no IK targets but still need to enforce constraints
+            std::map<int, RotationConstraint*>::iterator constraintItr = _constraints.begin();
+            while (constraintItr != _constraints.end()) {
+                int index = constraintItr->first;
+                glm::quat rotation = _relativePoses[index].rot;
+                constraintItr->second->apply(rotation);
+                _relativePoses[index].rot = rotation;
+                ++constraintItr;
+            }
+        } else {
+            solveWithCyclicCoordinateDescent(targets);
         }
     }
     return _relativePoses;

libraries/animation/src/AnimInverseKinematics.h

@@ -37,6 +37,14 @@ public:
     virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override;
 
 protected:
+    struct IKTarget {
+        AnimPose pose;
+        int index;
+        int rootIndex;
+    };
+
+    void computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets);
+    void solveWithCyclicCoordinateDescent(std::vector<IKTarget>& targets);
     virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton);
 
     // for AnimDebugDraw rendering
@@ -64,7 +72,7 @@ protected:
     };
 
     std::map<int, RotationConstraint*> _constraints;
-    std::map<int, RotationAccumulator> _accumulators;
+    std::map<int, RotationAccumulator> _accumulators; // class-member to exploit temporal coherency
     std::vector<IKTargetVar> _targetVarVec;
     AnimPoseVec _defaultRelativePoses; // poses of the relaxed state
     AnimPoseVec _relativePoses; // current relative poses

libraries/animation/src/AnimNode.h

@@ -87,6 +87,23 @@ public:
         return evaluate(animVars, dt, triggersOut);
     }
 
+    const AnimPose getRootPose(int jointIndex) const {
+        AnimPose pose = AnimPose::identity;
+        if (_skeleton && jointIndex != -1) {
+            const AnimPoseVec& poses = getPosesInternal();
+            int numJoints = (int)(poses.size());
+            if (jointIndex < numJoints) {
+                int parentIndex = _skeleton->getParentIndex(jointIndex);
+                while (parentIndex != -1 && parentIndex < numJoints) {
+                    jointIndex = parentIndex;
+                    parentIndex = _skeleton->getParentIndex(jointIndex);
+                }
+                pose = poses[jointIndex];
+            }
+        }
+        return pose;
+    }
+
 protected:
 
     void setCurrentFrame(float frame) {

libraries/animation/src/Rig.cpp

@@ -986,7 +986,7 @@ void Rig::updateLeanJoint(int index, float leanSideways, float leanForward, floa
 
 void Rig::updateNeckJoint(int index, const HeadParameters& params) {
     if (index >= 0 && _jointStates[index].getParentIndex() >= 0) {
-        if (_enableAnimGraph && _animSkeleton) {
+        if (_enableAnimGraph && _animSkeleton && _animNode) {
             // the params.localHeadOrientation is composed incorrectly, so re-compose it correctly from pitch, yaw and roll.
             glm::quat realLocalHeadOrientation = (glm::angleAxis(glm::radians(-params.localHeadRoll), Z_AXIS) *
                                                   glm::angleAxis(glm::radians(params.localHeadYaw), Y_AXIS) *
@@ -995,7 +995,9 @@ void Rig::updateNeckJoint(int index, const HeadParameters& params) {
 
             // There's a theory that when not in hmd, we should _animVars.unset("headPosition").
             // However, until that works well, let's always request head be positioned where requested by hmd, camera, or default.
-            _animVars.set("headPosition", params.localHeadPosition);
+            int headIndex = _animSkeleton->nameToJointIndex("Head");
+            AnimPose rootPose = _animNode->getRootPose(headIndex);
+            _animVars.set("headPosition", rootPose.trans + rootPose.rot * params.localHeadPosition);
         } else if (!_enableAnimGraph) {
 
             auto& state = _jointStates[index];
@@ -1044,20 +1046,22 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm
 
 void Rig::updateFromHandParameters(const HandParameters& params, float dt) {
 
-    if (_enableAnimGraph && _animSkeleton) {
+    if (_enableAnimGraph && _animSkeleton && _animNode) {
 
         // TODO: figure out how to obtain the yFlip from where it is actually stored
         glm::quat yFlipHACK = glm::angleAxis(PI, glm::vec3(0.0f, 1.0f, 0.0f));
+        int leftHandIndex = _animSkeleton->nameToJointIndex("LeftHand");
+        AnimPose rootPose = _animNode->getRootPose(leftHandIndex);
         if (params.isLeftEnabled) {
-            _animVars.set("leftHandPosition", yFlipHACK * params.leftPosition);
+            _animVars.set("leftHandPosition", rootPose.trans + rootPose.rot * yFlipHACK * params.leftPosition);
-            _animVars.set("leftHandRotation", yFlipHACK * params.leftOrientation);
+            _animVars.set("leftHandRotation", rootPose.rot * yFlipHACK * params.leftOrientation);
         } else {
             _animVars.unset("leftHandPosition");
             _animVars.unset("leftHandRotation");
         }
         if (params.isRightEnabled) {
-            _animVars.set("rightHandPosition", yFlipHACK * params.rightPosition);
+            _animVars.set("rightHandPosition", rootPose.trans + rootPose.rot * yFlipHACK * params.rightPosition);
-            _animVars.set("rightHandRotation", yFlipHACK * params.rightOrientation);
+            _animVars.set("rightHandRotation", rootPose.rot * yFlipHACK * params.rightOrientation);
         } else {
             _animVars.unset("rightHandPosition");
             _animVars.unset("rightHandRotation");