Merge pull request #5837 from AndrewMeadows/ik-repairs-002

blend rotations between distinct IK end effector solutions

libraries/animation/src/AnimInverseKinematics.cpp

@@ -153,6 +153,11 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
             ++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);
@@ -165,8 +170,8 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
         quint64 expiry = usecTimestampNow() + MAX_IK_TIME;
         do {
             largestError = 0.0f;
+            int lowestMovedIndex = _relativePoses.size();
             for (auto& target: targets) {
-                int lowestMovedIndex = _relativePoses.size() - 1;
                 int tipIndex = target.index;
                 AnimPose targetPose = target.pose;
                 int rootIndex = target.rootIndex;
@@ -226,7 +231,8 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
                                             glm::inverse(absolutePoses[pivotIndex].rot);
                                     }
                                 }
-                                _relativePoses[pivotIndex].rot = newRot;
+                                // 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) {
@@ -243,36 +249,47 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
                 if (largestError < error) {
                     largestError = error;
                 }
-
-                if (lowestMovedIndex <= _maxTargetIndex && lowestMovedIndex < tipIndex) {
-                    // only update the absolutePoses that matter: 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];
-                        }
-                    }
-                }
-
-                // finally set the relative rotation of the tip to agree with absolute target rotation
-                int parentIndex = _skeleton->getParentIndex(tipIndex);
-                if (parentIndex != -1) {
-                    // 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;
-                }
             }
             ++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;
+            }
+        }
     }
     return _relativePoses;
 }
@@ -628,6 +645,7 @@ void AnimInverseKinematics::setSkeletonInternal(AnimSkeleton::ConstPointer skele
 
     _maxTargetIndex = 0;
 
+    _accumulators.clear();
     if (skeleton) {
         initConstraints();
     } else {

libraries/animation/src/AnimInverseKinematics.h

@@ -12,8 +12,13 @@
 
 #include <string>
 
+#include <map>
+#include <vector>
+
 #include "AnimNode.h"
 
+#include "RotationAccumulator.h"
+
 class RotationConstraint;
 
 class AnimInverseKinematics : public AnimNode {
@@ -24,7 +29,6 @@ public:
 
     void loadDefaultPoses(const AnimPoseVec& poses);
     void loadPoses(const AnimPoseVec& poses);
-    const AnimPoseVec& getRelativePoses() const { return _relativePoses; }
     void computeAbsolutePoses(AnimPoseVec& absolutePoses) const;
 
     void setTargetVars(const QString& jointName, const QString& positionVar, const QString& rotationVar);
@@ -60,6 +64,7 @@ protected:
     };
 
     std::map<int, RotationConstraint*> _constraints;
+    std::map<int, RotationAccumulator> _accumulators;
     std::vector<IKTargetVar> _targetVarVec;
     AnimPoseVec _defaultRelativePoses; // poses of the relaxed state
     AnimPoseVec _relativePoses; // current relative poses

libraries/animation/src/RotationAccumulator.cpp

@@ -0,0 +1,27 @@
+//
+//  RotationAccumulator.h
+//
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "RotationAccumulator.h"
+
+#include <glm/gtx/quaternion.hpp>
+
+void RotationAccumulator::add(glm::quat rotation) {
+    // make sure both quaternions are on the same hyper-hemisphere before we add them linearly (lerp)
+    _rotationSum += copysignf(1.0f, glm::dot(_rotationSum, rotation)) * rotation;
+    ++_numRotations;
+}
+
+glm::quat RotationAccumulator::getAverage() {
+    return (_numRotations > 0) ?  glm::normalize(_rotationSum) : glm::quat();
+}
+
+void RotationAccumulator::clear() { 
+    _rotationSum *= 0.0f;
+    _numRotations = 0;
+}

libraries/animation/src/RotationAccumulator.h

@@ -0,0 +1,32 @@
+//
+//  RotationAccumulator.h
+//
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_RotationAccumulator_h
+#define hifi_RotationAccumulator_h
+
+#include <glm/gtc/quaternion.hpp>
+
+class RotationAccumulator {
+public:
+    RotationAccumulator() : _rotationSum(0.0f, 0.0f, 0.0f, 0.0f), _numRotations(0) { }
+
+    int size() const { return _numRotations; }
+
+    void add(glm::quat rotation);
+
+    glm::quat getAverage();
+
+    void clear();
+
+private:
+    glm::quat _rotationSum;
+    int _numRotations;
+};
+
+#endif // hifi_RotationAccumulator_h