////  AnimInverseKinematics.cpp////  Copyright 2015 High Fidelity, Inc.////  Distributed under the Apache License, Version 2.0.//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html// #include "AnimInverseKinematics.h" #include <GeometryUtil.h>#include <GLMHelpers.h>#include <NumericalConstants.h>#include <SharedUtil.h>#include <shared/NsightHelpers.h>#include <DebugDraw.h>#include <QElapsedTimer>#include "Rig.h" #include "ElbowConstraint.h"#include "SwingTwistConstraint.h"#include "AnimationLogging.h"#include "CubicHermiteSpline.h"#include "AnimUtil.h"#include "AnimSkeleton.h" static const int MAX_TARGET_MARKERS = 30;static const float JOINT_CHAIN_INTERP_TIME = 0.5f; static QElapsedTimer debounceJointWarningsClock;static const int JOINT_WARNING_DEBOUNCE_TIME = 30000; // 30 seconds static void lookupJointInfo(const AnimInverseKinematics::JointChainInfo& jointChainInfo,                            int indexA, int indexB,                            const AnimInverseKinematics::JointInfo** jointInfoA,                            const AnimInverseKinematics::JointInfo** jointInfoB) {    *jointInfoA = nullptr;    *jointInfoB = nullptr;    for (size_t i = 0; i < jointChainInfo.jointInfoVec.size(); i++) {        const AnimInverseKinematics::JointInfo* jointInfo = &jointChainInfo.jointInfoVec[i];        if (jointInfo->jointIndex == indexA) {            *jointInfoA = jointInfo;        }        if (jointInfo->jointIndex == indexB) {            *jointInfoB = jointInfo;        }        if (*jointInfoA && *jointInfoB) {            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& 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(AnimSkeleton::INVALID_JOINT_INDEX){    numFlexCoefficients = std::min(numFlexCoefficients, (size_t)MAX_FLEX_COEFFICIENTS);    for (size_t i = 0; i < numFlexCoefficients; i++) {        flexCoefficients[i] = flexCoefficientsIn[i];    }} AnimInverseKinematics::IKTargetVar::IKTargetVar(const IKTargetVar& orig) :    jointName(orig.jointName),    positionVar(orig.positionVar),    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){    numFlexCoefficients = std::min(numFlexCoefficients, (size_t)MAX_FLEX_COEFFICIENTS);    for (size_t i = 0; i < numFlexCoefficients; i++) {        flexCoefficients[i] = orig.flexCoefficients[i];    }} AnimInverseKinematics::AnimInverseKinematics(const QString& id) : AnimNode(AnimNode::Type::InverseKinematics, id) {    debounceJointWarningsClock.start();} AnimInverseKinematics::~AnimInverseKinematics() {    clearConstraints();    _rotationAccumulators.clear();    _translationAccumulators.clear();    _targetVarVec.clear();     // remove markers    for (int i = 0; i < MAX_TARGET_MARKERS; i++) {        QString name = QString("ikTarget%1").arg(i);        DebugDraw::getInstance().removeMyAvatarMarker(name);    }} void AnimInverseKinematics::loadDefaultPoses(const AnimPoseVec& poses) {    _defaultRelativePoses = poses;    assert(_skeleton && _skeleton->getNumJoints() == (int)poses.size());} void AnimInverseKinematics::loadPoses(const AnimPoseVec& poses) {    assert(_skeleton && ((poses.size() == 0) || (_skeleton->getNumJoints() == (int)poses.size())));    if (_skeleton->getNumJoints() == (int)poses.size()) {        _relativePoses = poses;        _rotationAccumulators.resize(_relativePoses.size());        _translationAccumulators.resize(_relativePoses.size());    } else {        _relativePoses.clear();        _rotationAccumulators.clear();        _translationAccumulators.clear();    }} void AnimInverseKinematics::computeAbsolutePoses(AnimPoseVec& absolutePoses) const {    int numJoints = (int)_relativePoses.size();    assert(numJoints <= _skeleton->getNumJoints());    assert(numJoints == (int)absolutePoses.size());    for (int i = 0; i < numJoints; ++i) {        int parentIndex = _skeleton->getParentIndex(i);        if (parentIndex < 0) {            absolutePoses[i] = _relativePoses[i];        } else {            absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i];        }    }} 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& 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;    for (auto& targetVarIter: _targetVarVec) {        if (targetVarIter.jointName == jointName) {            targetVarIter = targetVar;            found = true;            break;        }    }    if (!found) {        // create a new entry        _targetVarVec.push_back(targetVar);    }} bool debounceJointWarnings() {    if (debounceJointWarningsClock.elapsed() >= JOINT_WARNING_DEBOUNCE_TIME) {        debounceJointWarningsClock.restart();        return true;    }    return false;} void AnimInverseKinematics::computeTargets(const AnimVariantMap& animVars, std::vector<IKTarget>& targets, const AnimPoseVec& underPoses) {     _hipsTargetIndex = AnimSkeleton::INVALID_JOINT_INDEX;     targets.reserve(_targetVarVec.size());     for (auto& targetVar : _targetVarVec) {         // update targetVar jointIndex cache        if (targetVar.jointIndex == AnimSkeleton::INVALID_JOINT_INDEX) {            int jointIndex = _skeleton->nameToJointIndex(targetVar.jointName);            if (jointIndex >= 0) {                // this targetVar has a valid joint --> cache the indices                targetVar.jointIndex = jointIndex;            } else if (debounceJointWarnings()) {                qCWarning(animation) << "AnimInverseKinematics could not find jointName" << targetVar.jointName << "in skeleton";            }        }         IKTarget target;        if (targetVar.jointIndex != AnimSkeleton::INVALID_JOINT_INDEX) {            target.setType(animVars.lookup(targetVar.typeVar, (int)IKTarget::Type::RotationAndPosition));            target.setIndex(targetVar.jointIndex);            if (target.getType() != IKTarget::Type::Unknown) {                AnimPose absPose = _skeleton->getAbsolutePose(targetVar.jointIndex, underPoses);                glm::quat rotation = animVars.lookupRigToGeometry(targetVar.rotationVar, absPose.rot());                glm::vec3 translation = animVars.lookupRigToGeometry(targetVar.positionVar, absPose.trans());                float weight = animVars.lookup(targetVar.weightVar, targetVar.weight);                 target.setPose(rotation, translation);                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));                 // record the index of the hips ik target.                if (target.getIndex() == _hipsIndex) {                    _hipsTargetIndex = (int)targets.size();                }            }        } else {            target.setType((int)IKTarget::Type::Unknown);        }         targets.push_back(target);    }} 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<IKTarget>& targets, float dt, JointChainInfoVec& jointChainInfoVec) {    // compute absolute poses that correspond to relative target poses    AnimPoseVec absolutePoses;    absolutePoses.resize(_relativePoses.size());    computeAbsolutePoses(absolutePoses);     // clear the accumulators before we start the IK solver    for (auto& accumulator : _rotationAccumulators) {        accumulator.clearAndClean();    }    for (auto& accumulator : _translationAccumulators) {        accumulator.clearAndClean();    }     std::map<int, int> targetToChainMap;     float maxError = 0.0f;    int numLoops = 0;    const int MAX_IK_LOOPS = 16;    while (numLoops < MAX_IK_LOOPS) {        ++numLoops;         bool debug = context.getEnableDebugDrawIKChains() && numLoops == MAX_IK_LOOPS;         // solve all targets        for (size_t i = 0; i < targets.size(); i++) {            switch (targets[i].getType()) {            case IKTarget::Type::Unknown:                break;            case IKTarget::Type::Spline:                solveTargetWithSpline(context, targets[i], absolutePoses, debug, jointChainInfoVec[i]);                break;            default:                solveTargetWithCCD(context, targets[i], absolutePoses, debug, jointChainInfoVec[i]);                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<int, int>(_prevJointChainInfoVec[i].target.getIndex(), (int)i));                if (_prevJointChainInfoVec[i].timer > 0.0f) {                    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) {                        // if we are interping into an enabled target type, i.e. not off, lerp the rot and the trans.                        for (size_t j = 0; j < chainSize; j++) {                            jointChainInfoVec[i].jointInfoVec[j].rot = safeMix(_prevJointChainInfoVec[i].jointInfoVec[j].rot, jointChainInfoVec[i].jointInfoVec[j].rot, alpha);                            jointChainInfoVec[i].jointInfoVec[j].trans = lerp(_prevJointChainInfoVec[i].jointInfoVec[j].trans, jointChainInfoVec[i].jointInfoVec[j].trans, alpha);                        }                    } else {                        // if we are interping into a disabled target type, keep the rot & trans the same, but lerp the weight down to zero.                        jointChainInfoVec[i].target.setType((int)_prevJointChainInfoVec[i].target.getType());                        jointChainInfoVec[i].target.setWeight(_prevJointChainInfoVec[i].target.getWeight() * (1.0f - alpha));                        jointChainInfoVec[i].jointInfoVec = _prevJointChainInfoVec[i].jointInfoVec;                    }                }            }        }         // copy jointChainInfoVecs into accumulators        for (size_t i = 0; i < targets.size(); i++) {            const std::vector<JointInfo>& jointInfoVec = jointChainInfoVec[i].jointInfoVec;             // don't accumulate disabled or rotation only ik targets.            IKTarget::Type type = jointChainInfoVec[i].target.getType();            if (type != IKTarget::Type::Unknown && type != IKTarget::Type::RotationOnly) {                float weight = jointChainInfoVec[i].target.getWeight();                if (weight > 0.0f) {                    for (size_t j = 0; j < jointInfoVec.size(); j++) {                        const JointInfo& info = jointInfoVec[j];                        if (info.jointIndex >= 0) {                            _rotationAccumulators[info.jointIndex].add(info.rot, weight);                            _translationAccumulators[info.jointIndex].add(info.trans, weight);                        }                    }                }            }        }         // harvest accumulated rotations and apply the average        // don't apply accumulators to hips, or parents of hips        for (int i = (_hipsIndex+1); i < (int)_relativePoses.size(); ++i) {            if (_rotationAccumulators[i].size() > 0) {                _relativePoses[i].rot() = _rotationAccumulators[i].getAverage();                _rotationAccumulators[i].clear();            }            if (_translationAccumulators[i].size() > 0) {                _relativePoses[i].trans() = _translationAccumulators[i].getAverage();                _translationAccumulators[i].clear();            }        }         // update the absolutePoses        for (int i = 0; i < (int)_relativePoses.size(); ++i) {            auto parentIndex = _skeleton->getParentIndex((int)i);            if (parentIndex != AnimSkeleton::INVALID_JOINT_INDEX) {                absolutePoses[i] = absolutePoses[parentIndex] * _relativePoses[i];            }        }         // compute maxError        maxError = 0.0f;        for (size_t i = 0; i < targets.size(); i++) {            if (targets[i].getType() == IKTarget::Type::RotationAndPosition || targets[i].getType() == IKTarget::Type::HmdHead ||                targets[i].getType() == IKTarget::Type::HipsRelativeRotationAndPosition) {                float error = glm::length(absolutePoses[targets[i].getIndex()].trans() - targets[i].getTranslation());                if (error > maxError) {                    maxError = error;                }            }        }    }    _maxErrorOnLastSolve = maxError;     // finally set the relative rotation of each tip to agree with absolute target rotation    for (auto& target: targets) {        int tipIndex = target.getIndex();        int parentIndex = (tipIndex >= 0) ? _skeleton->getParentIndex(tipIndex) : AnimSkeleton::INVALID_JOINT_INDEX;        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 != AnimSkeleton::INVALID_JOINT_INDEX && !_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<int, glm::quat>(tipIndex, _relativePoses[tipIndex].rot()));                } else {                    _rotationOnlyIKRotations[tipIndex] = _relativePoses[tipIndex].rot();                }                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);                }            }        }    }     // copy jointChainInfoVec into _prevJointChainInfoVec, and update timers    for (size_t i = 0; i < jointChainInfoVec.size(); i++) {        _prevJointChainInfoVec[i].timer = _prevJointChainInfoVec[i].timer - dt;        if (_prevJointChainInfoVec[i].timer <= 0.0f) {            _prevJointChainInfoVec[i] = jointChainInfoVec[i];            _prevJointChainInfoVec[i].target = targets[i];            // store relative poses into unknown/rotation only joint chains.            // so we have something to interpolate from if this chain is activated.            IKTarget::Type type = _prevJointChainInfoVec[i].target.getType();            if (type == IKTarget::Type::Unknown || type == IKTarget::Type::RotationOnly) {                for (size_t j = 0; j < _prevJointChainInfoVec[i].jointInfoVec.size(); j++) {                    auto& info = _prevJointChainInfoVec[i].jointInfoVec[j];                    if (info.jointIndex >= 0) {                        info.rot = _relativePoses[info.jointIndex].rot();                        info.trans = _relativePoses[info.jointIndex].trans();                    } else {                        info.rot = Quaternions::IDENTITY;                        info.trans = glm::vec3(0.0f);                    }                }            }        }    }} void AnimInverseKinematics::solveTargetWithCCD(const AnimContext& context, const IKTarget& target, const AnimPoseVec& absolutePoses,                                               bool debug, JointChainInfo& jointChainInfoOut) const {    size_t chainDepth = 0;     IKTarget::Type targetType = target.getType();    if (targetType == IKTarget::Type::RotationOnly) {        // the final rotation will be enforced after the iterations        // TODO: solve this correctly        return;    }     int tipIndex = target.getIndex();    int pivotIndex = _skeleton->getParentIndex(tipIndex);    if (pivotIndex == AnimSkeleton::INVALID_JOINT_INDEX || pivotIndex == _hipsIndex) {        return;    }    int pivotsParentIndex = _skeleton->getParentIndex(pivotIndex);    if (pivotsParentIndex == AnimSkeleton::INVALID_JOINT_INDEX) {        // TODO?: handle case where tip's parent is root?        return;    }     // cache tip's absolute orientation    glm::quat tipOrientation = absolutePoses[tipIndex].rot();     // also cache tip's parent's absolute orientation so we can recompute    // the tip's parent-relative as we proceed up the chain    glm::quat tipParentOrientation = absolutePoses[pivotIndex].rot();     // NOTE: if this code is removed, the head will remain rigid, causing the spine/hips to thrust forward backward    // as the head is nodded.    if (targetType == IKTarget::Type::HmdHead ||        targetType == IKTarget::Type::RotationAndPosition ||        targetType == IKTarget::Type::HipsRelativeRotationAndPosition) {         // rotate tip toward target orientation        glm::quat deltaRot = target.getRotation() * glm::inverse(tipOrientation);         deltaRot *= target.getFlexCoefficient(chainDepth);        glm::normalize(deltaRot);         // compute parent relative rotation        glm::quat tipRelativeRotation = glm::inverse(tipParentOrientation) * deltaRot * tipOrientation;         // then enforce tip's constraint        RotationConstraint* constraint = getConstraint(tipIndex);        bool constrained = false;        if (constraint) {            constrained = constraint->apply(tipRelativeRotation);            if (constrained) {                tipOrientation = tipParentOrientation * tipRelativeRotation;            }        }         glm::vec3 tipRelativeTranslation = _relativePoses[target.getIndex()].trans();        jointChainInfoOut.jointInfoVec[chainDepth] = { tipRelativeRotation, tipRelativeTranslation, tipIndex, constrained };    }     // cache tip absolute position    glm::vec3 tipPosition = absolutePoses[tipIndex].trans();     chainDepth++;     // descend toward root, pivoting each joint to get tip closer to target position    while (pivotIndex != _hipsIndex && pivotsParentIndex != AnimSkeleton::INVALID_JOINT_INDEX) {         assert(chainDepth < jointChainInfoOut.jointInfoVec.size());         // compute the two lines that should be aligned        glm::vec3 jointPosition = absolutePoses[pivotIndex].trans();        glm::vec3 leverArm = tipPosition - jointPosition;         glm::quat deltaRotation;        if (targetType == IKTarget::Type::RotationAndPosition ||            targetType == IKTarget::Type::HipsRelativeRotationAndPosition) {

Lines 1–500 of 1897. Load more