承接前文骨骼动画理论及程序部分实现（一）前向动力学。

简介

  如果说前向动力学是找到父骨骼的空间，来变换得到子骨骼空间，那么通过子骨骼得到父骨骼的位置，就称之为反向动力学，也称为逆向动力学。
  常见应用于机器人，或者即时演算游戏中，人物腿部在凹凸不平地面中的表现。诸如刺客信条等拥有丰富攀爬动画系统的游戏对IK会有更多的应用。

IK解算

基本原理

  对于只有两个骨骼关节点，其中一点固定，那么剩下一点的位置也随之确定：

黑X是目标点   这样末端点无法到达目标，但方向总是能确定的。
  对于有三个关节点的问题，可以用余弦定理解决   转换成三角形问题，已知三角形三边长度求夹角，余弦定理已经给出了公式：
  常见的IK骨，例如腿，用这个就能解决，不过有时候给头发或者蜈蚣、触手、机械臂等物体加IK时，IK链会很长，余弦定理就无法很好的解决了。
  对于更通用的IK解算方法，常用的有循环坐标下降法（Cyclic Coordinate Descent, 简称CCD）和雅克比矩阵法(Jacobian Matrix)。
  针对于MikuMikudance的模型文件存储参数，很容易发现它是用CCD来解决IK解算问题的，因此我们也用CCD来进行IK解算。

循环坐标下降法

名词

  首先我们确定一些名词。

• IK链（Link\Chain）：IK解算的单位，受IK效应器影响的一系列父子骨骼。
• 开始节点（Start Joint）：IK链固定的那一端节点。
• 末端效应器（End Effector）：IK链的末端节点，也是尽量要到达目标的节点。
• IK目标（Target）：希望末端效应器到达的目标点。

  注意：这里我们约定，IK链条从接近末端开始计数，如下图。
  现在比照MMD模型对应上面的名词：

  最下面脚踝处选中变红的骨骼便是左足IK，同时和它相叠加在一起（位置一样）的还有左足首。
  其中IK链包含左膝（左ひざ）、开始节点左足（左腿跟），以及末端效应器。
  注意末端效应器在MMD中其实是上面标着Target的左足首，而真正的IK目标其实是左足IK本身。
  如果看骨骼亲子继承关系，左足>左ひざ>左足首才是一条链的，而真·IK目标的继承关系是全ての親>左足IK親>左足IK，这意为着左足IK直属于全亲骨，只随着全亲骨的变化而变化，其位置不会轻易改变。

算法

  从IK链靠近末端效应器的第一个节点开始，算当前节点到末端效应器的向量与当前节点到IK目标向量的夹角，然后旋转之；一直算到开始节点，然后再从第一个节点开始，不断循环。

  如上图，先像红色线条那样算角度，然后让末端效应器落在当前节点和IK目标的向量（下一张图的黑色线）之上。（由于我作图时是目测旋转，后两张图没有准确落到黑色线条张，这个别在意）
  然后没有然后了，原理就这么简单，下面开始放部分程序实现。

程序实现

  首先，因为不断要更新骨骼的旋转，之前前向动力学的BNode不够用，我们此时不光是要记录生成骨骼空间变换本身，同时要计算它自身的旋转：

struct BNode {
    BNode(PMX::Bone& _bone) : bone(_bone) {};
    int32_t index;
    PMX::Bone& bone;
    BNode* parent = nullptr;
    std::vector<BNode*> childs;

    bool haveAppendParent = false;
    BNode* appendParent = nullptr;
    float appendWeight;
    std::vector<BNode*> appendChilds;
        
    //弃用Mconv矩阵，而是每次用getLocalMat生成
    glm::vec3 position;
    glm::quat rotate;
    //记录骨骼本身经历的移动和旋转
    glm::vec3 translate;
    glm::quat quaternion;

    inline glm::mat4 getLocalMat() {//conv before local to after world 变换初始状态骨骼空间坐标到完成位置的世界坐标
        return glm::translate(glm::mat4(1), position) * glm::mat4_cast(rotate);
    }
    inline glm::mat4 getGlobalMat() {//conv before world to after world 变换初始状态世界坐标到完成位置的世界坐标
        return glm::translate(glm::mat4(1), position) * glm::mat4_cast(rotate) * glm::translate(glm::mat4(1), -bone.position);
    }

