Merge pull request #10730 from hyperlogic/feature/pole-vector

Pole vector support for elbow and knee joints

interface/resources/avatar/avatar-animation.json

@@ -68,7 +68,10 @@
                                     "typeVar": "rightHandType",
                                     "weightVar": "rightHandWeight",
                                     "weight": 1.0,
-                                    "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0]
+                                    "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0],
+                                    "poleVectorEnabledVar": "rightHandPoleVectorEnabled",
+                                    "poleReferenceVectorVar": "rightHandPoleReferenceVector",
+                                    "poleVectorVar": "rightHandPoleVector"
                                 },
                                 {
                                     "jointName": "LeftHand",
@@ -77,7 +80,10 @@
                                     "typeVar": "leftHandType",
                                     "weightVar": "leftHandWeight",
                                     "weight": 1.0,
-                                    "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0]
+                                    "flexCoefficients": [1, 0.5, 0.5, 0.2, 0.01, 0.005, 0.001, 0.0, 0.0],
+                                    "poleVectorEnabledVar": "leftHandPoleVectorEnabled",
+                                    "poleReferenceVectorVar": "leftHandPoleReferenceVector",
+                                    "poleVectorVar": "leftHandPoleVector"
                                 },
                                 {
                                     "jointName": "RightFoot",
@@ -86,7 +92,10 @@
                                     "typeVar": "rightFootType",
                                     "weightVar": "rightFootWeight",
                                     "weight": 1.0,
-                                    "flexCoefficients": [1, 0.45, 0.45]
+                                    "flexCoefficients": [1, 0.45, 0.45],
+                                    "poleVectorEnabledVar": "rightFootPoleVectorEnabled",
+                                    "poleReferenceVectorVar": "rightFootPoleReferenceVector",
+                                    "poleVectorVar": "rightFootPoleVector"
                                 },
                                 {
                                     "jointName": "LeftFoot",
@@ -95,7 +104,10 @@
                                     "typeVar": "leftFootType",
                                     "weightVar": "leftFootWeight",
                                     "weight": 1.0,
-                                    "flexCoefficients": [1, 0.45, 0.45]
+                                    "flexCoefficients": [1, 0.45, 0.45],
+                                    "poleVectorEnabledVar": "leftFootPoleVectorEnabled",
+                                    "poleReferenceVectorVar": "leftFootPoleReferenceVector",
+                                    "poleVectorVar": "leftFootPoleVector"
                                 },
                                 {
                                     "jointName": "Spine2",

interface/src/avatar/MySkeletonModel.cpp

@@ -47,110 +47,113 @@ void MySkeletonModel::updateRig(float deltaTime, glm::mat4 parentTransform) {
 
     MyAvatar* myAvatar = static_cast<MyAvatar*>(_owningAvatar);
 
-    Rig::HeadParameters headParams;
+    Rig::ControllerParameters params;
+
+    AnimPose avatarToRigPose(glm::vec3(1.0f), Quaternions::Y_180, glm::vec3(0.0f));
 
     // input action is the highest priority source for head orientation.
     auto avatarHeadPose = myAvatar->getHeadControllerPoseInAvatarFrame();
     if (avatarHeadPose.isValid()) {
-        glm::mat4 rigHeadMat = Matrices::Y_180 *
+        AnimPose pose(avatarHeadPose.getRotation(), avatarHeadPose.getTranslation());
-            createMatFromQuatAndPos(avatarHeadPose.getRotation(), avatarHeadPose.getTranslation());
+        params.controllerPoses[Rig::ControllerType_Head] = avatarToRigPose * pose;
-        headParams.rigHeadPosition = extractTranslation(rigHeadMat);
+        params.controllerActiveFlags[Rig::ControllerType_Head] = true;
-        headParams.rigHeadOrientation = glmExtractRotation(rigHeadMat);
-        headParams.headEnabled = true;
     } else {
         // even though full head IK is disabled, the rig still needs the head orientation to rotate the head up and
         // down in desktop mode.
         // preMult 180 is necessary to convert from avatar to rig coordinates.
         // postMult 180 is necessary to convert head from -z forward to z forward.
-        headParams.rigHeadOrientation = Quaternions::Y_180 * head->getFinalOrientationInLocalFrame() * Quaternions::Y_180;
+        glm::quat headRot = Quaternions::Y_180 * head->getFinalOrientationInLocalFrame() * Quaternions::Y_180;
-        headParams.headEnabled = false;
+        params.controllerPoses[Rig::ControllerType_Head] = AnimPose(glm::vec3(1.0f), headRot, glm::vec3(0.0f));
+        params.controllerActiveFlags[Rig::ControllerType_Head] = false;
     }
 
     auto avatarHipsPose = myAvatar->getHipsControllerPoseInAvatarFrame();
     if (avatarHipsPose.isValid()) {
-        glm::mat4 rigHipsMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarHipsPose.getRotation(), avatarHipsPose.getTranslation());
+        AnimPose pose(avatarHipsPose.getRotation(), avatarHipsPose.getTranslation());
-        headParams.hipsMatrix = rigHipsMat;
+        params.controllerPoses[Rig::ControllerType_Hips] = avatarToRigPose * pose;
-        headParams.hipsEnabled = true;
+        params.controllerActiveFlags[Rig::ControllerType_Hips] = true;
     } else {
-        headParams.hipsEnabled = false;
+        params.controllerPoses[Rig::ControllerType_Hips] = AnimPose::identity;
+        params.controllerActiveFlags[Rig::ControllerType_Hips] = false;
     }
 
     auto avatarSpine2Pose = myAvatar->getSpine2ControllerPoseInAvatarFrame();
     if (avatarSpine2Pose.isValid()) {
-        glm::mat4 rigSpine2Mat = Matrices::Y_180 * createMatFromQuatAndPos(avatarSpine2Pose.getRotation(), avatarSpine2Pose.getTranslation());
+        AnimPose pose(avatarSpine2Pose.getRotation(), avatarSpine2Pose.getTranslation());
-        headParams.spine2Matrix = rigSpine2Mat;
+        params.controllerPoses[Rig::ControllerType_Spine2] = avatarToRigPose * pose;
-        headParams.spine2Enabled = true;
+        params.controllerActiveFlags[Rig::ControllerType_Spine2] = true;
     } else {
-        headParams.spine2Enabled = false;
+        params.controllerPoses[Rig::ControllerType_Spine2] = AnimPose::identity;
+        params.controllerActiveFlags[Rig::ControllerType_Spine2] = false;
     }
 
     auto avatarRightArmPose = myAvatar->getRightArmControllerPoseInAvatarFrame();
     if (avatarRightArmPose.isValid()) {
-        glm::mat4 rightArmMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarRightArmPose.getRotation(), avatarRightArmPose.getTranslation());
+        AnimPose pose(avatarRightArmPose.getRotation(), avatarRightArmPose.getTranslation());
-        headParams.rightArmPosition = extractTranslation(rightArmMat);
+        params.controllerPoses[Rig::ControllerType_RightArm] = avatarToRigPose * pose;
-        headParams.rightArmRotation = glmExtractRotation(rightArmMat);
+        params.controllerActiveFlags[Rig::ControllerType_RightArm] = true;
-        headParams.rightArmEnabled = true;
     } else {
-        headParams.rightArmEnabled = false;
+        params.controllerPoses[Rig::ControllerType_RightArm] = AnimPose::identity;
+        params.controllerActiveFlags[Rig::ControllerType_RightArm] = false;
     }
-    
+
     auto avatarLeftArmPose = myAvatar->getLeftArmControllerPoseInAvatarFrame();
     if (avatarLeftArmPose.isValid()) {
-        glm::mat4 leftArmMat = Matrices::Y_180 * createMatFromQuatAndPos(avatarLeftArmPose.getRotation(), avatarLeftArmPose.getTranslation());
+        AnimPose pose(avatarLeftArmPose.getRotation(), avatarLeftArmPose.getTranslation());
-        headParams.leftArmPosition = extractTranslation(leftArmMat);
+        params.controllerPoses[Rig::ControllerType_LeftArm] = avatarToRigPose * pose;
-        headParams.leftArmRotation = glmExtractRotation(leftArmMat);
+        params.controllerActiveFlags[Rig::ControllerType_LeftArm] = true;
-        headParams.leftArmEnabled = true;
     } else {
-        headParams.leftArmEnabled = false;
+        params.controllerPoses[Rig::ControllerType_LeftArm] = AnimPose::identity;
+        params.controllerActiveFlags[Rig::ControllerType_LeftArm] = false;
     }
 
