IK算法之Fabrik

简介

IK（反向运动，Inverse Kinematics）是计算运动关节末端(如机械臂臂爪或人物骨架手臂末端的手掌)相对于关节的起始位置和方向到达所需位置的关节参数的数学过程。 –百度百科

Fabrik 算法通过计算骨骼节点的空间坐标关系来实现 IK 的效果。

原理

概括

Fabrik 算法一次迭代执行两个方向的遍历，首先从后往前计算到达目标点情况下所有骨骼的空间位置变化（不包括根节点），然后从前往后计算从根节点出发到达目标节点的骨骼节点位置。

从后往前

首先我们把最后一个骨骼节点置于目标位置，然后从后向前遍历骨骼节点，前一个节点的位置由后一个节点指向前一个节点的方向向量的单位向量乘以骨骼长度加上后一个节点当前的位置得到，即

$\vec{dir}*length+P_{back}$

$\vec{dir}=normalized(P_{forward}-P_{back})$

经过一轮遍历之后我们就可以得到除了根节点外的所有节点到达目标点的位置。

从前往后

然后我们从根节点开始从前向后遍历骨骼节点，后一个节点的位置由前一个节点指向后一个节点的方向向量的单位向量乘以骨骼长度加上前一个节点当前的位置得到，即

$\vec{dir}*length+P_{forward}$

$\vec{dir}=normalized(P_{back}-P_{forward})$

和从后往前的算法相反。

经过一轮迭代后，我们就可以得到一个相对正确的位置了。

约束

因为 Fabrik 是基于骨骼节点位置的，因此会出现关节往奇怪方向弯曲的问题。所以为了解决这个问题就要对关节进行约束。

我使用向量叉乘来判断当前关节前后两个骨骼的方向是否符合约定的左右关系，不符合的话就旋转方向向量到我们需要的方向。

因为我初次接触 IK 算法，因此约束相关的内容还没有好的方案，这部分内容就请自行调整。

代码

IKData

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class IKData
{
    public Vector3 Pos { get; set; }
    public float Length { get; set; }
    public Transform Node { get; set; }
}

IKRootScript

using System;
using System.Collections.Generic;
using UnityEngine;

public class IKRootScript : MonoBehaviour
{
    public List<IKData> _ikList = new List<IKData>();
    public Transform _leaf;
    public Vector3 _lastPos;
    public List<GameObject> _boneLine = new List<GameObject>();

    void Start()
    {
        InitBone(transform);
        InitLength();
        _leaf = GameObject.Find("CtrlPoint").transform;
        _lastPos = _leaf.position;
        Debug.Log("骨骼数量" + _ikList.Count);
        CreateBoneLine();
    }

    void LateUpdate()
    {
        //控制点发生变化的时候计算IK
        if (_lastPos != _leaf.position)
        {
            //IK迭代
            for (int i = 0; i < 3; i++)
            {
                IKBack(_leaf.position);
                IKForward();
            }
            _lastPos = _leaf.position;
            UpdateBone();
        }
    }
    /// <summary>
    /// 初始化骨骼节点
    /// </summary>
    /// <param name="trans"></param>
    private void InitBone(Transform trans)
    {
        IKData bone = new IKData();
        bone.Pos = trans.position;
        bone.Node = trans;
        _ikList.Add(bone);

        if (trans.childCount > 0)
        {
            Transform child = trans.GetChild(0);
            InitBone(child);
        }
    }
    /// <summary>
    /// 初始化骨骼长度
    /// </summary>
    private void InitLength()
    {
        for (int i = 0, len = _ikList.Count - 1; i < len; i++)
        {
            IKData first = _ikList[i];
            IKData second = _ikList[i + 1];
            first.Length = Math.Abs((second.Pos - first.Pos).magnitude);
        }
    }
    /// <summary>
    /// 创建骨架
    /// </summary>
    private void CreateBoneLine()
    {
        for (int i = 0, len = _ikList.Count - 1; i < len; i++)
        {
            IKData parent = _ikList[i];
            IKData child = _ikList[i + 1];
            Vector3 center = (parent.Pos + child.Pos) * 0.5f;
            GameObject cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
            cube.transform.position = center;
            cube.transform.localScale = new Vector3(0.2f, 0.2f, parent.Length);
            cube.transform.LookAt(child.Pos);
            _boneLine.Add(cube);
        }
    }
    /// <summary>
    /// 更新骨骼位置
    /// </summary>
    private void UpdateBone()
    {
        for (int i = 0, len = _ikList.Count; i < len; i++)
        {
            //更新关节位置
            IKData parent = _ikList[i];
            parent.Node.position = parent.Pos;
            //更新骨架位置
            if (i < len - 1)
            {
                IKData child = _ikList[i + 1];
                GameObject cube = _boneLine[i];
                Vector3 center = (parent.Pos + child.Pos) * 0.5f;
                cube.transform.position = center;
                cube.transform.LookAt(child.Pos);
            }
        }
    }

    /// <summary>
    /// 反向迭代
    /// </summary>
    /// <param name="target"></param>
    private void IKBack(Vector3 target)
    {
        //末节点位置置为target
        _ikList[_ikList.Count - 1].Pos = target;
        //遍历迭代
        for (int i = _ikList.Count - 1; i > 1; i--)
        {
            IKData parent = _ikList[i - 1];
            IKData child = _ikList[i];

            Vector3 nextBone = (parent.Pos - child.Pos).normalized;
            Vector3 lastBone = (parent.Pos - _ikList[i - 2].Pos).normalized;
            Vector3 side = Vector3.Cross(nextBone, lastBone);
            Debug.Log(i + ":" + side);


            Vector3 normal = (parent.Pos - child.Pos).normalized;

            if (side.x <= 0 || side.y <= 0 || side.z <= 0)
            {
                //按照权重和旋转方向调整方向向量
                normal = Quaternion.Euler(new Vector3(2f, 0, 0)) * normal;
            }

            parent.Pos = child.Pos + normal * parent.Length;
            Debug.Log(i + ":" + normal);
        }
    }

    /// <summary>
    /// 正向迭代
    /// </summary>
    private void IKForward()
    {
        //遍历迭代
        for (int i = 0, len = _ikList.Count - 1; i < len; i++)
        {
            IKData parent = _ikList[i];
            IKData child = _ikList[i + 1];
            Vector3 normal = (child.Pos - parent.Pos).normalized;
            child.Pos = parent.Pos + normal * parent.Length;
        }
    }
}