FABRIK в Unity: инверсная кинематика на C#

В этой статье мы с нуля напишем на C# солвер инверсной кинематики FABRIK для Unity: цепочка костей любой длины тянется к цели, локоть или колено сгибается в нужную сторону, суставы не выворачиваются, а ноги персонажа встают на ступеньки и склоны. В конце разберём, когда своё решение оправдано, а когда проще взять Chain IK из пакета Animation Rigging или встроенный IK гуманоидного Animator.

Зачем нужна инверсная кинематика

Обычная анимация — это прямая кинематика: аниматор задаёт углы суставов, а положение кисти или стопы получается как следствие. Плечо повернули на 30 градусов, локоть на 45 — кисть оказалась где-то. В игре часто нужно наоборот: известно, где должна быть кисть (на ручке двери, на прикладе, на перилах), и нужно подобрать углы суставов, чтобы она туда попала. Это и есть инверсная кинематика, IK.

Типичные задачи:

ноги персонажа на неровной поверхности: без IK на лестнице одна стопа висит в воздухе, а другая проваливается в ступеньку;
руки на оружии или руле, которые должны оставаться на месте при любой анимации корпуса;
голова и позвоночник, следящие за целью;
щупальца, хвосты, роботизированные манипуляторы, процедурные пауки;
верёвки и цепи, которые цепляются концом за точку.

Для двух костей (плечо и предплечье, бедро и голень) задача решается аналитически по теореме косинусов. Для трёх и более костей точного решения в общем виде нет, и в ход идут итеративные методы. FABRIK — один из самых простых и быстрых среди них.

Как работает FABRIK

FABRIK расшифровывается как Forward And Backward Reaching Inverse Kinematics. Алгоритм опубликовали Андреас Аристиду и Джоан Ласенби в 2011 году. Его главная идея: не работать с углами вообще. Цепочка представлена как набор точек-суставов и длин костей между ними, а решатель двигает точки, сохраняя расстояния.

Пусть у нас суставы p0…pn, где p0 — корень (плечо), pn — кончик (кисть), и длины костей d0…dn-1. Одна итерация состоит из двух проходов.

Проход от кончика к корню. Ставим кончик ровно в цель. Затем для каждого предыдущего сустава смотрим на направление от уже сдвинутого соседа к нему и ставим его на расстоянии длины кости по этому направлению. Цепочка «подтягивается» к цели, но корень при этом уезжает со своего места.
Проход от корня к кончику. Возвращаем корень на исходную позицию и делаем то же самое в обратном порядке: каждый следующий сустав ставим на расстоянии длины кости от предыдущего по направлению к его текущему положению.

После пары проходов кончик оказывается близко к цели, а корень — на месте. Повторяем, пока расстояние от кончика до цели не станет меньше допуска или не кончится лимит итераций. На практике цепочке из 3–5 костей хватает 3–10 итераций.

В оригинальной статье первый проход (от кончика) называется forward reaching, а второй (от корня) — backward reaching. В некоторых реализациях названия перепутаны, поэтому ориентируйтесь на смысл: сначала тянем кончик к цели, потом возвращаем корень.

Отдельно обрабатывается случай, когда цель дальше, чем суммарная длина цепочки. Тогда итерировать бессмысленно: цепочку просто вытягивают в прямую линию от корня в сторону цели.

Почему FABRIK так популярен в играх:

нет матриц, якобианов и тригонометрии в основном цикле — только нормализация векторов;
быстро сходится и не дёргается так, как часто дёргается CCD (Cyclic Coordinate Descent);
поза получается естественной: изгиб распределяется по всей цепочке, а не скапливается в последнем суставе;
легко расширяется ограничениями и несколькими концевыми эффекторами.

Солвер на голых точках

Начнём с чистой функции, которая ничего не знает о Transform и работает с массивом позиций. Её удобно тестировать, а позже ту же логику можно перенести в Burst-джоб.

using UnityEngine;