-    headParams.isTalking = head->getTimeWithoutTalking() <= 1.5f;
+    auto avatarLeftHandPose = myAvatar->getLeftHandControllerPoseInAvatarFrame();
-
+    if (avatarLeftHandPose.isValid()) {
-    _rig.updateFromHeadParameters(headParams, deltaTime);
+        AnimPose pose(avatarLeftHandPose.getRotation(), avatarLeftHandPose.getTranslation());
-
+        params.controllerPoses[Rig::ControllerType_LeftHand] = avatarToRigPose * pose;
-    Rig::HandAndFeetParameters handAndFeetParams;
+        params.controllerActiveFlags[Rig::ControllerType_LeftHand] = true;
-
-    auto leftPose = myAvatar->getLeftHandControllerPoseInAvatarFrame();
-    if (leftPose.isValid()) {
-        handAndFeetParams.isLeftEnabled = true;
-        handAndFeetParams.leftPosition = Quaternions::Y_180 * leftPose.getTranslation();
-        handAndFeetParams.leftOrientation = Quaternions::Y_180 * leftPose.getRotation();
     } else {
-        handAndFeetParams.isLeftEnabled = false;
+        params.controllerPoses[Rig::ControllerType_LeftHand] = AnimPose::identity;
+        params.controllerActiveFlags[Rig::ControllerType_LeftHand] = false;
     }
 
-    auto rightPose = myAvatar->getRightHandControllerPoseInAvatarFrame();
+    auto avatarRightHandPose = myAvatar->getRightHandControllerPoseInAvatarFrame();
-    if (rightPose.isValid()) {
+    if (avatarRightHandPose.isValid()) {
-        handAndFeetParams.isRightEnabled = true;
+        AnimPose pose(avatarRightHandPose.getRotation(), avatarRightHandPose.getTranslation());
-        handAndFeetParams.rightPosition = Quaternions::Y_180 * rightPose.getTranslation();
+        params.controllerPoses[Rig::ControllerType_RightHand] = avatarToRigPose * pose;
-        handAndFeetParams.rightOrientation = Quaternions::Y_180 * rightPose.getRotation();
+        params.controllerActiveFlags[Rig::ControllerType_RightHand] = true;
     } else {
-        handAndFeetParams.isRightEnabled = false;
+        params.controllerPoses[Rig::ControllerType_RightHand] = AnimPose::identity;
+        params.controllerActiveFlags[Rig::ControllerType_RightHand] = false;
     }
 
-    auto leftFootPose = myAvatar->getLeftFootControllerPoseInAvatarFrame();
+    auto avatarLeftFootPose = myAvatar->getLeftFootControllerPoseInAvatarFrame();
-    if (leftFootPose.isValid()) {
+    if (avatarLeftFootPose.isValid()) {
-        handAndFeetParams.isLeftFootEnabled = true;
+        AnimPose pose(avatarLeftFootPose.getRotation(), avatarLeftFootPose.getTranslation());
-        handAndFeetParams.leftFootPosition = Quaternions::Y_180 * leftFootPose.getTranslation();
+        params.controllerPoses[Rig::ControllerType_LeftFoot] = avatarToRigPose * pose;
-        handAndFeetParams.leftFootOrientation = Quaternions::Y_180 * leftFootPose.getRotation();
+        params.controllerActiveFlags[Rig::ControllerType_LeftFoot] = true;
     } else {
-        handAndFeetParams.isLeftFootEnabled = false;
+        params.controllerPoses[Rig::ControllerType_LeftFoot] = AnimPose::identity;
+        params.controllerActiveFlags[Rig::ControllerType_LeftFoot] = false;
     }
 
-    auto rightFootPose = myAvatar->getRightFootControllerPoseInAvatarFrame();
+    auto avatarRightFootPose = myAvatar->getRightFootControllerPoseInAvatarFrame();
-    if (rightFootPose.isValid()) {
+    if (avatarRightFootPose.isValid()) {
-        handAndFeetParams.isRightFootEnabled = true;
+        AnimPose pose(avatarRightFootPose.getRotation(), avatarRightFootPose.getTranslation());
-        handAndFeetParams.rightFootPosition = Quaternions::Y_180 * rightFootPose.getTranslation();
+        params.controllerPoses[Rig::ControllerType_RightFoot] = avatarToRigPose * pose;
-        handAndFeetParams.rightFootOrientation = Quaternions::Y_180 * rightFootPose.getRotation();
+        params.controllerActiveFlags[Rig::ControllerType_RightFoot] = true;
     } else {
-        handAndFeetParams.isRightFootEnabled = false;
+        params.controllerPoses[Rig::ControllerType_RightFoot] = AnimPose::identity;
+        params.controllerActiveFlags[Rig::ControllerType_RightFoot] = false;
     }
 
-    handAndFeetParams.bodyCapsuleRadius = myAvatar->getCharacterController()->getCapsuleRadius();
+    params.bodyCapsuleRadius = myAvatar->getCharacterController()->getCapsuleRadius();
-    handAndFeetParams.bodyCapsuleHalfHeight = myAvatar->getCharacterController()->getCapsuleHalfHeight();
+    params.bodyCapsuleHalfHeight = myAvatar->getCharacterController()->getCapsuleHalfHeight();
-    handAndFeetParams.bodyCapsuleLocalOffset = myAvatar->getCharacterController()->getCapsuleLocalOffset();
+    params.bodyCapsuleLocalOffset = myAvatar->getCharacterController()->getCapsuleLocalOffset();
 
-    _rig.updateFromHandAndFeetParameters(handAndFeetParams, deltaTime);
+    params.isTalking = head->getTimeWithoutTalking() <= 1.5f;
+
+    _rig.updateFromControllerParameters(params, deltaTime);
 
     Rig::CharacterControllerState ccState = convertCharacterControllerState(myAvatar->getCharacterController()->getState());
 

libraries/animation/src/AnimInverseKinematics.cpp

@@ -23,13 +23,40 @@
 #include "CubicHermiteSpline.h"
 #include "AnimUtil.h"
 
+static void lookupJointChainInfo(AnimInverseKinematics::JointChainInfo* jointChainInfos, size_t numJointChainInfos,
+                                 int indexA, int indexB,
+                                 AnimInverseKinematics::JointChainInfo** jointChainInfoA,
+                                 AnimInverseKinematics::JointChainInfo** jointChainInfoB) {
+    *jointChainInfoA = nullptr;
+    *jointChainInfoB = nullptr;
+    for (size_t i = 0; i < numJointChainInfos; i++) {
+        if (jointChainInfos[i].jointIndex == indexA) {
+            *jointChainInfoA = jointChainInfos + i;
+        }
+        if (jointChainInfos[i].jointIndex == indexB) {
+            *jointChainInfoB = jointChainInfos + i;
+        }
+        if (*jointChainInfoA && *jointChainInfoB) {
+            break;
+        }
+    }
+}
+
+static float easeOutExpo(float t) {
+    return 1.0f - powf(2, -10.0f * t);
+}
+
 AnimInverseKinematics::IKTargetVar::IKTargetVar(const QString& jointNameIn, const QString& positionVarIn, const QString& rotationVarIn,
-                                                const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector<float>& flexCoefficientsIn) :
+                                                const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector<float>& flexCoefficientsIn,
+                                                const QString& poleVectorEnabledVarIn, const QString& poleReferenceVectorVarIn, const QString& poleVectorVarIn) :
     jointName(jointNameIn),
     positionVar(positionVarIn),
     rotationVar(rotationVarIn),
     typeVar(typeVarIn),
     weightVar(weightVarIn),
+    poleVectorEnabledVar(poleVectorEnabledVarIn),
+    poleReferenceVectorVar(poleReferenceVectorVarIn),
+    poleVectorVar(poleVectorVarIn),
     weight(weightIn),
     numFlexCoefficients(flexCoefficientsIn.size()),
     jointIndex(-1)
@@ -46,6 +73,9 @@ AnimInverseKinematics::IKTargetVar::IKTargetVar(const IKTargetVar& orig) :
     rotationVar(orig.rotationVar),
     typeVar(orig.typeVar),
     weightVar(orig.weightVar),
+    poleVectorEnabledVar(orig.poleVectorEnabledVar),
+    poleReferenceVectorVar(orig.poleReferenceVectorVar),
+    poleVectorVar(orig.poleVectorVar),
     weight(orig.weight),
     numFlexCoefficients(orig.numFlexCoefficients),
     jointIndex(orig.jointIndex)
@@ -99,8 +129,9 @@ void AnimInverseKinematics::computeAbsolutePoses(AnimPoseVec& absolutePoses) con
 }
 
 void AnimInverseKinematics::setTargetVars(const QString& jointName, const QString& positionVar, const QString& rotationVar,
-                                          const QString& typeVar, const QString& weightVar, float weight, const std::vector<float>& flexCoefficients) {
+                                          const QString& typeVar, const QString& weightVar, float weight, const std::vector<float>& flexCoefficients,
-    IKTargetVar targetVar(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients);
+                                          const QString& poleVectorEnabledVar, const QString& poleReferenceVectorVar, const QString& poleVectorVar) {
+    IKTargetVar targetVar(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients, poleVectorEnabledVar, poleReferenceVectorVar, poleVectorVar);
 
     // if there are dups, last one wins.
     bool found = false;
@@ -138,9 +169,9 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::
             IKTarget target;
             target.setType(animVars.lookup(targetVar.typeVar, (int)IKTarget::Type::RotationAndPosition));
             if (target.getType() != IKTarget::Type::Unknown) {
-                AnimPose defaultPose = _skeleton->getAbsolutePose(targetVar.jointIndex, underPoses);
+                AnimPose absPose = _skeleton->getAbsolutePose(targetVar.jointIndex, underPoses);
-                glm::quat rotation = animVars.lookupRigToGeometry(targetVar.rotationVar, defaultPose.rot());
+                glm::quat rotation = animVars.lookupRigToGeometry(targetVar.rotationVar, absPose.rot());
-                glm::vec3 translation = animVars.lookupRigToGeometry(targetVar.positionVar, defaultPose.trans());
+                glm::vec3 translation = animVars.lookupRigToGeometry(targetVar.positionVar, absPose.trans());
                 float weight = animVars.lookup(targetVar.weightVar, targetVar.weight);
 
                 target.setPose(rotation, translation);
@@ -148,6 +179,15 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::
                 target.setWeight(weight);
                 target.setFlexCoefficients(targetVar.numFlexCoefficients, targetVar.flexCoefficients);
 
+                bool poleVectorEnabled = animVars.lookup(targetVar.poleVectorEnabledVar, false);
+                target.setPoleVectorEnabled(poleVectorEnabled);
+
+                glm::vec3 poleVector = animVars.lookupRigToGeometryVector(targetVar.poleVectorVar, Vectors::UNIT_Z);
+                target.setPoleVector(glm::normalize(poleVector));
+
+                glm::vec3 poleReferenceVector = animVars.lookupRigToGeometryVector(targetVar.poleReferenceVectorVar, Vectors::UNIT_Z);
+                target.setPoleReferenceVector(glm::normalize(poleReferenceVector));
+
                 targets.push_back(target);
 
                 if (targetVar.jointIndex > _maxTargetIndex) {
@@ -298,7 +338,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
     // the tip's parent-relative as we proceed up the chain
     glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot();
 
-    std::map<int, DebugJoint> debugJointMap;
+    const size_t MAX_CHAIN_DEPTH = 30;
+    JointChainInfo jointChainInfos[MAX_CHAIN_DEPTH];
 
     // NOTE: if this code is removed, the head will remain rigid, causing the spine/hips to thrust forward backward
     // as the head is nodded.
@@ -326,15 +367,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
             }
         }
 
-        // store the relative rotation change in the accumulator
-        _rotationAccumulators[tipIndex].add(tipRelativeRotation, target.getWeight());
-
         glm::vec3 tipRelativeTranslation = _relativePoses[target.getIndex()].trans();
-        _translationAccumulators[tipIndex].add(tipRelativeTranslation);
+        jointChainInfos[chainDepth] = { tipRelativeRotation, tipRelativeTranslation, target.getWeight(), tipIndex, constrained };
-
-        if (debug) {
-            debugJointMap[tipIndex] = DebugJoint(tipRelativeRotation, tipRelativeTranslation, constrained);
-        }
     }
 
     // cache tip absolute position
@@ -344,6 +378,9 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
 
     // descend toward root, pivoting each joint to get tip closer to target position
     while (pivotIndex != _hipsIndex && pivotsParentIndex != -1) {
+
+        assert(chainDepth < MAX_CHAIN_DEPTH);
+
         // compute the two lines that should be aligned
         glm::vec3 jointPosition = absolutePoses[pivotIndex].trans();
         glm::vec3 leverArm = tipPosition - jointPosition;
@@ -357,6 +394,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
             const float MIN_AXIS_LENGTH = 1.0e-4f;
             RotationConstraint* constraint = getConstraint(pivotIndex);
 
+
             // only allow swing on lowerSpine if there is a hips IK target.
             if (_hipsTargetIndex < 0 && constraint && constraint->isLowerSpine() && tipIndex != _headIndex) {
                 // for these types of targets we only allow twist at the lower-spine
@@ -382,6 +420,7 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
                 float cosAngle = glm::clamp(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine)), -1.0f, 1.0f);
                 float angle = acosf(cosAngle);
                 const float MIN_ADJUSTMENT_ANGLE = 1.0e-4f;
+
                 if (angle > MIN_ADJUSTMENT_ANGLE) {
                     // reduce angle by a flexCoefficient
                     angle *= target.getFlexCoefficient(chainDepth);
@@ -440,15 +479,8 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
             }
         }
 
-        // store the relative rotation change in the accumulator
-        _rotationAccumulators[pivotIndex].add(newRot, target.getWeight());
-
         glm::vec3 newTrans = _relativePoses[pivotIndex].trans();
-        _translationAccumulators[pivotIndex].add(newTrans);
+        jointChainInfos[chainDepth] = { newRot, newTrans, target.getWeight(), pivotIndex, constrained };
-
-        if (debug) {
-            debugJointMap[pivotIndex] = DebugJoint(newRot, newTrans, constrained);
-        }
 
         // keep track of tip's new transform as we descend towards root
         tipPosition = jointPosition + deltaRotation * (tipPosition - jointPosition);
@@ -461,8 +493,127 @@ void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const
         chainDepth++;
     }
 