    //更新rotate 和 position ，使用前确保所有父骨骼都是最新更新的
    //只管自己，不管子骨和赋予亲
    bool updateSelfData() {
        glm::mat4 parentLocalMat(1);

        glm::vec3 parentPosition(0);
        glm::quat parentRotate(1, 0, 0, 0);

        glm::vec3 appendParentTranslate(0);
        glm::quat appendParentQuaternion(1, 0, 0, 0);
        if (parent != nullptr) {
            parentPosition = parent->bone.position;
            parentRotate = parent->rotate;

            parentLocalMat = parent->getLocalMat();
        }
        if (appendParent != nullptr) {
            appendParentTranslate = appendParent->translate;
            appendParentQuaternion = appendParent->quaternion;
        }

        position = parentLocalMat * glm::vec4(bone.position - parentPosition + translate + appendParentTranslate * appendWeight, 1);
        rotate = parentRotate * quaternion * glm::quat(glm::eulerAngles(appendParentQuaternion) * appendWeight);
        return true;
    }

    //更新自身和所有子骨和赋予亲，使用前确保父骨都是更新过的
    //更新规则：先更新自身，然后更新自身的赋予亲本身和赋予亲的直属子骨，再更新自身子骨
    bool updateSelfAndChildData() {
        std::stack<Animation::BNode*> nodes;
        nodes.push(this);
        while (!nodes.empty()) {
            Animation::BNode* curr = nodes.top();
            nodes.pop();

            curr->updateSelfData();//更新自身

            for (Animation::BNode* appendChild : curr->appendChilds) {//更新赋予亲
                std::stack<Animation::BNode*> appendChildNodes;
                appendChildNodes.push(appendChild);
                while (!appendChildNodes.empty()) {
                    Animation::BNode* appendChildCurr = appendChildNodes.top();
                    appendChildNodes.pop();

                    appendChildCurr->updateSelfData();
                    for (auto child = appendChildCurr->childs.rbegin(); child != appendChildCurr->childs.rend(); ++child) {
                        appendChildNodes.push(*child);
                    }
                }
            }

            for (auto child = curr->childs.rbegin(); child != curr->childs.rend(); ++child) {//所有孩子压栈
                nodes.push(*child);
            }
        }
        return true;
    }
}

  吐个槽，之前没有存translate和quaternion，每次都用亲骨反向计算，后来还有顾及赋予亲的影响。更新也是将当前骨骼到所有子骨、赋予子骨都更新一遍，为了保证更新的子骨用的是亲骨更新前的变换矩阵，还搞了个双栈，分别前序和后续遍历，结果输出程序结果不正确，我也要被繁杂的逻辑搞炸了。完全搞不清到底是IK解算的问题，还是更新骨骼的问题。
  然后多存了那两个变量，更新骨骼简化为只更新自身，程序就变得特别清爽和可控；说多了都是泪。
  关于updateSelfAndChildData的更新规则也是根据情况来的。我发现赋予亲和赋予子骨的父骨一般是一样的，大致情况如下：

红色是骨骼继承，蓝色是赋予继承 ，所以一开始的继承规则是：先更新自身，然后更新赋予子骨（同级的蓝色继承），但不会向下看赋予子骨的子骨和赋予子骨，再深度更新子骨。
  结果个别骨骼就特殊了，它只继承被赋予骨，不继承普通骨骼，这样的更新规则更新不到它，于是我只能设定更新赋予骨的同时，向下更新一级直接子骨。
  虽说这样很依赖模型本身，但也是没办法的事情，假如两个骨骼互相认爹（反向寝室关系），那一开始个骨骼树解析也是没完没了的。建模不规范，程序两行泪啊。
  前向动力学部分遍历骨骼树，要记录一开始的translate和quaternion这个没有技术含量，代码我就不放了（别忘了赋予亲就行），接下来是重点的IK解算部分。
  我将IK解算器封装为一个类，在此之前先定义一个内部结构IKChain(这个命名可能有问题，应该叫IKJoint更准确)

struct IKChain {
    Animation::BNode* node;
    bool enableAxisLimit;

    glm::vec3 min;
    glm::vec3 max;