public static class Fabrik
{
    // p — позиции суставов от корня (0) до кончика (n - 1),
    // lengths[i] — длина кости между p[i] и p[i + 1].
    // Возвращает true, если кончик дотянулся до цели с заданным допуском.
    public static bool Solve(Vector3[] p, float[] lengths, Vector3 target,
                             int maxIterations, float tolerance)
    {
        int n = p.Length;
        Vector3 root = p[0];

        float total = 0f;
        for (int i = 0; i < n - 1; i++)
            total += lengths[i];

        // Цель недостижима: вытягиваем цепочку в прямую.
        if ((target - root).sqrMagnitude >= total * total)
        {
            Vector3 dir = (target - root).normalized;
            for (int i = 1; i < n; i++)
                p[i] = p[i - 1] + dir * lengths[i - 1];
            return false;
        }

        float tolSq = tolerance * tolerance;

        for (int iter = 0; iter < maxIterations; iter++)
        {
            // Проход от кончика к корню.
            p[n - 1] = target;
            for (int i = n - 2; i >= 0; i--)
                p[i] = p[i + 1] + SafeDir(p[i] - p[i + 1]) * lengths[i];

            // Проход от корня к кончику.
            p[0] = root;
            for (int i = 1; i < n; i++)
                p[i] = p[i - 1] + SafeDir(p[i] - p[i - 1]) * lengths[i - 1];

            if ((p[n - 1] - target).sqrMagnitude < tolSq)
                return true;
        }

        return false;
    }

    static Vector3 SafeDir(Vector3 v)
    {
        float sq = v.sqrMagnitude;
        // Два сустава в одной точке: направление не определено.
        return sq > 1e-12f ? v / Mathf.Sqrt(sq) : Vector3.up;
    }
}

Пара деталей, на которых часто спотыкаются:

SafeDir нужен потому, что Vector3.normalized у нулевого вектора возвращает ноль, и сустав «схлопнется» в соседа, а длина кости потеряется навсегда. Такое случается, если цепочка изначально сложена вдвое или цель совпала с одним из суставов.
Сравниваем квадраты расстояний, чтобы не считать корень без нужды.
Проверку сходимости делаем после полного цикла, иначе корень может остаться сдвинутым.

Из точек в повороты костей

Солвер выдаёт позиции, а скелету нужны повороты: кости в Unity — это иерархия Transform, и если просто записать position каждой кости, меш скиннинга порвётся, а дочерние объекты разъедутся. Правильный путь — повернуть каждую кость так, чтобы направление на её дочерний сустав совпало с новым направлением из солвера.

Удобный приём — идти от корня к кончику и на каждом шаге считать «откуда» по текущему положению дочернего сустава. Поскольку родитель уже повёрнут, дочерние кости переехали вместе с ним, и разница остаётся только локальной.

using UnityEngine;

[DefaultExecutionOrder(100)]
public class FabrikChain : MonoBehaviour
{
    [SerializeField] Transform[] bones;      // от корня к кончику, минимум 3
    [SerializeField] Transform target;
    [SerializeField] Transform pole;         // необязательно
    [SerializeField, Range(1, 30)] int iterations = 10;
    [SerializeField] float tolerance = 0.001f;
    [SerializeField, Range(0f, 1f)] float weight = 1f;
    [SerializeField] bool matchTargetRotation = true;

    Vector3[] positions;
    float[] lengths;

    void Awake()
    {
        positions = new Vector3[bones.Length];
        lengths = new float[bones.Length - 1];
    }

    void LateUpdate()
    {
        if (target == null || weight <= 0f)
            return;

        // Читаем позу после Animator: так IK работает поверх анимации.
        for (int i = 0; i < bones.Length; i++)
            positions[i] = bones[i].position;
        for (int i = 0; i < lengths.Length; i++)
            lengths[i] = Vector3.Distance(positions[i], positions[i + 1]);

        Fabrik.Solve(positions, lengths, target.position, iterations, tolerance);

        if (pole != null)
            FabrikPole.Apply(positions, pole.position);

        for (int i = 0; i < bones.Length - 1; i++)
        {
            Vector3 from = bones[i + 1].position - bones[i].position;
            Vector3 to = positions[i + 1] - positions[i];
            Quaternion solved = Quaternion.FromToRotation(from, to) * bones[i].rotation;
            bones[i].rotation = Quaternion.Slerp(bones[i].rotation, solved, weight);
        }

        if (matchTargetRotation)
        {
            Transform tip = bones[bones.Length - 1];
            tip.rotation = Quaternion.Slerp(tip.rotation, target.rotation, weight);
        }
    }
}

Почему длины костей пересчитываются каждый кадр, а не один раз в Awake: если анимация масштабирует кости (сквош-стретч, разные персонажи на одном скелете), закешированные длины разойдутся с реальными. Расчёт дешёвый — несколько вычитаний и корней.

Quaternion.FromToRotation даёт минимальный поворот, переводящий один вектор в другой. Он не добавляет скручивания вокруг оси кости, поэтому твист из анимации сохраняется. Это обычно именно то, что нужно: предплечье не начинает вращаться вокруг своей оси без причины.

