diff --git a/libraries/animation/src/AnimInverseKinematics.cpp b/libraries/animation/src/AnimInverseKinematics.cpp index 8d98f9c775..db8799db46 100644 --- a/libraries/animation/src/AnimInverseKinematics.cpp +++ b/libraries/animation/src/AnimInverseKinematics.cpp @@ -213,6 +213,12 @@ void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std:: } } +float AnimInverseKinematics::getInterpolationAlpha(float timer) { + float alpha = (JOINT_CHAIN_INTERP_TIME - timer) / JOINT_CHAIN_INTERP_TIME; + alpha = 1.0f - powf(2.0f, -10.0f * alpha); + return alpha; +} + void AnimInverseKinematics::solve(const AnimContext& context, const std::vector& targets, float dt, JointChainInfoVec& jointChainInfoVec) { // compute absolute poses that correspond to relative target poses AnimPoseVec absolutePoses; @@ -227,6 +233,8 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector< accumulator.clearAndClean(); } + std::map targetToChainMap; + float maxError = 0.0f; int numLoops = 0; const int MAX_IK_LOOPS = 16; @@ -248,17 +256,13 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector< break; } } - + // on last iteration, interpolate jointChains, if necessary if (numLoops == MAX_IK_LOOPS) { for (size_t i = 0; i < _prevJointChainInfoVec.size(); i++) { + targetToChainMap.insert(std::pair(_prevJointChainInfoVec[i].target.getIndex(), (int)i)); if (_prevJointChainInfoVec[i].timer > 0.0f) { - - float alpha = (JOINT_CHAIN_INTERP_TIME - _prevJointChainInfoVec[i].timer) / JOINT_CHAIN_INTERP_TIME; - - // ease in expo - alpha = 1.0f - powf(2.0f, -10.0f * alpha); - + float alpha = getInterpolationAlpha(_prevJointChainInfoVec[i].timer); size_t chainSize = std::min(_prevJointChainInfoVec[i].jointInfoVec.size(), jointChainInfoVec[i].jointInfoVec.size()); if (jointChainInfoVec[i].target.getType() != IKTarget::Type::Unknown) { @@ -336,22 +340,47 @@ void AnimInverseKinematics::solve(const AnimContext& context, const std::vector< for (auto& target: targets) { int tipIndex = target.getIndex(); int parentIndex = (tipIndex >= 0) ? _skeleton->getParentIndex(tipIndex) : -1; - + int chainIndex = targetToChainMap[tipIndex]; + bool needsInterpolation = _prevJointChainInfoVec[chainIndex].timer > 0.0f; + float alpha = needsInterpolation ? getInterpolationAlpha(_prevJointChainInfoVec[chainIndex].timer) : 0.0f; // update rotationOnly targets that don't lie on the ik chain of other ik targets. - if (parentIndex != -1 && !_rotationAccumulators[tipIndex].isDirty() && target.getType() == IKTarget::Type::RotationOnly) { - const glm::quat& targetRotation = target.getRotation(); - // compute tip's new parent-relative rotation - // Q = Qp * q --> q' = Qp^ * Q - glm::quat newRelativeRotation = glm::inverse(absolutePoses[parentIndex].rot()) * targetRotation; - RotationConstraint* constraint = getConstraint(tipIndex); - if (constraint) { - constraint->apply(newRelativeRotation); - // TODO: ATM the final rotation target just fails but we need to provide - // feedback to the IK system so that it can adjust the bones up the skeleton - // to help this rotation target get met. + if (parentIndex != -1 && !_rotationAccumulators[tipIndex].isDirty() && + (target.getType() == IKTarget::Type::RotationOnly || target.getType() == IKTarget::Type::Unknown)) { + if (target.getType() == IKTarget::Type::RotationOnly) { + const glm::quat& targetRotation = target.getRotation(); + // compute tip's new parent-relative rotation + // Q = Qp * q --> q' = Qp^ * Q + glm::quat newRelativeRotation = glm::inverse(absolutePoses[parentIndex].rot()) * targetRotation; + RotationConstraint* constraint = getConstraint(tipIndex); + if (constraint) { + constraint->apply(newRelativeRotation); + // TODO: ATM the final rotation target just fails but we need to provide + // feedback to the IK system so that it can adjust the bones up the skeleton + // to help this rotation target get met. + } + if (needsInterpolation) { + _relativePoses[tipIndex].rot() = safeMix(_relativePoses[tipIndex].rot(), newRelativeRotation, alpha); + } else { + _relativePoses[tipIndex].rot() = newRelativeRotation; + // Add last known rotations to interpolate from + if (_rotationOnlyIKRotations.find(tipIndex) == _rotationOnlyIKRotations.end()) { + _rotationOnlyIKRotations.insert(std::pair(tipIndex, newRelativeRotation)); + } else { + _rotationOnlyIKRotations[tipIndex] = newRelativeRotation; + } + } + absolutePoses[tipIndex].rot() = targetRotation; + } else { + bool rotationSnapshotExist = _rotationOnlyIKRotations.find(tipIndex) != _rotationOnlyIKRotations.end(); + if (needsInterpolation) { + if (rotationSnapshotExist) { + glm::quat lastKnownRotation = _rotationOnlyIKRotations[tipIndex]; + _relativePoses[tipIndex].rot() = safeMix(_relativePoses[tipIndex].rot(), lastKnownRotation, (1 - alpha)); + } + } else if (rotationSnapshotExist) { + _rotationOnlyIKRotations.erase(tipIndex); + } } - _relativePoses[tipIndex].rot() = newRelativeRotation; - absolutePoses[tipIndex].rot() = targetRotation; } } @@ -928,6 +957,10 @@ const AnimPoseVec& AnimInverseKinematics::overlay(const AnimVariantMap& animVars (jointChainInfoVec[i].target.getType() != _prevJointChainInfoVec[i].target.getType() || jointChainInfoVec[i].target.getPoleVectorEnabled() != _prevJointChainInfoVec[i].target.getPoleVectorEnabled())) { _prevJointChainInfoVec[i].timer = JOINT_CHAIN_INTERP_TIME; + // Clear the rotations when the target is known + if (jointChainInfoVec[i].target.getType() != IKTarget::Type::Unknown) { + _rotationOnlyIKRotations.erase(jointChainInfoVec[i].target.getIndex()); + } } } } diff --git a/libraries/animation/src/AnimInverseKinematics.h b/libraries/animation/src/AnimInverseKinematics.h index 6d58ecedec..8d3f898e67 100644 --- a/libraries/animation/src/AnimInverseKinematics.h +++ b/libraries/animation/src/AnimInverseKinematics.h @@ -148,6 +148,7 @@ protected: void clearConstraints(); void initConstraints(); void initLimitCenterPoses(); + float getInterpolationAlpha(float timer); // no copies AnimInverseKinematics(const AnimInverseKinematics&) = delete; @@ -181,6 +182,7 @@ protected: AnimPoseVec _defaultRelativePoses; // poses of the relaxed state AnimPoseVec _relativePoses; // current relative poses AnimPoseVec _limitCenterPoses; // relative + std::map _rotationOnlyIKRotations; std::map _secondaryTargetsInRigFrame;