+    if (target.getPoleVectorEnabled()) {
+        int topJointIndex = target.getIndex();
+        int midJointIndex = _skeleton->getParentIndex(topJointIndex);
+        if (midJointIndex != -1) {
+            int baseJointIndex = _skeleton->getParentIndex(midJointIndex);
+            if (baseJointIndex != -1) {
+                int baseParentJointIndex = _skeleton->getParentIndex(baseJointIndex);
+                AnimPose topPose, midPose, basePose;
+                int topChainIndex = -1, baseChainIndex = -1;
+                AnimPose postAbsPoses[MAX_CHAIN_DEPTH];
+                AnimPose accum = absolutePoses[_hipsIndex];
+                AnimPose baseParentPose = absolutePoses[_hipsIndex];
+                for (int i = (int)chainDepth - 1; i >= 0; i--) {
+                    accum = accum * AnimPose(glm::vec3(1.0f), jointChainInfos[i].relRot, jointChainInfos[i].relTrans);
+                    postAbsPoses[i] = accum;
+                    if (jointChainInfos[i].jointIndex == topJointIndex) {
+                        topChainIndex = i;
+                        topPose = accum;
+                    }
+                    if (jointChainInfos[i].jointIndex == midJointIndex) {
+                        midPose = accum;
+                    }
+                    if (jointChainInfos[i].jointIndex == baseJointIndex) {
+                        baseChainIndex = i;
+                        basePose = accum;
+                    }
+                    if (jointChainInfos[i].jointIndex == baseParentJointIndex) {
+                        baseParentPose = accum;
+                    }
+                }
+
+                glm::quat poleRot = Quaternions::IDENTITY;
+                glm::vec3 d = basePose.trans() - topPose.trans();
+                float dLen = glm::length(d);
+                if (dLen > EPSILON) {
+                    glm::vec3 dUnit = d / dLen;
+                    glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector());
+                    glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit;
+                    float eProjLen = glm::length(eProj);
+
+                    const float MIN_EPROJ_LEN = 0.5f;
+                    if (eProjLen < MIN_EPROJ_LEN) {
+                        glm::vec3 midPoint = topPose.trans() + d * 0.5f;
+                        e = midPose.trans() - midPoint;
+                        eProj = e - glm::dot(e, dUnit) * dUnit;
+                        eProjLen = glm::length(eProj);
+                    }
+
+                    glm::vec3 p = target.getPoleVector();
+                    glm::vec3 pProj = p - glm::dot(p, dUnit) * dUnit;
+                    float pProjLen = glm::length(pProj);
+
+                    if (eProjLen > EPSILON && pProjLen > EPSILON) {
+                        // as pProjLen become orthognal to d, reduce the amount of rotation.
+                        float magnitude = easeOutExpo(pProjLen);
+                        float dot = glm::clamp(glm::dot(eProj / eProjLen, pProj / pProjLen), 0.0f, 1.0f);
+                        float theta = acosf(dot);
+                        glm::vec3 cross = glm::cross(eProj, pProj);
+                        const float MIN_ADJUSTMENT_ANGLE = 0.001745f;  // 0.1 degree
+                        if (theta > MIN_ADJUSTMENT_ANGLE) {
+                            glm::vec3 axis = dUnit;
+                            if (glm::dot(cross, dUnit) < 0) {
+                                axis = -dUnit;
+                            }
+                            poleRot = glm::angleAxis(magnitude * theta, axis);
+                        }
+                    }
+                }
+
+                if (debug) {
+                    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 YELLOW(1.0f, 1.0f, 0.0f, 1.0f);
+                    const vec4 WHITE(1.0f, 1.0f, 1.0f, 1.0f);
+
+                    AnimPose geomToWorldPose = AnimPose(context.getRigToWorldMatrix() * context.getGeometryToRigMatrix());
+
+                    glm::vec3 dUnit = d / dLen;
+                    glm::vec3 e = midPose.xformVector(target.getPoleReferenceVector());
+                    glm::vec3 eProj = e - glm::dot(e, dUnit) * dUnit;
+                    float eProjLen = glm::length(eProj);
+                    const float MIN_EPROJ_LEN = 0.5f;
+                    if (eProjLen < MIN_EPROJ_LEN) {
+                        glm::vec3 midPoint = topPose.trans() + d * 0.5f;
+                        e = midPose.trans() - midPoint;
+                        eProj = e - glm::dot(e, dUnit) * dUnit;
+                        eProjLen = glm::length(eProj);
+                    }
+
+                    glm::vec3 p = target.getPoleVector();
+                    const float PROJ_VECTOR_LEN = 10.0f;
+                    const float POLE_VECTOR_LEN = 100.0f;
+                    glm::vec3 midPoint = (basePose.trans() + topPose.trans()) * 0.5f;
+                    DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(basePose.trans()),
+                                                     geomToWorldPose.xformPoint(topPose.trans()),
+                                                     YELLOW);
+                    DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint),
+                                                     geomToWorldPose.xformPoint(midPoint + PROJ_VECTOR_LEN * glm::normalize(e)),
+                                                     RED);
+                    DebugDraw::getInstance().drawRay(geomToWorldPose.xformPoint(midPoint),
+                                                     geomToWorldPose.xformPoint(midPoint + POLE_VECTOR_LEN * glm::normalize(p)),
+                                                     BLUE);
+                }
+
+                glm::quat newBaseRelRot = glm::inverse(baseParentPose.rot()) * poleRot * basePose.rot();
+                jointChainInfos[baseChainIndex].relRot = newBaseRelRot;
+
+                glm::quat newTopRelRot = glm::inverse(midPose.rot()) * glm::inverse(poleRot) * topPose.rot();
+                jointChainInfos[topChainIndex].relRot = newTopRelRot;
+            }
+        }
+    }
+
+    for (size_t i = 0; i < chainDepth; i++) {
+        _rotationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relRot, jointChainInfos[i].weight);
+        _translationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relTrans, jointChainInfos[i].weight);
+    }
+
     if (debug) {
-        debugDrawIKChain(debugJointMap, context);
+        debugDrawIKChain(jointChainInfos, chainDepth, context);
     }
 }
 
@@ -548,7 +699,8 @@ const std::vector<AnimInverseKinematics::SplineJointInfo>* AnimInverseKinematics
 
 void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug) {
 
-    std::map<int, DebugJoint> debugJointMap;
+    const size_t MAX_CHAIN_DEPTH = 30;
+    JointChainInfo jointChainInfos[MAX_CHAIN_DEPTH];
 
     const int baseIndex = _hipsIndex;
 
@@ -608,7 +760,6 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co
             ::blend(1, &absolutePoses[splineJointInfo.jointIndex], &desiredAbsPose, target.getFlexCoefficient(i), &flexedAbsPose);
 
             AnimPose relPose = parentAbsPose.inverse() * flexedAbsPose;
-            _rotationAccumulators[splineJointInfo.jointIndex].add(relPose.rot(), target.getWeight());
 
             bool constrained = false;
             if (splineJointInfo.jointIndex != _hipsIndex) {
@@ -632,18 +783,19 @@ void AnimInverseKinematics::solveTargetWithSpline(const AnimContext& context, co
                 }
             }
 
-            _translationAccumulators[splineJointInfo.jointIndex].add(relPose.trans(), target.getWeight());
+            jointChainInfos[i] = { relPose.rot(), relPose.trans(), target.getWeight(), splineJointInfo.jointIndex, constrained };
-
-            if (debug) {
-                debugJointMap[splineJointInfo.jointIndex] = DebugJoint(relPose.rot(), relPose.trans(), constrained);
-            }
 
             parentAbsPose = flexedAbsPose;
         }
     }
 
+    for (size_t i = 0; i < splineJointInfoVec->size(); i++) {
+        _rotationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relRot, jointChainInfos[i].weight);
+        _translationAccumulators[jointChainInfos[i].jointIndex].add(jointChainInfos[i].relTrans, jointChainInfos[i].weight);
+    }
+
     if (debug) {
-        debugDrawIKChain(debugJointMap, context);
+        debugDrawIKChain(jointChainInfos, splineJointInfoVec->size(), context);
     }
 }
 
@@ -654,6 +806,7 @@ const AnimPoseVec& AnimInverseKinematics::evaluate(const AnimVariantMap& animVar
     return _relativePoses;
 }
 
+
 //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
@@ -767,6 +920,7 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars
 
             {
                 PROFILE_RANGE_EX(simulation_animation, "ik/ccd", 0xffff00ff, 0);
+                preconditionRelativePosesToAvoidLimbLock(context, targets);
                 solve(context, targets);
             }
 