Вес работает через Quaternion.Slerp между анимационным поворотом и решением. Плавно меняя weight от 0 до 1, можно включать IK, когда персонаж тянется к предмету, и выключать, когда отпустил. Резкое переключение с 0 на 1 даёт заметный рывок, поэтому вес почти всегда стоит анимировать во времени.

Pole target: куда сгибать локоть

У цепочки длиннее двух суставов бесконечно много решений: локоть можно поднять вверх, опустить вниз или отвести в сторону, и кисть останется на месте. FABRIK выберет решение, ближайшее к исходной позе, и при резких движениях цели колено может перескочить на другую сторону. Чтобы управлять плоскостью изгиба, используют pole target — точку, к которой «тянется» средний сустав.

Идея простая. Каждый внутренний сустав можно вращать вокруг оси, проходящей через его соседей, не меняя длин костей. Проецируем сустав и pole на плоскость, перпендикулярную этой оси, и поворачиваем сустав на угол между проекциями.

using UnityEngine;

public static class FabrikPole
{
    public static void Apply(Vector3[] p, Vector3 pole)
    {
        for (int i = 1; i < p.Length - 1; i++)
        {
            Vector3 a = p[i - 1];
            Vector3 axis = p[i + 1] - a;
            if (axis.sqrMagnitude < 1e-8f)
                continue;
            axis.Normalize();

            Plane plane = new Plane(axis, a);
            Vector3 jointOnPlane = plane.ClosestPointOnPlane(p[i]) - a;
            Vector3 poleOnPlane = plane.ClosestPointOnPlane(pole) - a;
            if (jointOnPlane.sqrMagnitude < 1e-8f || poleOnPlane.sqrMagnitude < 1e-8f)
                continue;

            float angle = Vector3.SignedAngle(jointOnPlane, poleOnPlane, axis);
            p[i] = a + Quaternion.AngleAxis(angle, axis) * (p[i] - a);
        }
    }
}

Проверки на малые векторы нужны для вырожденных случаев: когда цепочка вытянута в прямую (сустав лежит на оси, и поворачивать нечего) и когда pole лежит на самой оси.

Для ноги pole ставят впереди колена, для руки — позади и чуть ниже локтя. Pole делают дочерним объектом персонажа, чтобы он поворачивался вместе с корпусом. Если повесить его в мировых координатах, при развороте персонажа на 180 градусов колени согнутся назад.

Ограничения суставов

Без ограничений FABRIK честно найдёт любую позу, в которой кончик достаёт до цели, включая колено, вывернутое в обратную сторону. Для щупальца это не страшно, для человека — нет. Ограничения встраиваются прямо в проход от корня к кончику: прежде чем поставить сустав, мы корректируем направление кости.

Конус

Самое универсальное ограничение — конус: угол между костью и направлением родительской кости не больше заданного. Подходит для позвоночника, хвоста, шеи, плеча.

static Vector3 ClampToCone(Vector3 dir, Vector3 parentDir, float maxAngleDeg)
{
    if (Vector3.Angle(parentDir, dir) <= maxAngleDeg)
        return dir;

    // Поворачиваем направление родителя к желаемому, но не дальше границы конуса.
    return Vector3.RotateTowards(parentDir, dir, maxAngleDeg * Mathf.Deg2Rad, 0f).normalized;
}

Встраиваем в прямой проход солвера:

p[0] = root;
for (int i = 1; i < n; i++)
{
    Vector3 dir = SafeDir(p[i] - p[i - 1]);
    if (i >= 2)
    {
        Vector3 parentDir = SafeDir(p[i - 1] - p[i - 2]);
        dir = ClampToCone(dir, parentDir, maxAngles[i - 1]);
    }
    p[i] = p[i - 1] + dir * lengths[i - 1];
}

Для первой кости родительского направления внутри цепочки нет. Его можно взять из кости-родителя корня (например, ключицы для плеча) или не ограничивать вовсе.

Шарнир

Колено и локоть — шарниры: они гнутся в одной плоскости. Шарнирное ограничение проецирует направление кости на плоскость, перпендикулярную оси шарнира:

static Vector3 ClampToHinge(Vector3 dir, Vector3 hingeAxis)
{
    Vector3 projected = Vector3.ProjectOnPlane(dir, hingeAxis);
    return projected.sqrMagnitude > 1e-8f ? projected.normalized : dir;
}

Ось шарнира берут из текущего поворота кости до решения, например bones[i].TransformDirection(Vector3.right) — какая именно локальная ось нужна, зависит от того, как скелет ориентирован в DCC-пакете. Это приближение: в процессе решения кость поворачивается, и ось тоже должна бы поворачиваться. Для одного колена или локтя надёжнее обходиться pole target без шарнирных ограничений, а ограничения оставить для длинных цепочек.