    IKChain* pre = nullptr;//前一条链节
    Animation::BNode* endPre = nullptr;//末端效应器没有链节的属性，额外存储

    bool updateChain() {//更新IK链
        IKChain* curr = this;
        do {
            curr->node->updateSelfData();//更新自身
            if (curr->pre == nullptr) {//如果是第一个链节，就更新末端效应器
                curr->endPre->updateSelfData();
            }
            curr = curr->pre;
        } while (curr != nullptr);
                
        return true;
    }
};

  注意：这是更新顺序，不是解算顺序，可以理解为：第一次解算更新前一个关节就行，第二次要更新前两个……

class IKSolve
{
public:
    IKSolve(BNode* _ikNode, BoneManager& boneManager);//初始化
    ~IKSolve();

    void Solve();//解算
private:
    Animation::BNode* ikNode;
    std::vector<IKChain> ikChains;
    Animation::BNode* targetNode;

    float limitAngle;
};

IKSolve::IKSolve(Animation::BNode* ikNode, Animation::BoneManager& boneManager)
{
    assert(ikNode->bone.isIK());//确保是IK骨

    targetNode = boneManager.linearList[ikNode->bone.ikTargetBoneIndex];//按照MMD命名为Target，其实是末端效应器
    limitAngle = ikNode->bone.ikLimit;//单位角，不让一次旋转角度过大的限制
    this->ikNode = ikNode;//IK目标节点

    ikChains.resize(ikNode->bone.ikLinks.size());
    for (int i = 0; i < ikChains.size(); ++i) {
        ikChains[i].node = boneManager.linearList[ikNode->bone.ikLinks[i].boneIndex];
        ikChains[i].enableAxisLimit = ikNode->bone.ikLinks[i].enableLimit();
        ikChains[i].min = ikNode->bone.ikLinks[i].min;
        ikChains[i].max = ikNode->bone.ikLinks[i].max;

        if (i != 0) {
            ikChains[i].pre = &ikChains[i - 1];
        }
    }
    ikChains[0].endPre = targetNode;
}

  接下来是重头戏：

void IKSolve::Solve() {
    for (int ite = 0; ite < ikNode->bone.ikIterationCount; ++ite) {

        for (size_t chainIdx = 0; chainIdx < ikChains.size(); ++chainIdx) {
            IKChain& chain = ikChains[chainIdx];

            glm::mat4 worldToLocalMat = glm::inverse(chain.node->getLocalMat());
            glm::vec3 jointLocalIK = worldToLocalMat * glm::vec4(ikNode->position, 1); //关节本地坐标系下IK目标位置
            glm::vec3 jointLocalTarget = worldToLocalMat * glm::vec4(targetNode->position, 1); //关节本地坐标系下末端效应器位置

            if (glm::distance(jointLocalIK, jointLocalTarget) < 1e-3) {//两个向量差距太小，目的达到，直接退出解算
                ite = ikNode->bone.ikIterationCount;
                break;
            }

            float cos_deltaAngle = glm::dot(glm::normalize(jointLocalIK), glm::normalize(jointLocalTarget));
            if (cos_deltaAngle > 1 - 1e-6) {//夹角太小pass
                continue;
            }
            float deltaAngle = glm::acos(cos_deltaAngle);
            deltaAngle = glm::clamp(deltaAngle, -limitAngle, limitAngle);//一次旋转的度数不得超过限制角

                
            glm::vec3 axis = glm::normalize(glm::cross(glm::normalize(jointLocalTarget), glm::normalize(jointLocalIK)));//旋转轴
            chain.node->quaternion *= glm::quat_cast(glm::rotate(glm::mat4(1), deltaAngle, axis));
            chain.updateChain();
        }
    }

    ikChains.end()[-1].node->updateSelfAndChildData();//从IK链末端（开始节点）更新所有子骨
}

  第一重循环的循环次数模型自己会给出，第二重循环是对每个关节链进行对比、旋转。
  关于那个逆矩阵是这样的：我们上次推导了矩阵变换，其中是点所处骨骼空间的坐标，是点所处世界空间的坐标，左右两边的左侧都乘上的逆矩阵得到新的等式：，也就是可以输入世界坐标，得到骨骼关节空间的坐标，这样旋转的角度能保证是基于骨骼关节坐标系的。
  两个向量、角度检测不得不做，当夹角太小时，cos值接近1，求arccos可能会出现nan。
  是不是很简单？（才怪，这程序写的头都秃了）把IK解算放到FK之后（上一章POSE的构造函数最后）：

