Merge pull request #5874 from howard-stearns/AndrewMeadows-ik-repairs-003

cleanup IK implementation plusplus

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) {
@@ -113,7 +99,6 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
             }
         } else {
             // TODO: get this done without a double-lookup of each var in animVars
-            if (animVars.hasKey(targetVar.positionVar) || animVars.hasKey(targetVar.rotationVar)) {
             IKTarget target;
             AnimPose defaultPose = _skeleton->getAbsolutePose(targetVar.jointIndex, _relativePoses);
             target.pose.trans = animVars.lookup(targetVar.positionVar, defaultPose.trans);
@@ -123,7 +108,6 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
             targets.push_back(target);
         }
     }
-    }
 
     if (removeUnfoundJoints) {
         int numVars = _targetVarVec.size();
@@ -141,27 +125,18 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
             }
         }
     }
+}
+
+void AnimInverseKinematics::solveWithCyclicCoordinateDescent(std::vector<IKTarget>& targets) {
+    // compute absolute poses that correspond to relative target poses
+    AnimPoseVec absolutePoses;
+    computeAbsolutePoses(absolutePoses);
 
-    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 {
     // 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
-        AnimPoseVec absolutePoses;
-        computeAbsolutePoses(absolutePoses);
-
     float largestError = 0.0f;
     const float ACCEPTABLE_RELATIVE_ERROR = 1.0e-3f;
     int numLoops = 0;
@@ -174,13 +149,6 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
         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);
@@ -290,6 +258,28 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
             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.cpp

@@ -0,0 +1,54 @@
+//
+//  AnimNode.cpp
+//
+//  Created by Anthony J. Thibault on 9/2/15.
+//  Copyright (c) 2015 High Fidelity, Inc. All rights reserved.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "AnimNode.h"
+
+void AnimNode::removeChild(AnimNode::Pointer child) {
+    auto iter = std::find(_children.begin(), _children.end(), child);
+    if (iter != _children.end()) {
+        _children.erase(iter);
+    }
+}
+
+AnimNode::Pointer AnimNode::getChild(int i) const {
+    assert(i >= 0 && i < (int)_children.size());
+    return _children[i];
+}
+
+void AnimNode::setSkeleton(const AnimSkeleton::Pointer skeleton) {
+    setSkeletonInternal(skeleton);
+    for (auto&& child : _children) {
+        child->setSkeleton(skeleton);
+    }
+}
+
+const AnimPose AnimNode::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;
+}
+
+void AnimNode::setCurrentFrame(float frame) {
+    setCurrentFrameInternal(frame);
+    for (auto&& child : _children) {
+        child->setCurrentFrameInternal(frame);
+    }
+}

libraries/animation/src/AnimNode.h

@@ -60,25 +60,13 @@ public:
 
     // hierarchy accessors
     void addChild(Pointer child) { _children.push_back(child); }
-    void removeChild(Pointer child) {
+    void removeChild(Pointer child);
-        auto iter = std::find(_children.begin(), _children.end(), child);
+    
-        if (iter != _children.end()) {
+    Pointer getChild(int i) const;
-            _children.erase(iter);
-        }
-    }
-    Pointer getChild(int i) const {
-        assert(i >= 0 && i < (int)_children.size());
-        return _children[i];
-    }
     int getChildCount() const { return (int)_children.size(); }
 
     // pair this AnimNode graph with a skeleton.
-    void setSkeleton(const AnimSkeleton::Pointer skeleton) {
+    void setSkeleton(const AnimSkeleton::Pointer skeleton);
-        setSkeletonInternal(skeleton);
-        for (auto&& child : _children) {
-            child->setSkeleton(skeleton);
-        }
-    }
 
     AnimSkeleton::ConstPointer getSkeleton() const { return _skeleton; }
 
@@ -87,19 +75,14 @@ public:
         return evaluate(animVars, dt, triggersOut);
     }
 
+    const AnimPose getRootPose(int jointIndex) const;
+
 protected:
 
-    void setCurrentFrame(float frame) {
+    void setCurrentFrame(float frame);
-        setCurrentFrameInternal(frame);
-        for (auto&& child : _children) {
-            child->setCurrentFrameInternal(frame);
-        }
-    }
 
     virtual void setCurrentFrameInternal(float frame) {}
-    virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) {
+    virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) { _skeleton = skeleton; }
-        _skeleton = skeleton;
-    }
 
     // for AnimDebugDraw rendering
     virtual const AnimPoseVec& getPosesInternal() const = 0;

libraries/animation/src/Rig.cpp

@@ -986,16 +986,20 @@ 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) *
                                                   glm::angleAxis(glm::radians(-params.localHeadPitch), X_AXIS));
             _animVars.set("headRotation", realLocalHeadOrientation);
 
-            // There's a theory that when not in hmd, we should _animVars.unset("headPosition").
+            if (params.isInHMD) {
-            // However, until that works well, let's always request head be positioned where requested by hmd, camera, or default.
+                int headIndex = _animSkeleton->nameToJointIndex("Head");
-            _animVars.set("headPosition", params.localHeadPosition);
+                AnimPose rootPose = _animNode->getRootPose(headIndex);
+                _animVars.set("headPosition", rootPose.trans + params.localHeadPosition); // rootPose.rot is handled in params?d
+            } else {
+                _animVars.unset("headPosition");
+            }
         } else if (!_enableAnimGraph) {
 
             auto& state = _jointStates[index];
@@ -1044,20 +1048,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");