Важный побочный эффект: с ограничениями FABRIK может не сойтись к цели, даже если она теоретически достижима. Это нормально — лучше кисть, не дотянувшаяся на пару сантиметров, чем вывернутая рука. Солвер вернёт false, и по этому сигналу можно, например, начать наклонять корпус.

Практика: ноги на неровной поверхности

Классическое применение IK — постановка стоп. Анимация ходьбы сделана для ровного пола, а уровень состоит из лестниц, камней и склонов. Задача: каждый кадр найти под каждой стопой поверхность и поставить стопу на неё, сохранив анимацию в целом.

using UnityEngine;

[DefaultExecutionOrder(50)]
public class FootPlacement : MonoBehaviour
{
    [SerializeField] Transform foot;          // кость стопы из анимации
    [SerializeField] Transform ikTarget;      // цель, которую читает FabrikChain
    [SerializeField] LayerMask groundMask;
    [SerializeField] float rayHeight = 0.5f;
    [SerializeField] float rayLength = 1.0f;
    [SerializeField] float footOffset = 0.08f; // от кости до подошвы
    [SerializeField] float smoothTime = 0.05f;

    Vector3 velocity;

    void LateUpdate()
    {
        Vector3 animated = foot.position;
        Vector3 origin = animated + Vector3.up * rayHeight;

        Vector3 desired = animated;
        Quaternion desiredRot = foot.rotation;

        if (Physics.Raycast(origin, Vector3.down, out RaycastHit hit,
                            rayHeight + rayLength, groundMask, QueryTriggerInteraction.Ignore))
        {
            // Поднимаем стопу только если она ниже поверхности,
            // чтобы не прижимать к земле ногу в фазе шага.
            float groundY = hit.point.y + footOffset;
            if (animated.y < groundY + 0.02f)
                desired.y = groundY;

            desiredRot = Quaternion.FromToRotation(Vector3.up, hit.normal) * foot.rotation;
        }

        ikTarget.position = Vector3.SmoothDamp(ikTarget.position, desired, ref velocity, smoothTime);
        ikTarget.rotation = Quaternion.Slerp(ikTarget.rotation, desiredRot, 1f - Mathf.Exp(-30f * Time.deltaTime));
    }
}

На что обратить внимание:

Порядок выполнения. Animator записывает позу между Update и LateUpdate. Поэтому и чтение позиции стопы, и IK должны идти в LateUpdate, причём постановка стоп — раньше солвера. Атрибут [DefaultExecutionOrder] задаёт порядок без ручной настройки в Project Settings: 50 у FootPlacement и 100 у FabrikChain.
Цель стопы не должна быть дочерней для кости ноги, иначе после поворота бедра она уедет вместе с ним и солвер будет гоняться за собственным хвостом. Делайте цели дочерними для корня персонажа или вообще отдельными объектами.
Таз. Если одна стопа стоит на ступеньке выше, а другая ниже уровня капсулы, нижняя нога не дотянется. Обычное решение — опустить таз на разницу между самой низкой целью и анимационной позицией стопы, а уже потом решать IK для ног. Смещение таза тоже сглаживают.
Сглаживание. Без SmoothDamp стопа будет прыгать при переходе через край ступеньки. Слишком большое время сглаживания даёт «плавающие» ноги, начните с 0,03–0,08 с.
Вес по кривой анимации. В фазе переноса ноги IK лучше ослаблять, а в фазе опоры — включать полностью. Для этого в клип добавляют кривую, читают её через Animator.GetFloat и передают в weight.

Производительность

Одна цепочка из четырёх костей с десятью итерациями — это несколько сотен векторных операций, микросекунды. Проблемы начинаются, когда персонажей сотни или цепочки длинные (процедурные существа, верёвки).

Никаких аллокаций в кадре. Массивы позиций и длин создаются один раз. Не используйте LINQ и не создавайте списки в LateUpdate.
Ранний выход. Если цель почти не сдвинулась с прошлого кадра и анимационная поза тоже, можно сразу начинать с прошлого решения: сходимость наступит за одну-две итерации.
Адаптивное число итераций. Для далёких персонажей хватит 2–3 итераций или IK можно отключить вовсе. Сравнивайте расстояние до камеры и снижайте качество так же, как LOD для мешей.
Burst и Job System. Функция Solve работает с массивами чисел и не трогает Unity API, поэтому её легко переписать на NativeArray<float3> и Unity.Mathematics, а цепочки всех персонажей решать в одном IJobParallelFor. Чтение и запись Transform переносят в IJobParallelForTransform или в анимационные джобы.
Профилирование. Оберните солвер в ProfilerMarker, чтобы видеть его стоимость отдельной строкой в профайлере, а не размазанной по LateUpdate.