for (Animation::BNode* node : boneManager.linearList) {
    if (node->bone.isIK()) {
        Animation::IKSolve solve(node, boneManager);
        solve.Solve();
    }
}

  看看现在的成果：

  左侧由刚刚理论写成的程序生成，末端效应器看来是落在目标点上了，不过小姐姐你的左腿是不是有点不对劲啊，赶紧去砍医生吧。
  才怪了，单单是我们上面那个三节点模型，用余弦定理，在三维空间中也能解除无数个解（第二个关节点在空间中一个圆边上都可以），类似腿这样的关节点都是有限制的，膝盖关节只能在自己关节点空间的X轴旋转，并且只能向内弯折。
  看其他人的程序，以前的PMD模型文件可能给骨骼一个标记，告诉程序：这个骨骼是膝盖（isLeg），也有部分程序直接把左右膝盖的shift-jis编码值写死在程序里，特殊处理这些关节。
  不过PMX文件的IK链中，给了每个链节的限制范围（在最上面的图能看到，注意下上面记录的都是左手坐标系的范围），由此，我们对这样的关节特殊处理：

if (chain.enableAxisLimit) {//如果这个关节点有轴限制
    glm::mat4 inv = glm::inverse(chain.node->parent->getLocalMat());
    glm::vec3 selfRotate = glm::eulerAngles(chain.node->quaternion);//本身基于父空间的旋转的欧拉角表示

    if (ite == 0) {//第一次迭代，直接到旋转到可容忍区间的一半
        glm::vec3 targetAngles = (chain.min + chain.max) / 2.0f;//目标旋转
        chain.node->quaternion = glm::quat(targetAngles);//令关节和全部子骨旋转
        chain.updateChain();
    }
    else {//不是第一次迭代，也要保证旋转在区间内
        glm::vec3 axis = glm::normalize(glm::cross(glm::normalize(jointLocalTarget), glm::normalize(jointLocalIK)));
        glm::vec3 deltaRotate = glm::eulerAngles(glm::quat_cast(glm::rotate(glm::mat4(1), deltaAngle, axis)));
        deltaRotate = glm::clamp(deltaRotate, chain.min - selfRotate, chain.max - selfRotate);
        chain.node->quaternion *= glm::quat(deltaRotate);
        chain.updateChain();
    }
    continue;
}
else {//没有轴限制，程序和上边一样
    glm::vec3 axis = glm::normalize(glm::cross(glm::normalize(jointLocalTarget), glm::normalize(jointLocalIK)));
    chain.node->quaternion *= glm::quat_cast(glm::rotate(glm::mat4(1), deltaAngle, axis));
    chain.updateChain();
}

  第一次迭代中，直接将关节旋转到允许空间的一半，如膝盖的限制是[(0~180), (0, 0), (0,0)]，那么就直接旋转到[90, 0, 0](右手坐标系的)。随后其他迭代中，保证限制关节不出允许区间，例如本身是[30, 0, 0]，那么它只能旋转范围为：[(-30, 150), (0, 0), (0, 0)]。

  这样，我们就得到了正确的IK解算：

其他

  IK解算参考了很多github上现有的代码，但还是写的头快秃了，赋予亲中间还过来捣乱，简直……给你们看看我中间调试时出现得到N种奇葩状态：

  模型来自女仆丽塔-洛丝薇瑟2.0-神帝宇，希望不要打我（滑稽。
  如果还是有些看不懂，可以看看以下的网站：

引用

循环坐标下降（CCD）算法中对骨骼动画中膝盖等关节的特殊处理
挺久前一个先辈的文章，也是搞MMD的
saba-OpenGL Viewer (OBJ PMD PMX)
从我未接触图形学时就看上了这个github项目，现在越写越心惊，骨骼动画、表情动画、物理都有，以后也要继续借鉴这里的代码
MikuMikuDance PMX/VMD Viewer for Windows, OSX, and Linux
上面的saba项目太庞大，中间封装了很多层，疑惑的时候看看轻量些的代码也是个不错的选择
MMDAgent
也是个不错的借鉴