////  AnimOverlay.cpp////  Created by Anthony J. Thibault on 9/2/15.//  Copyright (c) 2015 High Fidelity, Inc. All rights reserved.////  Distributed under the Apache License, Version 2.0.//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html// #include "AnimOverlay.h"#include "AnimUtil.h"#include <queue> AnimOverlay::AnimOverlay(const QString& id, BoneSet boneSet, float alpha) :    AnimNode(AnimNode::Type::Overlay, id), _boneSet(boneSet), _alpha(alpha) {} AnimOverlay::~AnimOverlay() { } void AnimOverlay::buildBoneSet(BoneSet boneSet) {    assert(_skeleton);    switch (boneSet) {    case FullBodyBoneSet: buildFullBodyBoneSet(); break;    case UpperBodyBoneSet: buildUpperBodyBoneSet(); break;    case LowerBodyBoneSet: buildLowerBodyBoneSet(); break;    case LeftArmBoneSet: buildLeftArmBoneSet(); break;    case RightArmBoneSet: buildRightArmBoneSet(); break;    case AboveTheHeadBoneSet: buildAboveTheHeadBoneSet(); break;    case BelowTheHeadBoneSet: buildBelowTheHeadBoneSet(); break;    case HeadOnlyBoneSet: buildHeadOnlyBoneSet(); break;    case SpineOnlyBoneSet: buildSpineOnlyBoneSet(); break;    case LeftHandBoneSet: buildLeftHandBoneSet(); break;    case RightHandBoneSet: buildRightHandBoneSet(); break;    case HipsOnlyBoneSet: buildHipsOnlyBoneSet(); break;    case BothFeetBoneSet: buildBothFeetBoneSet(); break;    default:    case EmptyBoneSet: buildEmptyBoneSet(); break;    }} const AnimPoseVec& AnimOverlay::evaluate(const AnimVariantMap& animVars, const AnimContext& context, float dt, AnimVariantMap& triggersOut) {     // lookup parameters from animVars, using current instance variables as defaults.    // NOTE: switching bonesets can be an expensive operation, let's try to avoid it.    auto prevBoneSet = _boneSet;    _boneSet = (BoneSet)animVars.lookup(_boneSetVar, (int)_boneSet);    if (_boneSet != prevBoneSet && _skeleton) {        buildBoneSet(_boneSet);    }    _alpha = animVars.lookup(_alphaVar, _alpha);     if (_children.size() >= 2) {        auto& underPoses = _children[1]->evaluate(animVars, context, dt, triggersOut);         if (_alpha == 0.0f) {            _poses = underPoses;        } else {            auto& overPoses = _children[0]->overlay(animVars, context, dt, triggersOut, underPoses);             if (underPoses.size() > 0 && underPoses.size() == overPoses.size()) {                _poses.resize(underPoses.size());                assert(_boneSetVec.size() == _poses.size());                 for (size_t i = 0; i < _poses.size(); i++) {                    float alpha = _boneSetVec[i] * _alpha;                    ::blend(1, &underPoses[i], &overPoses[i], alpha, &_poses[i]);                }            }        }    }     processOutputJoints(triggersOut);     return _poses;} template <typename Func>void for_each_child_joint(AnimSkeleton::ConstPointer skeleton, int startJoint, Func f) {    std::queue<int> q;    q.push(startJoint);    while(q.size() > 0) {        int jointIndex = q.front();        for (int i = 0; i < skeleton->getNumJoints(); i++) {            if (jointIndex == skeleton->getParentIndex(i)) {                f(i);                q.push(i);            }        }        q.pop();    }} void AnimOverlay::buildFullBodyBoneSet() {    assert(_skeleton);    _boneSetVec.resize(_skeleton->getNumJoints());    for (int i = 0; i < _skeleton->getNumJoints(); i++) {        _boneSetVec[i] = 1.0f;    }} void AnimOverlay::buildUpperBodyBoneSet() {    assert(_skeleton);    buildEmptyBoneSet();    int spineJoint = _skeleton->nameToJointIndex("Spine");    for_each_child_joint(_skeleton, spineJoint, [&](int i) {        _boneSetVec[i] = 1.0f;    });} void AnimOverlay::buildLowerBodyBoneSet() {    assert(_skeleton);    buildFullBodyBoneSet();    int hipsJoint = _skeleton->nameToJointIndex("Hips");    int spineJoint = _skeleton->nameToJointIndex("Spine");    _boneSetVec.resize(_skeleton->getNumJoints());    for_each_child_joint(_skeleton, spineJoint, [&](int i) {        _boneSetVec[i] = 0.0f;    });    _boneSetVec[hipsJoint] = 0.0f;} void AnimOverlay::buildLeftArmBoneSet() {    assert(_skeleton);    buildEmptyBoneSet();    int leftShoulderJoint = _skeleton->nameToJointIndex("LeftShoulder");    for_each_child_joint(_skeleton, leftShoulderJoint, [&](int i) {        _boneSetVec[i] = 1.0f;    });} void AnimOverlay::buildRightArmBoneSet() {    assert(_skeleton);    buildEmptyBoneSet();    int rightShoulderJoint = _skeleton->nameToJointIndex("RightShoulder");    for_each_child_joint(_skeleton, rightShoulderJoint, [&](int i) {        _boneSetVec[i] = 1.0f;    });} void AnimOverlay::buildAboveTheHeadBoneSet() {    assert(_skeleton);    buildEmptyBoneSet();    int headJoint = _skeleton->nameToJointIndex("Head");    for_each_child_joint(_skeleton, headJoint, [&](int i) {        _boneSetVec[i] = 1.0f;    });} void AnimOverlay::buildBelowTheHeadBoneSet() {    assert(_skeleton);    buildFullBodyBoneSet();    int headJoint = _skeleton->nameToJointIndex("Head");    for_each_child_joint(_skeleton, headJoint, [&](int i) {        _boneSetVec[i] = 0.0f;    });} void AnimOverlay::buildHeadOnlyBoneSet() {    assert(_skeleton);    buildEmptyBoneSet();    int headJoint = _skeleton->nameToJointIndex("Head");    _boneSetVec[headJoint] = 1.0f;} void AnimOverlay::buildSpineOnlyBoneSet() {    assert(_skeleton);    buildEmptyBoneSet();    int spineJoint = _skeleton->nameToJointIndex("Spine");    _boneSetVec[spineJoint] = 1.0f;} void AnimOverlay::buildEmptyBoneSet() {    assert(_skeleton);    _boneSetVec.resize(_skeleton->getNumJoints());    for (int i = 0; i < _skeleton->getNumJoints(); i++) {        _boneSetVec[i] = 0.0f;    }} void AnimOverlay::buildLeftHandBoneSet() {    assert(_skeleton);    buildEmptyBoneSet();    int handJoint = _skeleton->nameToJointIndex("LeftHand");    for_each_child_joint(_skeleton, handJoint, [&](int i) {        _boneSetVec[i] = 1.0f;    });} void AnimOverlay::buildRightHandBoneSet() {    assert(_skeleton);    buildEmptyBoneSet();    int handJoint = _skeleton->nameToJointIndex("RightHand");    for_each_child_joint(_skeleton, handJoint, [&](int i) {        _boneSetVec[i] = 1.0f;    });} void AnimOverlay::buildHipsOnlyBoneSet() {    assert(_skeleton);    buildEmptyBoneSet();    int hipsJoint = _skeleton->nameToJointIndex("Hips");    _boneSetVec[hipsJoint] = 1.0f;} void AnimOverlay::buildBothFeetBoneSet() {    assert(_skeleton);    buildEmptyBoneSet();    int rightFoot = _skeleton->nameToJointIndex("RightFoot");    for_each_child_joint(_skeleton, rightFoot, [&](int i) {        _boneSetVec[i] = 1.0f;    });    int leftFoot = _skeleton->nameToJointIndex("LeftFoot");    for_each_child_joint(_skeleton, leftFoot, [&](int i) {        _boneSetVec[i] = 1.0f;    });}  // for AnimDebugDraw renderingconst AnimPoseVec& AnimOverlay::getPosesInternal() const {    return _poses;} void AnimOverlay::setSkeletonInternal(AnimSkeleton::ConstPointer skeleton) {    AnimNode::setSkeletonInternal(skeleton);     // we have to re-build the bone set when the skeleton changes.    buildBoneSet(_boneSet);}