@@ -921,7 +1075,7 @@ void AnimInverseKinematics::initConstraints() {
     //         y                |
     //         |                |
     //         |            O---O---O RightUpLeg
-    //      z  |            | Hips2 |
+    //      z  |            | Hips  |
     //       \ |            |       |
     //        \|            |       |
     //  x -----+            O       O RightLeg
@@ -966,7 +1120,9 @@ void AnimInverseKinematics::initConstraints() {
         if (0 == baseName.compare("Arm", Qt::CaseSensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot());
-            stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f);
+            //stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f);
+            const float TWIST_LIMIT = 5.0f * PI / 8.0f;
+            stConstraint->setTwistLimits(-TWIST_LIMIT, TWIST_LIMIT);
 
             /* KEEP THIS CODE for future experimentation
             // these directions are approximate swing limits in root-frame
@@ -992,7 +1148,7 @@ void AnimInverseKinematics::initConstraints() {
 
             // simple cone
             std::vector<float> minDots;
-            const float MAX_HAND_SWING = PI / 2.0f;
+            const float MAX_HAND_SWING = 5.0f * PI / 8.0f;
             minDots.push_back(cosf(MAX_HAND_SWING));
             stConstraint->setSwingLimits(minDots);
 
@@ -1000,7 +1156,7 @@ void AnimInverseKinematics::initConstraints() {
         } else if (0 == baseName.compare("UpLeg", Qt::CaseSensitive)) {
             SwingTwistConstraint* stConstraint = new SwingTwistConstraint();
             stConstraint->setReferenceRotation(_defaultRelativePoses[i].rot());
-            stConstraint->setTwistLimits(-PI / 4.0f, PI / 4.0f);
+            stConstraint->setTwistLimits(-PI / 2.0f, PI / 2.0f);
 
             std::vector<glm::vec3> swungDirections;
             float deltaTheta = PI / 4.0f;
@@ -1142,7 +1298,7 @@ void AnimInverseKinematics::initConstraints() {
             // we determine the max/min angles by rotating the swing limit lines from parent- to child-frame
             // then measure the angles to swing the yAxis into alignment
             glm::vec3 hingeAxis = - mirror * Vectors::UNIT_Z;
-            const float MIN_ELBOW_ANGLE = 0.05f;
+            const float MIN_ELBOW_ANGLE = 0.0f;
             const float MAX_ELBOW_ANGLE = 11.0f * PI / 12.0f;
             glm::quat invReferenceRotation = glm::inverse(referenceRotation);
             glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_ELBOW_ANGLE, hingeAxis) * Vectors::UNIT_Y;
@@ -1173,8 +1329,8 @@ void AnimInverseKinematics::initConstraints() {
 
             // we determine the max/min angles by rotating the swing limit lines from parent- to child-frame
             // then measure the angles to swing the yAxis into alignment
-            const float MIN_KNEE_ANGLE = 0.097f;  // ~5 deg
+            const float MIN_KNEE_ANGLE = 0.0f;
-            const float MAX_KNEE_ANGLE = 7.0f * PI / 8.0f;
+            const float MAX_KNEE_ANGLE = 7.0f * PI / 8.0f; // 157.5 deg
             glm::quat invReferenceRotation = glm::inverse(referenceRotation);
             glm::vec3 minSwingAxis = invReferenceRotation * glm::angleAxis(MIN_KNEE_ANGLE, hingeAxis) * Vectors::UNIT_Y;
             glm::vec3 maxSwingAxis = invReferenceRotation * glm::angleAxis(MAX_KNEE_ANGLE, hingeAxis) * Vectors::UNIT_Y;
@@ -1308,6 +1464,7 @@ void AnimInverseKinematics::debugDrawRelativePoses(const AnimContext& context) c
     // convert relative poses to absolute
     _skeleton->convertRelativePosesToAbsolute(poses);
 
+
     mat4 geomToWorldMatrix = context.getRigToWorldMatrix() * context.getGeometryToRigMatrix();
 
     const vec4 RED(1.0f, 0.0f, 0.0f, 1.0f);
@@ -1338,13 +1495,14 @@ void AnimInverseKinematics::debugDrawRelativePoses(const AnimContext& context) c
     }
 }
 
-void AnimInverseKinematics::debugDrawIKChain(std::map<int, DebugJoint>& debugJointMap, const AnimContext& context) const {
+void AnimInverseKinematics::debugDrawIKChain(JointChainInfo* jointChainInfos, size_t numJointChainInfos, const AnimContext& context) const {
     AnimPoseVec poses = _relativePoses;
 
     // copy debug joint rotations into the relative poses
-    for (auto& debugJoint : debugJointMap) {
+    for (size_t i = 0; i < numJointChainInfos; i++) {
-        poses[debugJoint.first].rot() = debugJoint.second.relRot;
+        const JointChainInfo& info = jointChainInfos[i];
-        poses[debugJoint.first].trans() = debugJoint.second.relTrans;
+        poses[info.jointIndex].rot() = info.relRot;
+        poses[info.jointIndex].trans() = info.relTrans;
     }
 
     // convert relative poses to absolute
@@ -1360,11 +1518,11 @@ void AnimInverseKinematics::debugDrawIKChain(std::map<int, DebugJoint>& debugJoi
 
     // draw each pose
     for (int i = 0; i < (int)poses.size(); i++) {
-
+        int parentIndex = _skeleton->getParentIndex(i);
-        // only draw joints that are actually in debugJointMap, or their parents
+        JointChainInfo* jointInfo = nullptr;
-        auto iter = debugJointMap.find(i);
+        JointChainInfo* parentJointInfo = nullptr;
-        auto parentIter = debugJointMap.find(_skeleton->getParentIndex(i));
+        lookupJointChainInfo(jointChainInfos, numJointChainInfos, i, parentIndex, &jointInfo, &parentJointInfo);
-        if (iter != debugJointMap.end() || parentIter != debugJointMap.end()) {
+        if (jointInfo && parentJointInfo) {
 
             // transform local axes into world space.
             auto pose = poses[i];
@@ -1377,13 +1535,12 @@ void AnimInverseKinematics::debugDrawIKChain(std::map<int, DebugJoint>& debugJoi
             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());
                 glm::vec4 color = GRAY;
 
                 // draw constrained joints with a RED link to their parent.
-                if (parentIter != debugJointMap.end() && parentIter->second.constrained) {
+                if (parentJointInfo->constrained) {
                     color = RED;
                 }
                 DebugDraw::getInstance().drawRay(pos, parentPos, color);
@@ -1486,7 +1643,7 @@ void AnimInverseKinematics::debugDrawConstraints(const AnimContext& context) con
                             glm::vec3 worldSwungAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * swungAxis);
                             glm::vec3 swingTip = pos + SWING_LENGTH * worldSwungAxis;
 
-                            float prevPhi = acos(swingTwistConstraint->getMinDots()[j]);
+                            float prevPhi = acosf(swingTwistConstraint->getMinDots()[j]);
                             float prevTheta = theta - D_THETA;
                             glm::vec3 prevSwungAxis = sphericalToCartesian(prevPhi, prevTheta - PI_2);
                             glm::vec3 prevWorldSwungAxis = transformVectorFast(geomToWorldMatrix, parentAbsRot * refRot * prevSwungAxis);
@@ -1521,6 +1678,50 @@ void AnimInverseKinematics::blendToPoses(const AnimPoseVec& targetPoses, const A
     }
 }
 
+void AnimInverseKinematics::preconditionRelativePosesToAvoidLimbLock(const AnimContext& context, const std::vector<IKTarget>& targets) {
+    const int NUM_LIMBS = 4;
+    std::pair<int, int> limbs[NUM_LIMBS] = {
+        {_skeleton->nameToJointIndex("LeftHand"), _skeleton->nameToJointIndex("LeftArm")},
+        {_skeleton->nameToJointIndex("RightHand"), _skeleton->nameToJointIndex("RightArm")},
+        {_skeleton->nameToJointIndex("LeftFoot"), _skeleton->nameToJointIndex("LeftUpLeg")},
+        {_skeleton->nameToJointIndex("RightFoot"), _skeleton->nameToJointIndex("RightUpLeg")}
+    };
+    const float MIN_AXIS_LENGTH = 1.0e-4f;
+
+    for (auto& target : targets) {
+        if (target.getIndex() != -1) {
+            for (int i = 0; i < NUM_LIMBS; i++) {
+                if (limbs[i].first == target.getIndex()) {
+                    int tipIndex = limbs[i].first;
+                    int baseIndex = limbs[i].second;
+
+                    // TODO: as an optimization, these poses can be computed in one pass down the chain, instead of three.
+                    AnimPose tipPose = _skeleton->getAbsolutePose(tipIndex, _relativePoses);
+                    AnimPose basePose = _skeleton->getAbsolutePose(baseIndex, _relativePoses);
+                    AnimPose baseParentPose = _skeleton->getAbsolutePose(_skeleton->getParentIndex(baseIndex), _relativePoses);
+
+                    // to help reduce limb locking, and to help the CCD solver converge faster
+                    // rotate the limbs leverArm over the targetLine.
+                    glm::vec3 targetLine = target.getTranslation() - basePose.trans();
+                    glm::vec3 leverArm = tipPose.trans() - basePose.trans();
+                    glm::vec3 axis = glm::cross(leverArm, targetLine);
+                    float axisLength = glm::length(axis);
+                    if (axisLength > MIN_AXIS_LENGTH) {
+                        // compute angle of rotation that brings tip to target
+                        axis /= axisLength;
+                        float cosAngle = glm::clamp(glm::dot(leverArm, targetLine) / (glm::length(leverArm) * glm::length(targetLine)), -1.0f, 1.0f);
+                        float angle = acosf(cosAngle);
+                        glm::quat newBaseRotation = glm::angleAxis(angle, axis) * basePose.rot();
+
+                        // convert base rotation into relative space of base.
+                        _relativePoses[baseIndex].rot() = glm::inverse(baseParentPose.rot()) * newBaseRotation;
+                    }
+                }
+            }
+        }
+    }
+}
+
 void AnimInverseKinematics::initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPoses) {
     const float RELAX_BLEND_FACTOR = (1.0f / 16.0f);
     const float COPY_BLEND_FACTOR = 1.0f;
@@ -1540,7 +1741,7 @@ void AnimInverseKinematics::initRelativePosesFromSolutionSource(SolutionSource s
         break;
     case SolutionSource::LimitCenterPoses:
         // essentially copy limitCenterPoses over to _relativePoses.
-        blendToPoses(_limitCenterPoses, underPoses, COPY_BLEND_FACTOR);
+        blendToPoses(underPoses, _limitCenterPoses, COPY_BLEND_FACTOR);
         break;
     }
 }

libraries/animation/src/AnimInverseKinematics.h

@@ -26,6 +26,14 @@ class RotationConstraint;
 class AnimInverseKinematics : public AnimNode {
 public:
 
+    struct JointChainInfo {
+        glm::quat relRot;
+        glm::vec3 relTrans;
+        float weight;
+        int jointIndex;
+        bool constrained;
+    };
+
     explicit AnimInverseKinematics(const QString& id);
     virtual ~AnimInverseKinematics() override;
 
@@ -34,7 +42,8 @@ public:
     void computeAbsolutePoses(AnimPoseVec& absolutePoses) const;
 
     void setTargetVars(const QString& jointName, const QString& positionVar, const QString& rotationVar,
-                       const QString& typeVar, const QString& weightVar, float weight, const std::vector<float>& flexCoefficients);
+                       const QString& typeVar, const QString& weightVar, float weight, const std::vector<float>& flexCoefficients,
+                       const QString& poleVectorEnabledVar, const QString& poleReferenceVectorVar, const QString& poleVectorVar);
 
     virtual const AnimPoseVec& evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimNode::Triggers& triggersOut) override;
     virtual const AnimPoseVec& overlay(const AnimVariantMap& animVars, const AnimContext& context, float dt, Triggers& triggersOut, const AnimPoseVec& underPoses) override;
@@ -67,19 +76,13 @@ protected:
     void solveTargetWithCCD(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug);
     void solveTargetWithSpline(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses, bool debug);
     virtual void setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) override;
-    struct DebugJoint {
+    void debugDrawIKChain(JointChainInfo* jointChainInfos, size_t numJointChainInfos, const AnimContext& context) const;
-        DebugJoint() : relRot(), constrained(false) {}
-        DebugJoint(const glm::quat& relRotIn, const glm::vec3& relTransIn, bool constrainedIn) : relRot(relRotIn), relTrans(relTransIn), constrained(constrainedIn) {}
-        glm::quat relRot;
-        glm::vec3 relTrans;
-        bool constrained;
-    };
-    void debugDrawIKChain(std::map<int, DebugJoint>& debugJointMap, const AnimContext& context) const;
     void debugDrawRelativePoses(const AnimContext& context) const;
     void debugDrawConstraints(const AnimContext& context) const;
     void debugDrawSpineSplines(const AnimContext& context, const std::vector<IKTarget>& targets) const;
     void initRelativePosesFromSolutionSource(SolutionSource solutionSource, const AnimPoseVec& underPose);
     void blendToPoses(const AnimPoseVec& targetPoses, const AnimPoseVec& underPose, float blendFactor);
+    void preconditionRelativePosesToAvoidLimbLock(const AnimContext& context, const std::vector<IKTarget>& targets);
 
     // used to pre-compute information about each joint influeced by a spline IK target.
     struct SplineJointInfo {
@@ -107,7 +110,8 @@ protected:
     enum FlexCoefficients { MAX_FLEX_COEFFICIENTS = 10 };
     struct IKTargetVar {
         IKTargetVar(const QString& jointNameIn, const QString& positionVarIn, const QString& rotationVarIn,
-                    const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector<float>& flexCoefficientsIn);
+                    const QString& typeVarIn, const QString& weightVarIn, float weightIn, const std::vector<float>& flexCoefficientsIn,
+                    const QString& poleVectorEnabledVar, const QString& poleReferenceVectorVar, const QString& poleVectorVar);
         IKTargetVar(const IKTargetVar& orig);
 
         QString jointName;
@@ -115,6 +119,9 @@ protected:
         QString rotationVar;
         QString typeVar;
         QString weightVar;
+        QString poleVectorEnabledVar;
+        QString poleReferenceVectorVar;
+        QString poleVectorVar;
         float weight;
         float flexCoefficients[MAX_FLEX_COEFFICIENTS];
         size_t numFlexCoefficients;

libraries/animation/src/AnimNodeLoader.cpp

@@ -479,6 +479,9 @@ AnimNode::Pointer loadInverseKinematicsNode(const QJsonObject& jsonObj, const QS
         READ_OPTIONAL_STRING(typeVar, targetObj);
         READ_OPTIONAL_STRING(weightVar, targetObj);
         READ_OPTIONAL_FLOAT(weight, targetObj, 1.0f);
+        READ_OPTIONAL_STRING(poleVectorEnabledVar, targetObj);
+        READ_OPTIONAL_STRING(poleReferenceVectorVar, targetObj);
+        READ_OPTIONAL_STRING(poleVectorVar, targetObj);
 
         auto flexCoefficientsValue = targetObj.value("flexCoefficients");
         if (!flexCoefficientsValue.isArray()) {
@@ -491,7 +494,7 @@ AnimNode::Pointer loadInverseKinematicsNode(const QJsonObject& jsonObj, const QS
             flexCoefficients.push_back((float)value.toDouble());
         }
 
-        node->setTargetVars(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients);
+        node->setTargetVars(jointName, positionVar, rotationVar, typeVar, weightVar, weight, flexCoefficients, poleVectorEnabledVar, poleReferenceVectorVar, poleVectorVar);
     };
 
     READ_OPTIONAL_STRING(solutionSource, jsonObj);

libraries/animation/src/AnimPose.h

@@ -21,6 +21,8 @@ class AnimPose {
 public:
     AnimPose() {}
     explicit AnimPose(const glm::mat4& mat);
+    explicit AnimPose(const glm::quat& rotIn) : _scale(1.0f), _rot(rotIn), _trans(0.0f) {}
+    AnimPose(const glm::quat& rotIn, const glm::vec3& transIn) : _scale(1.0f), _rot(rotIn), _trans(transIn) {}
     AnimPose(const glm::vec3& scaleIn, const glm::quat& rotIn, const glm::vec3& transIn) : _scale(scaleIn), _rot(rotIn), _trans(transIn) {}
     static const AnimPose identity;
 

libraries/animation/src/AnimSkeleton.cpp

@@ -76,11 +76,11 @@ const QString& AnimSkeleton::getJointName(int jointIndex) const {
     return _joints[jointIndex].name;
 }
 
-AnimPose AnimSkeleton::getAbsolutePose(int jointIndex, const AnimPoseVec& poses) const {
+AnimPose AnimSkeleton::getAbsolutePose(int jointIndex, const AnimPoseVec& relativePoses) const {
-    if (jointIndex < 0 || jointIndex >= (int)poses.size() || jointIndex >= _jointsSize) {
+    if (jointIndex < 0 || jointIndex >= (int)relativePoses.size() || jointIndex >= _jointsSize) {
         return AnimPose::identity;
     } else {
-        return getAbsolutePose(_joints[jointIndex].parentIndex, poses) * poses[jointIndex];
+        return getAbsolutePose(_joints[jointIndex].parentIndex, relativePoses) * relativePoses[jointIndex];
     }
 }
 

libraries/animation/src/AnimSkeleton.h

@@ -50,7 +50,7 @@ public:
 
     int getParentIndex(int jointIndex) const;
 
-    AnimPose getAbsolutePose(int jointIndex, const AnimPoseVec& poses) const;
+    AnimPose getAbsolutePose(int jointIndex, const AnimPoseVec& relativePoses) const;
 
     void convertRelativePosesToAbsolute(AnimPoseVec& poses) const;
     void convertAbsolutePosesToRelative(AnimPoseVec& poses) const;

libraries/animation/src/IKTarget.h

@@ -30,10 +30,16 @@ public:
     const glm::vec3& getTranslation() const { return _pose.trans(); }
     const glm::quat& getRotation() const { return _pose.rot(); }
     const AnimPose& getPose() const { return _pose; }
+    glm::vec3 getPoleVector() const { return _poleVector; }
+    glm::vec3 getPoleReferenceVector() const { return _poleReferenceVector; }
+    bool getPoleVectorEnabled() const { return _poleVectorEnabled; }
     int getIndex() const { return _index; }
     Type getType() const { return _type; }
 
     void setPose(const glm::quat& rotation, const glm::vec3& translation);
+    void setPoleVector(const glm::vec3& poleVector) { _poleVector = poleVector; }
+    void setPoleReferenceVector(const glm::vec3& poleReferenceVector) { _poleReferenceVector = poleReferenceVector; }
+    void setPoleVectorEnabled(bool poleVectorEnabled) { _poleVectorEnabled = poleVectorEnabled; }
     void setIndex(int index) { _index = index; }
     void setType(int);
     void setFlexCoefficients(size_t numFlexCoefficientsIn, const float* flexCoefficientsIn);
@@ -46,8 +52,11 @@ public:
 
 private:
     AnimPose _pose;
-    int _index{-1};
+    glm::vec3 _poleVector;
-    Type _type{Type::RotationAndPosition};
+    glm::vec3 _poleReferenceVector;
+    bool _poleVectorEnabled { false };
+    int _index { -1 };
+    Type _type { Type::RotationAndPosition };
     float _weight;
     float _flexCoefficients[MAX_FLEX_COEFFICIENTS];
     size_t _numFlexCoefficients;

libraries/animation/src/Rig.cpp

@@ -479,12 +479,6 @@ bool Rig::getAbsoluteJointPoseInRigFrame(int jointIndex, AnimPose& returnPose) c
     }
 }
 
-bool Rig::getJointCombinedRotation(int jointIndex, glm::quat& result, const glm::quat& rotation) const {
-    // AJT: TODO: used by attachments
-    ASSERT(false);
-    return false;
-}
-
 void Rig::calcAnimAlpha(float speed, const std::vector<float>& referenceSpeeds, float* alphaOut) const {
 
     ASSERT(referenceSpeeds.size() > 0);
@@ -950,6 +944,7 @@ void Rig::updateAnimations(float deltaTime, const glm::mat4& rootTransform, cons
 
         // evaluate the animation
         AnimNode::Triggers triggersOut;
+
         _internalPoseSet._relativePoses = _animNode->evaluate(_animVars, context, deltaTime, triggersOut);
         if ((int)_internalPoseSet._relativePoses.size() != _animSkeleton->getNumJoints()) {
             // animations haven't fully loaded yet.
@@ -1015,46 +1010,6 @@ glm::quat Rig::getJointDefaultRotationInParentFrame(int jointIndex) {
     return glm::quat();
 }
 
-void Rig::updateFromHeadParameters(const HeadParameters& params, float dt) {
-    updateHeadAnimVars(params);
-
-    _animVars.set("isTalking", params.isTalking);
-    _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);
-    }
-
-    if (params.hipsEnabled && params.spine2Enabled) {
-        _animVars.set("spine2Type", (int)IKTarget::Type::Spline);
-        _animVars.set("spine2Position", extractTranslation(params.spine2Matrix));
-        _animVars.set("spine2Rotation", glmExtractRotation(params.spine2Matrix));
-    } else {
-        _animVars.set("spine2Type", (int)IKTarget::Type::Unknown);
-    }
-
-    if (params.leftArmEnabled) {
-        _animVars.set("leftArmType", (int)IKTarget::Type::RotationAndPosition);
-        _animVars.set("leftArmPosition", params.leftArmPosition);
-        _animVars.set("leftArmRotation", params.leftArmRotation);
-    } else {
-        _animVars.set("leftArmType", (int)IKTarget::Type::Unknown);
-    }
-
-    if (params.rightArmEnabled) {
-        _animVars.set("rightArmType", (int)IKTarget::Type::RotationAndPosition);
-        _animVars.set("rightArmPosition", params.rightArmPosition);
-        _animVars.set("rightArmRotation", params.rightArmRotation);
-    } else {
-        _animVars.set("rightArmType", (int)IKTarget::Type::Unknown);
-    }
-}
 
 void Rig::updateFromEyeParameters(const EyeParameters& params) {
     updateEyeJoint(params.leftEyeJointIndex, params.modelTranslation, params.modelRotation, params.eyeLookAt, params.eyeSaccade);
@@ -1086,12 +1041,12 @@ void Rig::computeHeadFromHMD(const AnimPose& hmdPose, glm::vec3& headPositionOut
     headOrientationOut = hmdOrientation;
 }
 
-void Rig::updateHeadAnimVars(const HeadParameters& params) {
+void Rig::updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headPose) {
     if (_animSkeleton) {
-        if (params.headEnabled) {
+        if (headEnabled) {
-            _animVars.set("headPosition", params.rigHeadPosition);
+            _animVars.set("headPosition", headPose.trans());
-            _animVars.set("headRotation", params.rigHeadOrientation);
+            _animVars.set("headRotation", headPose.rot());
-            if (params.hipsEnabled) {
+            if (hipsEnabled) {
                 // Since there is an explicit hips ik target, switch the head to use the more flexible Spline IK chain type.
                 // this will allow the spine to compress/expand and bend more natrually, ensuring that it can reach the head target position.
                 _animVars.set("headType", (int)IKTarget::Type::Spline);
@@ -1104,12 +1059,271 @@ void Rig::updateHeadAnimVars(const HeadParameters& params) {
             }
         } else {
             _animVars.unset("headPosition");
-            _animVars.set("headRotation", params.rigHeadOrientation);
+            _animVars.set("headRotation", headPose.rot());
             _animVars.set("headType", (int)IKTarget::Type::RotationOnly);
         }
     }
 }
 
+void Rig::updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool leftArmEnabled, bool rightArmEnabled, float dt,
+                      const AnimPose& leftHandPose, const AnimPose& rightHandPose,
+                      float bodyCapsuleRadius, float bodyCapsuleHalfHeight, const glm::vec3& bodyCapsuleLocalOffset) {
+
+    // Use this capsule to represent the avatar body.
+    int hipsIndex = indexOfJoint("Hips");
+    glm::vec3 hipsTrans;
+    if (hipsIndex >= 0) {
+        hipsTrans = _internalPoseSet._absolutePoses[hipsIndex].trans();
+    }
+
+    const glm::vec3 bodyCapsuleCenter = hipsTrans - bodyCapsuleLocalOffset;
+    const glm::vec3 bodyCapsuleStart = bodyCapsuleCenter - glm::vec3(0, bodyCapsuleHalfHeight, 0);
+    const glm::vec3 bodyCapsuleEnd = bodyCapsuleCenter + glm::vec3(0, bodyCapsuleHalfHeight, 0);
+
+    const float HAND_RADIUS = 0.05f;
+
+    const float RELAX_DURATION = 0.6f;
+    const float CONTROL_DURATION = 0.4f;
+    const bool TO_CONTROLLED = true;
+    const bool FROM_CONTROLLED = false;
+    const bool LEFT_HAND = true;
+    const bool RIGHT_HAND = false;
+
+    const float ELBOW_POLE_VECTOR_BLEND_FACTOR = 0.95f;
+
+    if (leftHandEnabled) {
+        if (!_isLeftHandControlled) {
+            _leftHandControlTimeRemaining = CONTROL_DURATION;
+            _isLeftHandControlled = true;
+        }
+
+        glm::vec3 handPosition = leftHandPose.trans();
+        glm::quat handRotation = leftHandPose.rot();
+
+        if (_leftHandControlTimeRemaining > 0.0f) {
+            // Move hand from non-controlled position to controlled position.
+            _leftHandControlTimeRemaining = std::max(_leftHandControlTimeRemaining - dt, 0.0f);
+            AnimPose handPose(Vectors::ONE, handRotation, handPosition);
+            if (transitionHandPose(_leftHandControlTimeRemaining, CONTROL_DURATION, handPose,
+                                   LEFT_HAND, TO_CONTROLLED, handPose)) {
+                handPosition = handPose.trans();
+                handRotation = handPose.rot();
+            }
+        }
+
+        if (!hipsEnabled) {
+            // prevent the hand IK targets from intersecting the body capsule
+            glm::vec3 displacement;
+            if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) {
+                handPosition -= displacement;
+            }
+        }
+
+        _animVars.set("leftHandPosition", handPosition);
+        _animVars.set("leftHandRotation", handRotation);
+        _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition);
+
+        _lastLeftHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition);
+        _isLeftHandControlled = true;
+
+        // compute pole vector
+        int handJointIndex = _animSkeleton->nameToJointIndex("LeftHand");
+        int armJointIndex = _animSkeleton->nameToJointIndex("LeftArm");
+        int elbowJointIndex = _animSkeleton->nameToJointIndex("LeftForeArm");
+        if (!leftArmEnabled && elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) {
+            glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, true);
+
+            // smooth toward desired pole vector from previous pole vector...  to reduce jitter
+            if (!_prevLeftHandPoleVectorValid) {
+                _prevLeftHandPoleVectorValid = true;
+                _prevLeftHandPoleVector = poleVector;
+            }
+            glm::quat deltaRot = rotationBetween(_prevLeftHandPoleVector, poleVector);
+            glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR);
+            _prevLeftHandPoleVector = smoothDeltaRot * _prevLeftHandPoleVector;
+
+            _animVars.set("leftHandPoleVectorEnabled", true);
+            _animVars.set("leftHandPoleReferenceVector", Vectors::UNIT_X);
+            _animVars.set("leftHandPoleVector", _prevLeftHandPoleVector);
+        } else {
+            _prevLeftHandPoleVectorValid = false;
+            _animVars.set("leftHandPoleVectorEnabled", false);
+        }
+    } else {
+        _prevLeftHandPoleVectorValid = false;
+        _animVars.set("leftHandPoleVectorEnabled", false);
+
+        if (_isLeftHandControlled) {
+            _leftHandRelaxTimeRemaining = RELAX_DURATION;
+            _isLeftHandControlled = false;
+        }
+
+        if (_leftHandRelaxTimeRemaining > 0.0f) {
+            // Move hand from controlled position to non-controlled position.
+            _leftHandRelaxTimeRemaining = std::max(_leftHandRelaxTimeRemaining - dt, 0.0f);
+            AnimPose handPose;
+            if (transitionHandPose(_leftHandRelaxTimeRemaining, RELAX_DURATION, _lastLeftHandControlledPose,
+                                   LEFT_HAND, FROM_CONTROLLED, handPose)) {
+                _animVars.set("leftHandPosition", handPose.trans());
+                _animVars.set("leftHandRotation", handPose.rot());
+                _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition);
+            }
+        } else {
+            _animVars.unset("leftHandPosition");
+            _animVars.unset("leftHandRotation");
+            _animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
+        }
+    }
+
+    if (rightHandEnabled) {
+        if (!_isRightHandControlled) {
+            _rightHandControlTimeRemaining = CONTROL_DURATION;
+            _isRightHandControlled = true;
+        }
+
+        glm::vec3 handPosition = rightHandPose.trans();
+        glm::quat handRotation = rightHandPose.rot();
+
+        if (_rightHandControlTimeRemaining > 0.0f) {
+            // Move hand from non-controlled position to controlled position.
+            _rightHandControlTimeRemaining = std::max(_rightHandControlTimeRemaining - dt, 0.0f);
+            AnimPose handPose(Vectors::ONE, handRotation, handPosition);
+            if (transitionHandPose(_rightHandControlTimeRemaining, CONTROL_DURATION, handPose, RIGHT_HAND, TO_CONTROLLED, handPose)) {
+                handPosition = handPose.trans();
+                handRotation = handPose.rot();
+            }
+        }
+
+        if (!hipsEnabled) {
+            // prevent the hand IK targets from intersecting the body capsule
+            glm::vec3 displacement;
+            if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) {
+                handPosition -= displacement;
+            }
+        }
+
+        _animVars.set("rightHandPosition", handPosition);
+        _animVars.set("rightHandRotation", handRotation);
+        _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition);
+
+        _lastRightHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition);
+        _isRightHandControlled = true;
+
+        // compute pole vector
+        int handJointIndex = _animSkeleton->nameToJointIndex("RightHand");
+        int armJointIndex = _animSkeleton->nameToJointIndex("RightArm");
+        int elbowJointIndex = _animSkeleton->nameToJointIndex("RightForeArm");
+        if (!rightArmEnabled && elbowJointIndex >= 0 && armJointIndex >= 0 && elbowJointIndex >= 0) {
+            glm::vec3 poleVector = calculateElbowPoleVector(handJointIndex, elbowJointIndex, armJointIndex, hipsIndex, false);
+
+            // smooth toward desired pole vector from previous pole vector...  to reduce jitter
+            if (!_prevRightHandPoleVectorValid) {
+                _prevRightHandPoleVectorValid = true;
+                _prevRightHandPoleVector = poleVector;
+            }
+            glm::quat deltaRot = rotationBetween(_prevRightHandPoleVector, poleVector);
+            glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, ELBOW_POLE_VECTOR_BLEND_FACTOR);
+            _prevRightHandPoleVector = smoothDeltaRot * _prevRightHandPoleVector;
+
+            _animVars.set("rightHandPoleVectorEnabled", true);
+            _animVars.set("rightHandPoleReferenceVector", -Vectors::UNIT_X);
+            _animVars.set("rightHandPoleVector", _prevRightHandPoleVector);
+        } else {
+            _prevRightHandPoleVectorValid = false;
+            _animVars.set("rightHandPoleVectorEnabled", false);
+        }
+    } else {
+        _prevRightHandPoleVectorValid = false;
+        _animVars.set("rightHandPoleVectorEnabled", false);
+
+        if (_isRightHandControlled) {
+            _rightHandRelaxTimeRemaining = RELAX_DURATION;
+            _isRightHandControlled = false;
+        }
+
+        if (_rightHandRelaxTimeRemaining > 0.0f) {
+            // Move hand from controlled position to non-controlled position.
+            _rightHandRelaxTimeRemaining = std::max(_rightHandRelaxTimeRemaining - dt, 0.0f);
+            AnimPose handPose;
+            if (transitionHandPose(_rightHandRelaxTimeRemaining, RELAX_DURATION, _lastRightHandControlledPose, RIGHT_HAND, FROM_CONTROLLED, handPose)) {
+                _animVars.set("rightHandPosition", handPose.trans());
+                _animVars.set("rightHandRotation", handPose.rot());
+                _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition);
+            }
+        } else {
+            _animVars.unset("rightHandPosition");
+            _animVars.unset("rightHandRotation");
+            _animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
+        }
+    }
+}
+
+void Rig::updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose& leftFootPose, const AnimPose& rightFootPose) {
+
+    const float KNEE_POLE_VECTOR_BLEND_FACTOR = 0.95f;
+
+    int hipsIndex = indexOfJoint("Hips");
+
+    if (leftFootEnabled) {
+        _animVars.set("leftFootPosition", leftFootPose.trans());
+        _animVars.set("leftFootRotation", leftFootPose.rot());
+        _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition);
+
+        int footJointIndex = _animSkeleton->nameToJointIndex("LeftFoot");
+        int kneeJointIndex = _animSkeleton->nameToJointIndex("LeftLeg");
+        int upLegJointIndex = _animSkeleton->nameToJointIndex("LeftUpLeg");
+        glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, kneeJointIndex, upLegJointIndex, hipsIndex, leftFootPose);
+
+        // smooth toward desired pole vector from previous pole vector...  to reduce jitter
+        if (!_prevLeftFootPoleVectorValid) {
+            _prevLeftFootPoleVectorValid = true;
+            _prevLeftFootPoleVector = poleVector;
+        }
+        glm::quat deltaRot = rotationBetween(_prevLeftFootPoleVector, poleVector);
+        glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, KNEE_POLE_VECTOR_BLEND_FACTOR);
+        _prevLeftFootPoleVector = smoothDeltaRot * _prevLeftFootPoleVector;
+
+        _animVars.set("leftFootPoleVectorEnabled", true);
+        _animVars.set("leftFootPoleReferenceVector", Vectors::UNIT_Z);
+        _animVars.set("leftFootPoleVector", _prevLeftFootPoleVector);
+    } else {
+        _animVars.unset("leftFootPosition");
+        _animVars.unset("leftFootRotation");
+        _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition);
+        _animVars.set("leftFootPoleVectorEnabled", false);
+        _prevLeftFootPoleVectorValid = false;
+    }
+
+    if (rightFootEnabled) {
+        _animVars.set("rightFootPosition", rightFootPose.trans());
+        _animVars.set("rightFootRotation", rightFootPose.rot());
+        _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition);
+
+        int footJointIndex = _animSkeleton->nameToJointIndex("RightFoot");
+        int kneeJointIndex = _animSkeleton->nameToJointIndex("RightLeg");
+        int upLegJointIndex = _animSkeleton->nameToJointIndex("RightUpLeg");
+        glm::vec3 poleVector = calculateKneePoleVector(footJointIndex, kneeJointIndex, upLegJointIndex, hipsIndex, rightFootPose);
+
+        // smooth toward desired pole vector from previous pole vector...  to reduce jitter
+        if (!_prevRightFootPoleVectorValid) {
+            _prevRightFootPoleVectorValid = true;
+            _prevRightFootPoleVector = poleVector;
+        }
+        glm::quat deltaRot = rotationBetween(_prevRightFootPoleVector, poleVector);
+        glm::quat smoothDeltaRot = safeMix(deltaRot, Quaternions::IDENTITY, KNEE_POLE_VECTOR_BLEND_FACTOR);
+        _prevRightFootPoleVector = smoothDeltaRot * _prevRightFootPoleVector;
+
+        _animVars.set("rightFootPoleVectorEnabled", true);
+        _animVars.set("rightFootPoleReferenceVector", Vectors::UNIT_Z);
+        _animVars.set("rightFootPoleVector", _prevRightFootPoleVector);
+    } else {
+        _animVars.unset("rightFootPosition");
+        _animVars.unset("rightFootRotation");
+        _animVars.set("rightFootPoleVectorEnabled", false);
+        _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition);
+    }
+}
+
 void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAtSpot, const glm::vec3& saccade) {
 
     // TODO: does not properly handle avatar scale.
@@ -1145,162 +1359,138 @@ void Rig::updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm
     }
 }
 
-void Rig::updateFromHandAndFeetParameters(const HandAndFeetParameters& params, float dt) {
+static glm::quat quatLerp(const glm::quat& q1, const glm::quat& q2, float alpha) {
-    if (_animSkeleton && _animNode) {
+    float dot = glm::dot(q1, q2);
-        const float HAND_RADIUS = 0.05f;
+    glm::quat temp;
-        int hipsIndex = indexOfJoint("Hips");
+    if (dot < 0.0f) {
-        glm::vec3 hipsTrans;
+        temp = -q2;
-        if (hipsIndex >= 0) {
+    } else {
-            hipsTrans = _internalPoseSet._absolutePoses[hipsIndex].trans();
+        temp = q2;
-        }
+    }
+    return glm::normalize(glm::lerp(q1, temp, alpha));
+}
 
-        // Use this capsule to represent the avatar body.
+glm::vec3 Rig::calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) const {
-        const float bodyCapsuleRadius = params.bodyCapsuleRadius;
+    AnimPose hipsPose = _externalPoseSet._absolutePoses[hipsIndex];
-        const glm::vec3 bodyCapsuleCenter = hipsTrans - params.bodyCapsuleLocalOffset;
+    AnimPose handPose = _externalPoseSet._absolutePoses[handIndex];
-        const glm::vec3 bodyCapsuleStart = bodyCapsuleCenter - glm::vec3(0, params.bodyCapsuleHalfHeight, 0);
+    AnimPose elbowPose = _externalPoseSet._absolutePoses[elbowIndex];
-        const glm::vec3 bodyCapsuleEnd = bodyCapsuleCenter + glm::vec3(0, params.bodyCapsuleHalfHeight, 0);
+    AnimPose armPose = _externalPoseSet._absolutePoses[armIndex];
 
-        // TODO: add isHipsEnabled
+    // ray from hand to arm.
-        bool bodySensorTrackingEnabled = params.isLeftFootEnabled || params.isRightFootEnabled;
+    glm::vec3 d = glm::normalize(handPose.trans() - armPose.trans());
 
-        const float RELAX_DURATION = 0.6f;
+    float sign = isLeft ? 1.0f : -1.0f;
-        const float CONTROL_DURATION = 0.4f;
-        const bool TO_CONTROLLED = true;
-        const bool FROM_CONTROLLED = false;
-        const bool LEFT_HAND = true;
-        const bool RIGHT_HAND = false;
 
-        if (params.isLeftEnabled) {
+    // form a plane normal to the hips x-axis.
-            if (!_isLeftHandControlled) {
+    glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X;
-                _leftHandControlTimeRemaining = CONTROL_DURATION;
+    glm::vec3 y = hipsPose.rot() * Vectors::UNIT_Y;
-                _isLeftHandControlled = true;
-            }
 
-            glm::vec3 handPosition = params.leftPosition;
+    // project d onto this plane
-            glm::quat handRotation = params.leftOrientation;
+    glm::vec3 dProj = d - glm::dot(d, n) * n;
 
-            if (_leftHandControlTimeRemaining > 0.0f) {
+    // give dProj a bit of offset away from the body.
-                // Move hand from non-controlled position to controlled position.
+    float avatarScale = extractUniformScale(_modelOffset);
-                _leftHandControlTimeRemaining = std::max(_leftHandControlTimeRemaining - dt, 0.0f);
+    const float LATERAL_OFFSET = 1.0f * avatarScale;
-                AnimPose handPose(Vectors::ONE, handRotation, handPosition);
+    const float VERTICAL_OFFSET = -0.333f * avatarScale;
-                if (transitionHandPose(_leftHandControlTimeRemaining, CONTROL_DURATION, handPose, LEFT_HAND, TO_CONTROLLED, 
+    glm::vec3 dProjWithOffset = dProj + sign * LATERAL_OFFSET * n + y * VERTICAL_OFFSET;
-                        handPose)) {
-                    handPosition = handPose.trans();
-                    handRotation = handPose.rot();
-                }
-            }
 
-            if (!bodySensorTrackingEnabled) {
+    // rotate dProj by 90 degrees to get the poleVector.
-                // prevent the hand IK targets from intersecting the body capsule
+    glm::vec3 poleVector = glm::angleAxis(PI / 2.0f, n) * dProjWithOffset;
-                glm::vec3 displacement;
-                if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) {
-                    handPosition -= displacement;
-                }
-            }
 
-            _animVars.set("leftHandPosition", handPosition);
+    // blend the wrist oreintation into the pole vector to reduce the painfully bent wrist problem.
-            _animVars.set("leftHandRotation", handRotation);
+    glm::quat elbowToHandDelta = handPose.rot() * glm::inverse(elbowPose.rot());
-            _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition);
+    const float WRIST_POLE_ADJUST_FACTOR = 0.5f;
+    glm::quat poleAdjust = quatLerp(Quaternions::IDENTITY, elbowToHandDelta, WRIST_POLE_ADJUST_FACTOR);
 
-            _lastLeftHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition);
+    return glm::normalize(poleAdjust * poleVector);
-        } else {
+}
-            if (_isLeftHandControlled) {
-                _leftHandRelaxTimeRemaining = RELAX_DURATION;
-                _isLeftHandControlled = false;
-            }
 
-            if (_leftHandRelaxTimeRemaining > 0.0f) {
+glm::vec3 Rig::calculateKneePoleVector(int footJointIndex, int kneeIndex, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const {
-                // Move hand from controlled position to non-controlled position.
-                _leftHandRelaxTimeRemaining = std::max(_leftHandRelaxTimeRemaining - dt, 0.0f);
-                AnimPose handPose;
-                if (transitionHandPose(_leftHandRelaxTimeRemaining, RELAX_DURATION, _lastLeftHandControlledPose, LEFT_HAND, 
-                        FROM_CONTROLLED, handPose)) {
-                    _animVars.set("leftHandPosition", handPose.trans());
-                    _animVars.set("leftHandRotation", handPose.rot());
-                    _animVars.set("leftHandType", (int)IKTarget::Type::RotationAndPosition);
-                }
-            } else {
-                _animVars.unset("leftHandPosition");
-                _animVars.unset("leftHandRotation");
-                _animVars.set("leftHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
-            }
-        }
 
-        if (params.isRightEnabled) {
+    AnimPose hipsPose = _externalPoseSet._absolutePoses[hipsIndex];
-            if (!_isRightHandControlled) {
+    AnimPose footPose = targetFootPose;
-                _rightHandControlTimeRemaining = CONTROL_DURATION;
+    AnimPose kneePose = _externalPoseSet._absolutePoses[kneeIndex];
-                _isRightHandControlled = true;
+    AnimPose upLegPose = _externalPoseSet._absolutePoses[upLegIndex];
-            }
 
-            glm::vec3 handPosition = params.rightPosition;
+    // ray from foot to upLeg
-            glm::quat handRotation = params.rightOrientation;
+    glm::vec3 d = glm::normalize(footPose.trans() - upLegPose.trans());
 
-            if (_rightHandControlTimeRemaining > 0.0f) {
+    // form a plane normal to the hips x-axis
-                // Move hand from non-controlled position to controlled position.
+    glm::vec3 n = hipsPose.rot() * Vectors::UNIT_X;
-                _rightHandControlTimeRemaining = std::max(_rightHandControlTimeRemaining - dt, 0.0f);
-                AnimPose handPose(Vectors::ONE, handRotation, handPosition);
-                if (transitionHandPose(_rightHandControlTimeRemaining, CONTROL_DURATION, handPose, RIGHT_HAND, TO_CONTROLLED, 
-                        handPose)) {
-                    handPosition = handPose.trans();
-                    handRotation = handPose.rot();
-                }
-            }
 
-            if (!bodySensorTrackingEnabled) {
+    // project d onto this plane
-                // prevent the hand IK targets from intersecting the body capsule
+    glm::vec3 dProj = d - glm::dot(d, n) * n;
-                glm::vec3 displacement;
-                if (findSphereCapsulePenetration(handPosition, HAND_RADIUS, bodyCapsuleStart, bodyCapsuleEnd, bodyCapsuleRadius, displacement)) {
-                    handPosition -= displacement;
-                }
-            }
 
-            _animVars.set("rightHandPosition", handPosition);
+    // rotate dProj by 90 degrees to get the poleVector.
-            _animVars.set("rightHandRotation", handRotation);
+    glm::vec3 poleVector = glm::angleAxis(-PI / 2.0f, n) * dProj;
-            _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition);
 
-            _lastRightHandControlledPose = AnimPose(Vectors::ONE, handRotation, handPosition);
+    // blend the foot oreintation into the pole vector
-        } else {
+    glm::quat kneeToFootDelta = footPose.rot() * glm::inverse(kneePose.rot());
-            if (_isRightHandControlled) {
+    const float WRIST_POLE_ADJUST_FACTOR = 0.5f;
-                _rightHandRelaxTimeRemaining = RELAX_DURATION;
+    glm::quat poleAdjust = quatLerp(Quaternions::IDENTITY, kneeToFootDelta, WRIST_POLE_ADJUST_FACTOR);
-                _isRightHandControlled = false;
-            }
 
-            if (_rightHandRelaxTimeRemaining > 0.0f) {
+    return glm::normalize(poleAdjust * poleVector);
-                // Move hand from controlled position to non-controlled position.
+}
-                _rightHandRelaxTimeRemaining = std::max(_rightHandRelaxTimeRemaining - dt, 0.0f);
-                AnimPose handPose;
-                if (transitionHandPose(_rightHandRelaxTimeRemaining, RELAX_DURATION, _lastRightHandControlledPose, RIGHT_HAND, 
-                        FROM_CONTROLLED, handPose)) {
-                    _animVars.set("rightHandPosition", handPose.trans());
-                    _animVars.set("rightHandRotation", handPose.rot());
-                    _animVars.set("rightHandType", (int)IKTarget::Type::RotationAndPosition);
-                }
-            } else {
-                _animVars.unset("rightHandPosition");
-                _animVars.unset("rightHandRotation");
-                _animVars.set("rightHandType", (int)IKTarget::Type::HipsRelativeRotationAndPosition);
-            }
-        }
 
-        if (params.isLeftFootEnabled) {
+void Rig::updateFromControllerParameters(const ControllerParameters& params, float dt) {
-            _animVars.set("leftFootPosition", params.leftFootPosition);
+    if (!_animSkeleton || !_animNode) {
-            _animVars.set("leftFootRotation", params.leftFootOrientation);
+        return;
-            _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition);
+    }
-        } else {
-            _animVars.unset("leftFootPosition");
-            _animVars.unset("leftFootRotation");
-            _animVars.set("leftFootType", (int)IKTarget::Type::RotationAndPosition);
-        }
 
-        if (params.isRightFootEnabled) {
+    _animVars.set("isTalking", params.isTalking);
-            _animVars.set("rightFootPosition", params.rightFootPosition);
+    _animVars.set("notIsTalking", !params.isTalking);
-            _animVars.set("rightFootRotation", params.rightFootOrientation);
+
-            _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition);
+    bool headEnabled = params.controllerActiveFlags[ControllerType_Head];
-        } else {
+    bool leftHandEnabled = params.controllerActiveFlags[ControllerType_LeftHand];
-            _animVars.unset("rightFootPosition");
+    bool rightHandEnabled = params.controllerActiveFlags[ControllerType_RightHand];
-            _animVars.unset("rightFootRotation");
+    bool hipsEnabled = params.controllerActiveFlags[ControllerType_Hips];
-            _animVars.set("rightFootType", (int)IKTarget::Type::RotationAndPosition);
+    bool leftFootEnabled = params.controllerActiveFlags[ControllerType_LeftFoot];
-        }
+    bool rightFootEnabled = params.controllerActiveFlags[ControllerType_RightFoot];
+    bool leftArmEnabled = params.controllerActiveFlags[ControllerType_LeftArm];
+    bool rightArmEnabled = params.controllerActiveFlags[ControllerType_RightArm];
+    bool spine2Enabled = params.controllerActiveFlags[ControllerType_Spine2];
+
+    updateHead(headEnabled, hipsEnabled, params.controllerPoses[ControllerType_Head]);
+
+    updateHands(leftHandEnabled, rightHandEnabled, hipsEnabled, leftArmEnabled, rightArmEnabled, dt,
+                params.controllerPoses[ControllerType_LeftHand], params.controllerPoses[ControllerType_RightHand],
+                params.bodyCapsuleRadius, params.bodyCapsuleHalfHeight, params.bodyCapsuleLocalOffset);
+
+    updateFeet(leftFootEnabled, rightFootEnabled,
+               params.controllerPoses[ControllerType_LeftFoot], params.controllerPoses[ControllerType_RightFoot]);
+
+    if (hipsEnabled) {
+        _animVars.set("solutionSource", (int)AnimInverseKinematics::SolutionSource::RelaxToLimitCenterPoses);
+        _animVars.set("hipsType", (int)IKTarget::Type::RotationAndPosition);
+        _animVars.set("hipsPosition", params.controllerPoses[ControllerType_Hips].trans());
+        _animVars.set("hipsRotation", params.controllerPoses[ControllerType_Hips].rot());
+    } else {
+        _animVars.set("solutionSource", (int)AnimInverseKinematics::SolutionSource::RelaxToUnderPoses);
+        _animVars.set("hipsType", (int)IKTarget::Type::Unknown);
+    }
+
+    if (hipsEnabled && spine2Enabled) {
+        _animVars.set("spine2Type", (int)IKTarget::Type::Spline);
+        _animVars.set("spine2Position", params.controllerPoses[ControllerType_Spine2].trans());
+        _animVars.set("spine2Rotation", params.controllerPoses[ControllerType_Spine2].rot());
+    } else {
+        _animVars.set("spine2Type", (int)IKTarget::Type::Unknown);
+    }
+
+    if (leftArmEnabled) {
+        _animVars.set("leftArmType", (int)IKTarget::Type::RotationAndPosition);
+        _animVars.set("leftArmPosition", params.controllerPoses[ControllerType_LeftArm].trans());
+        _animVars.set("leftArmRotation", params.controllerPoses[ControllerType_LeftArm].rot());
+    } else {
+        _animVars.set("leftArmType", (int)IKTarget::Type::Unknown);
+    }
+
+    if (rightArmEnabled) {
+        _animVars.set("rightArmType", (int)IKTarget::Type::RotationAndPosition);
+        _animVars.set("rightArmPosition", params.controllerPoses[ControllerType_RightArm].trans());
+        _animVars.set("rightArmRotation", params.controllerPoses[ControllerType_RightArm].rot());
+    } else {
+        _animVars.set("rightArmType", (int)IKTarget::Type::Unknown);
     }
 }
 
@@ -1486,22 +1676,18 @@ void Rig::computeAvatarBoundingCapsule(
     AnimInverseKinematics ikNode("boundingShape");
     ikNode.setSkeleton(_animSkeleton);
 
-    ikNode.setTargetVars("LeftHand",
+    ikNode.setTargetVars("LeftHand", "leftHandPosition", "leftHandRotation",
-                         "leftHandPosition",
+                         "leftHandType", "leftHandWeight", 1.0f, {},
-                         "leftHandRotation",
+                         QString(), QString(), QString());
-                         "leftHandType", "leftHandWeight", 1.0f, {});
+    ikNode.setTargetVars("RightHand", "rightHandPosition", "rightHandRotation",
-    ikNode.setTargetVars("RightHand",
+                         "rightHandType", "rightHandWeight", 1.0f, {},
-                         "rightHandPosition",
+                         QString(), QString(), QString());
-                         "rightHandRotation",
+    ikNode.setTargetVars("LeftFoot", "leftFootPosition", "leftFootRotation",
-                         "rightHandType", "rightHandWeight", 1.0f, {});
+                         "leftFootType", "leftFootWeight", 1.0f, {},
-    ikNode.setTargetVars("LeftFoot",
+                         QString(), QString(), QString());
-                         "leftFootPosition",
+    ikNode.setTargetVars("RightFoot", "rightFootPosition", "rightFootRotation",
-                         "leftFootRotation",
+                         "rightFootType", "rightFootWeight", 1.0f, {},
-                         "leftFootType", "leftFootWeight", 1.0f, {});
+                         QString(), QString(), QString());
-    ikNode.setTargetVars("RightFoot",
-                         "rightFootPosition",
-                         "rightFootRotation",
-                         "rightFootType", "rightFootWeight", 1.0f, {});
 
     AnimPose geometryToRig = _modelOffset * _geometryOffset;
 

libraries/animation/src/Rig.h

@@ -41,21 +41,26 @@ public:
         bool useNames;
     };
 
-    struct HeadParameters {
+    enum ControllerType {
-        glm::mat4 hipsMatrix = glm::mat4();           // rig space
+        ControllerType_Head = 0,
-        glm::mat4 spine2Matrix = glm::mat4();         // rig space
+        ControllerType_LeftHand,
-        glm::quat rigHeadOrientation = glm::quat();   // rig space (-z forward)
+        ControllerType_RightHand,
-        glm::vec3 rigHeadPosition = glm::vec3();      // rig space
+        ControllerType_Hips,
-        glm::vec3 rightArmPosition = glm::vec3();     // rig space
+        ControllerType_LeftFoot,
-        glm::quat rightArmRotation = glm::quat();     // rig space
+        ControllerType_RightFoot,
-        glm::vec3 leftArmPosition = glm::vec3();      // rig space
+        ControllerType_LeftArm,
-        glm::quat leftArmRotation = glm::quat();      // rig space
+        ControllerType_RightArm,
-        bool hipsEnabled = false;
+        ControllerType_Spine2,
-        bool headEnabled = false;
+        NumControllerTypes
-        bool spine2Enabled = false;
+    };
-        bool leftArmEnabled = false;
+
-        bool rightArmEnabled = false;
+    struct ControllerParameters {
-        bool isTalking = false;
+        AnimPose controllerPoses[NumControllerTypes];  // rig space
+        bool controllerActiveFlags[NumControllerTypes];
+        bool isTalking;
+        float bodyCapsuleRadius;
+        float bodyCapsuleHalfHeight;
+        glm::vec3 bodyCapsuleLocalOffset;
     };
 
     struct EyeParameters {
@@ -67,25 +72,6 @@ public:
         int rightEyeJointIndex = -1;
     };
 
-    struct HandAndFeetParameters {
-        bool isLeftEnabled;
-        bool isRightEnabled;
-        float bodyCapsuleRadius;
-        float bodyCapsuleHalfHeight;
-        glm::vec3 bodyCapsuleLocalOffset;
-        glm::vec3 leftPosition = glm::vec3();     // rig space
-        glm::quat leftOrientation = glm::quat();  // rig space (z forward)
-        glm::vec3 rightPosition = glm::vec3();    // rig space
-        glm::quat rightOrientation = glm::quat(); // rig space (z forward)
-
-        bool isLeftFootEnabled;
-        bool isRightFootEnabled;
-        glm::vec3 leftFootPosition = glm::vec3();     // rig space
-        glm::quat leftFootOrientation = glm::quat();  // rig space (z forward)
-        glm::vec3 rightFootPosition = glm::vec3();    // rig space
-        glm::quat rightFootOrientation = glm::quat(); // rig space (z forward)
-    };
-
     enum class CharacterControllerState {
         Ground = 0,
         Takeoff,
@@ -153,9 +139,6 @@ public:
     bool getAbsoluteJointTranslationInRigFrame(int jointIndex, glm::vec3& translation) const;
     bool getAbsoluteJointPoseInRigFrame(int jointIndex, AnimPose& returnPose) const;
 
-    // legacy
-    bool getJointCombinedRotation(int jointIndex, glm::quat& result, const glm::quat& rotation) const;
-
     // rig space
     glm::mat4 getJointTransform(int jointIndex) const;
 
@@ -195,9 +178,8 @@ public:
     // legacy
     void clearJointStatePriorities();
 
-    void updateFromHeadParameters(const HeadParameters& params, float dt);
+    void updateFromControllerParameters(const ControllerParameters& params, float dt);
     void updateFromEyeParameters(const EyeParameters& params);
-    void updateFromHandAndFeetParameters(const HandAndFeetParameters& params, float dt);
 
     void initAnimGraph(const QUrl& url);
 
@@ -247,11 +229,18 @@ protected:
     void applyOverridePoses();
     void buildAbsoluteRigPoses(const AnimPoseVec& relativePoses, AnimPoseVec& absolutePosesOut);
 
-    void updateHeadAnimVars(const HeadParameters& params);
+    void updateHead(bool headEnabled, bool hipsEnabled, const AnimPose& headMatrix);
+    void updateHands(bool leftHandEnabled, bool rightHandEnabled, bool hipsEnabled, bool leftArmEnabled, bool rightArmEnabled, float dt,
+                     const AnimPose& leftHandPose, const AnimPose& rightHandPose,
+                     float bodyCapsuleRadius, float bodyCapsuleHalfHeight, const glm::vec3& bodyCapsuleLocalOffset);
+    void updateFeet(bool leftFootEnabled, bool rightFootEnabled, const AnimPose& leftFootPose, const AnimPose& rightFootPose);
 
     void updateEyeJoint(int index, const glm::vec3& modelTranslation, const glm::quat& modelRotation, const glm::vec3& lookAt, const glm::vec3& saccade);
     void calcAnimAlpha(float speed, const std::vector<float>& referenceSpeeds, float* alphaOut) const;
 
+    glm::vec3 calculateElbowPoleVector(int handIndex, int elbowIndex, int armIndex, int hipsIndex, bool isLeft) const;
+    glm::vec3 calculateKneePoleVector(int footJointIndex, int kneeJoint, int upLegIndex, int hipsIndex, const AnimPose& targetFootPose) const;
+
     AnimPose _modelOffset;  // model to rig space
     AnimPose _geometryOffset; // geometry to model space (includes unit offset & fst offsets)
     AnimPose _invGeometryOffset;
@@ -347,13 +336,12 @@ protected:
     bool _enableDebugDrawIKConstraints { false };
     bool _enableDebugDrawIKChains { false };
 
-private:
     QMap<int, StateHandler> _stateHandlers;
     int _nextStateHandlerId { 0 };
     QMutex _stateMutex;
 
-    bool transitionHandPose(float deltaTime, float totalDuration, AnimPose& controlledHandPose, bool isLeftHand, 
+    bool transitionHandPose(float deltaTime, float totalDuration, AnimPose& controlledHandPose, bool isLeftHand,
-        bool isToControlled, AnimPose& returnHandPose);
+                            bool isToControlled, AnimPose& returnHandPose);
 
     bool _isLeftHandControlled { false };
     bool _isRightHandControlled { false };
@@ -363,6 +351,18 @@ private:
     float _rightHandRelaxTimeRemaining { 0.0f };
     AnimPose _lastLeftHandControlledPose;
     AnimPose _lastRightHandControlledPose;
+
+    glm::vec3 _prevRightFootPoleVector { Vectors::UNIT_Z };
+    bool _prevRightFootPoleVectorValid { false };
+
+    glm::vec3 _prevLeftFootPoleVector { Vectors::UNIT_Z };
+    bool _prevLeftFootPoleVectorValid { false };
+
+    glm::vec3 _prevRightHandPoleVector { -Vectors::UNIT_Z };
+    bool _prevRightHandPoleVectorValid { false };
+
+    glm::vec3 _prevLeftHandPoleVector { -Vectors::UNIT_Z };
+    bool _prevLeftHandPoleVectorValid { false };
 };
 
 #endif /* defined(__hifi__Rig__) */

libraries/render-utils/src/Model.cpp

@@ -867,10 +867,6 @@ bool Model::getRelativeDefaultJointTranslation(int jointIndex, glm::vec3& transl
     return _rig.getRelativeDefaultJointTranslation(jointIndex, translationOut);
 }
 
-bool Model::getJointCombinedRotation(int jointIndex, glm::quat& rotation) const {
-    return _rig.getJointCombinedRotation(jointIndex, rotation, _rotation);
-}
-
 QStringList Model::getJointNames() const {
     if (QThread::currentThread() != thread()) {
         QStringList result;

libraries/render-utils/src/Model.h

@@ -177,7 +177,6 @@ public:
     int getJointStateCount() const { return (int)_rig.getJointStateCount(); }
     bool getJointPositionInWorldFrame(int jointIndex, glm::vec3& position) const;
     bool getJointRotationInWorldFrame(int jointIndex, glm::quat& rotation) const;
-    bool getJointCombinedRotation(int jointIndex, glm::quat& rotation) const;
 
     /// \param jointIndex index of joint in model structure
     /// \param rotation[out] rotation of joint in model-frame