FABRIK, CCD или аналитика

FABRIK — не единственный вариант, и для части задач он не лучший.

Аналитическое решение для двух костей (Two Bone IK) по теореме косинусов даёт точный ответ за один шаг без итераций. Для руки и ноги человека это стандарт: быстро, предсказуемо, плоскость изгиба задаётся pole. Если в цепочке ровно две кости, FABRIK использовать незачем.
CCD поворачивает суставы по одному, начиная с ближайшего к кончику, каждый раз наводя кончик на цель. Он работает напрямую с углами, поэтому в нём проще задать ограничения в локальных осях. Минусы — склонность сгибать в основном последние суставы и подёргивания при движении цели.
FABRIK хорош для цепочек из трёх и более костей, где изгиб должен распределяться равномерно: позвоночник, хвост, щупальце, многосуставная лапа насекомого, механическая рука.
Методы на основе якобиана точнее и гибче (несколько целей, приоритеты), но заметно дороже и сложнее. В играх их применяют редко, в основном в полноценных системах full-body IK.

Готовые решения в Unity

Прежде чем писать своё, стоит знать, что уже есть в движке.

IK гуманоидного Animator. Для моделей с аватаром типа Humanoid есть встроенный IK рук, ног и взгляда: в методе OnAnimatorIK вызываются Animator.SetIKPosition, SetIKRotation, SetIKPositionWeight с целями AvatarIKGoal.LeftFoot и другими, а также SetLookAtPosition. Метод вызывается только у слоёв, где включён IK Pass. Это самый дешёвый путь для людей, но он работает только с Humanoid и не умеет произвольные цепочки.
Пакет Animation Rigging (com.unity.animation.rigging) содержит набор констрейнтов, работающих через Animation Jobs: Two Bone IK, Chain IK, Multi-Aim, Damped Transform и другие. Chain IK основан как раз на FABRIK, но не поддерживает pole target и лучше всего работает, когда исходная поза уже близка к целевой. Если нужен pole или нестандартные ограничения, пакет позволяет написать собственный констрейнт на тех же джобах.
Пакет 2D Animation для 2D-скелетов содержит компоненты IK, в том числе FABRIK Solver 2D, и статический класс FABRIK2D с методом Solve, который возвращает true, если солвер уложился в лимит итераций.

Своя реализация оправдана, когда нужен контроль: pole для длинной цепочки, особые ограничения, процедурные существа без аниматора, тысячи цепочек в Burst, или просто проект без пакета Animation Rigging. Кода, как видно выше, немного, и он полностью под вашим контролем.

Частые ошибки

IK в Update. Animator перезапишет позу после Update, и результат IK пропадёт. Всё, что меняет кости поверх анимации, — в LateUpdate или в OnAnimatorIK.
Запись позиций вместо поворотов. Кость с изменённым position растягивает скин и ломает иерархию. Позиции из солвера — только промежуточный результат.
Неверный порядок костей. Массив должен идти от корня к кончику и быть непрерывной цепочкой родитель-ребёнок. Пропущенная промежуточная кость (например, твист-кость предплечья) даёт неправильные длины и странный изгиб.
Цель внутри цепочки. Если цель — дочерний объект одной из костей, каждое решение сдвигает цель, и система никогда не успокаивается.
Pole в мировых координатах. При повороте персонажа сгиб уходит не туда. Pole должен двигаться вместе с телом.
Резкое включение веса. Переход с 0 на 1 за кадр выглядит как телепорт руки. Меняйте вес через Mathf.MoveTowards или кривую анимации.
Нулевые векторы. Сложенная цепочка или совпадение цели с суставом без защиты в нормализации навсегда теряет длину кости.

Итог

FABRIK — алгоритм, который помещается в полсотни строк и решает большую часть игровых задач IK: два прохода по точкам, сохранение длин и проверка сходимости. Чтобы превратить его в рабочий инструмент, нужно добавить четыре вещи: перевод позиций в повороты костей через FromToRotation, pole target для управления плоскостью изгиба, ограничения суставов в прямом проходе и правильное место в кадре — LateUpdate после Animator. Для рук и ног гуманоида чаще хватает встроенного IK или Two Bone IK из Animation Rigging, а своё решение окупается на длинных цепочках, процедурных существах и там, где нужен полный контроль над поведением скелета.