Chapter 12: Blending between Animations

This fade is usually short—a quarter of a second or less. (动画之间的fade一般很快，0.5s甚至更短)

本章重点：

• 两个Pose的Blending
• 不同Animations的CrossFading，会有一个cross fade controller来处理一个blend queue
• Additive Blending

Pose blending

也没啥新东西，主要是：

• Pose Blending很简单，如果没有动画遮罩(AvatarMaks)，把所有的Joints的Transform的各个Property按比例，进行插值即可，可以使用线性插值；如果有AvatarMask，那么就只插值那些没有被Mask过的Joints
• Animation Blending本章上是Pose的Blending，而Pose是Sample Animation得到的结果
• Blend操作的俩Pose需要是相似的，它们的Joint的hierarchy一般是相同的
• For blending to work, it must happen in local space

这里声明俩全局函数，放在Blending.h文件里，如下所示：

// root node有点特殊, 它代表Pose b的第root个Joint的subHierarchy会被Blend到Pose a的相关部分
// 也就是说, 这里的Blend函数本身就带了一定程度上的AvatarMask功能(但是它一次性只能mask整个subHierarchy
// 跟Unity的AvatarMask功能比起来还是弱了一点)
void Blend(Pose& a, const Pose& b,float t,int root);
// 如果Pose里的search节点是root节点的子节点, 返回true
bool IsInHierarchy(Pose& pose, unsigned int root, unsigned int search);

函数定义如下，就是各自的LocalTransform的插值，很简单：

// 根据a和b进行插值, 改变原本的pose a, 作为output pose
void Blend(Pose& a, const Pose& b, float t, int blendroot) 
{
	unsigned int numJoints = output.Size();
	// 遍历所有Joints
	for (unsigned int i = 0; i < numJoints; ++i) 
	{
		// 如果blendroot>=0, 则是在pose a的基础上, 在a的的blendroot节点的subTree与B对应的subTree进行Blend
		// 如果blendroot为-1, 则是两个Pose在整个Hierarchy上的blend, 不需要做第一步的check
		if (blendroot >= 0) 
		{
			// 	1. 如果joint不在混合的subTree下, 则不处理joint的数据
			if (!IsInHierarchy(a, (unsigned int)blendroot, i)) 
				continue;	
		}

		// 2. 否则进行混合
		a.SetLocalTransform(i, mix(a.GetLocalTransform(i), b.GetLocalTransform(i), t));
	}
}

// 判断search节点的父节点链表里是否有parent节点
// 这里输入的parent >=0
bool IsInHierarchy(Pose& pose, unsigned int parent, unsigned int search) 
{
	if (search == parent)
		return true;
	
	// 遍历父节点链表
	int p = pose.GetParent(search);
	while (p >= 0) // p为-1时搜寻到了root
	{
		if (p == (int)parent)
			return true;
		
		p = pose.GetParent(p);
	}

	return false;
}

Crossfading animations

A crossfade is a fast blend from one animation to another.

前面设计的Blend函数一般用于动画之间的transition，也就是cross fade，这个过程其实就是在一段较短的时间内，不断改变插值比例，从0变到1，在这个过程里，每帧调用一次Blend函数，返回对应的Pose而已。

由于CrossFade这个操作，不是一帧就能完成的，这是个长期的任务，所以可以先创建一个CrossFadeTask类，代表从Pose A转换到Animation B这个任务，接口如下，由于只是个Data类，所以只有Header文件，没有Source文件：

// 属于一个Helper类, 用于帮助根据时间数据Sampler Clip数据, 得到Pose, 再与原本的mPose进行Blend
struct CrossFadeTask
{
	Pose mPose;					// 本身的Pose
	Clip* mClip;				// 需要Sample的Clip, 其实就是RestPose
	float mTime;				// Sample的相关信息
	float mDuration;			// CrossFade的相关信息
	float mElapsed;				// 已经过渡部分的时间信息(0f<=mElapsed<=mDuration)
	// default ctor
	inline CrossFadeTask() : mClip(0), mTime(0.0f),
							   mDuration(0.0f), mElapsed(0.0f) 
	// ctor with params
	inline CrossFadeTask(Clip* target, Pose& pose, float dur): 
		mClip(target), mTime(target -> GetStartTime()), 
		mPose(pose), mDuration(dur),
		mElapsed(0.0f) { }
};

Declaring the cross-fade task manager

接下来创建一个类，用于实现上面的cross fade task，它应该有一个Input和Output，Input就是CrossFade之前的Pose，Output就是执行Task期间算出的新Pose，类声明如下：

class CrossFadeTaskManager
{
protected:
	// 由于可能存在多个动画的Blend任务, 一个动画同时blend到多个动画, 所以这里设计为数组
	// 比如说, 从run过渡到idle和walk的组合状态(感觉很像BlendSpace)
	std::vector<CrossFadeTask> mTasks;
	Clip* mClip;						// 为啥还有个mClip?
	float mTime;
	Pose mPose;							// 如果存在mSkeleton, 则mPose存储了mSkeleton里的RestPose
	Skeleton mSkeleton;	
	bool mWasSkeletonSet;				// 	mWasSkeletonSet用于标识CrossFadeTaskManager里是否已经设置好了Skeleton
public:
	CrossFadeTaskManager();
	CrossFadeTaskManager(Skeleton& skeleton);
	void SetSkeleton(Skeleton& skeleton);
	void BeginPlay(Clip* targetClip);								// 开始播放targetClip, 停止Blending
	void AddCrossFadeTask(Clip* target, float fadeTime);
	void Update(float dt);
	Pose& GetCurrentPose();
	Clip* GetcurrentClip();
};

相关代码实现如下：

#include "CrossFadeTaskManager.h"
#include "Blending.h"

// 默认ctor
CrossFadeTaskManager::CrossFadeTaskManager() 
{
	mClip = 0;
	mTime = 0.0f;
	mWasSkeletonSet = false;
}

// 带Skeleton参数的ctor
CrossFadeTaskManager::CrossFadeTaskManager(Skeleton& skeleton) 
{
	mClip = 0;
	mTime = 0.0f;
	SetSkeleton(skeleton);
}

void CrossFadeTaskManager::SetSkeleton(Skeleton& skeleton) 
{
	mSkeleton = skeleton;
	mPose = mSkeleton.GetRestPose();
	mWasSkeletonSet = true;
}

// 开始播放targetClip, 停止Blending
void CrossFadeTaskManager::BeginPlay(Clip* targetClip) 
{
	// 只是设置播放状态, 并没有真正开始Play
	mTasks.clear();
	mClip = targetClip;
	mPose = mSkeleton.GetRestPose();
	mTime = targetClip->GetStartTime();
}

// 从当前的mPose开始Fade到targetClip
void CrossFadeTaskManager::AddCrossFadeTask(Clip* targetClip, float fadeTime) 
{
	// 如果当前没有mPose, 则直接设置播放状态到targetClip
	if (mClip == 0) 
	{
		BeginPlay(targetClip);
		return;
	}
	
	// 两种情况下不会添加targetClip对应的CrossFadeTask
	// 1. Task数组最后一个已经是目标Task了
	// 2. 在Task数组为空时, 已经在处理targetClip对应的CrossFadeTask了

	// 如果存在mTask数组, 检查最后一个任务是不是已经为目标Clip
	if (mTasks.size() >= 1) 
	{
		if (mTasks[mTasks.size() - 1].mClip == targetClip)
			return;
	}
	else 
	{
		// mClip应该是代表了CrossFadeManager正在播放的clip
		if (mClip == targetClip) 
			return;// 如果新的要CrossFade到的targetClip也是正在播放的Clip
	}

	mTasks.push_back(CrossFadeTask(targetClip, mSkeleton.GetRestPose(), fadeTime));
}

// 这个函数应该会每帧执行
void CrossFadeTaskManager::Update(float dt) 
{
	if (mClip == 0 || !mWasSkeletonSet)
		return;

	// 遍历CrossFadeTask数组, 根据当前时间顺序判断哪些任务已经执行完了
	// 如果执行完了, 就从mTasks里删除掉, 注意, 一帧只可以删除一个BlendTask
	unsigned int numTasks = mTasks.size();
	for (unsigned int i = 0; i < numTasks ; ++i) 
	{
		// 判断任务如果执行完了, 标识当前帧就是播放的对应动画
		if (mTasks[i].mElapsed >= mTasks[i].mDuration) 
		{
			// 存储被执行完成任务的Clip、mTime和mPose
			// 这代码太奇怪了
			mClip = mTasks[i].mClip;
			mTime = mTasks[i].mTime;
			//mPose = mTasks[i].mPose;// ....?这代码有病吧? 后面反正会Reset的

			mTasks.erase(mTasks.begin() + i);
			break;
		}
	}

	// 
	numTasks = mTasks.size();
	// 每次Blend动画, 基础都是Skeleton里的RestPose
	mPose = mSkeleton.GetRestPose();
	// 正常来说, mTime是随着Sample函
	mTime = mClip->Sample(mPose, mTime + dt);// 这里的mTime会随着Loop参数被取模....

	// 更新剩余的没完成的BlendTask的时间
	for (unsigned int i = 0; i < numTasks ; ++i) 
	{
		// 注意, 这些Task的时间是同时增加的
		CrossFadeTask& task = mTasks[i];
		// Sample函数的结果会存在task.mPose里
		task.mTime = task.mClip->Sample(task.mPose, task.mTime + dt);
		task.mElapsed += dt;
		float t = task.mElapsed / task.mDuration;
		if (t > 1.0f)
			t = 1.0f;

		// 基于Skeleton的RestPose, 不断与BlendTasks里的动画对应的Pose进行Blend
		Blend(mPose, task.mPose, t, -1);// -1代表blend整个skeleton
	}
}

Pose& CrossFadeTaskManager::GetCurrentPose() 
{
	return mPose;
}

Clip* CrossFadeTaskManager::GetcurrentClip() 
{
	return mClip;
}

Additive blending

Additive animations are used to modify an animation by adding in extra joint movements.

通过一些Additive动画，可以通过添加额外的joint movements修改来动画，它的核心是把该Additive动画带来的offset加在原本的动画上。
这种动画一般都很特殊，比如说我有个左倾斜的动画，这个动画仅仅改变了人物的spine，时长为1.0s，0.0s时的人物是直立的，随着时间从0s过渡到1s，动画左倾的程度越来越明显。这种动画在实际应用时，一般不是简单的按时间来进行Sample的，而是经常与Input联系到一起。比如说，人物的左摇杆，向左扭动的幅度越大，这里的动画播放的时长越接近1.0s。

Not all animations are a good fit for additive animations. Additive animations are usually specifically made. Additive animations typically don’t play according to time, but rather, according to some other input. It’s common to sync the playback of additive animations to something other than time.

这里在Blending.h里再声明两个全局函数：

// 这个函数会在load additive animtion clip时被调用, 在time为0的地方Sample
// additive animtion clip, 返回对应的BasePose
Pose MakeAdditivePose(Skeleton& skeleton, Clip& clip);
// result pose = input pose + (additive pose – additive base pose)
void Add(Pose& inAndOutPose, const Pose& addPose, const Pose& additiveBasePose, int blendroot);

实现的代码如下：

// 基于restPose, Sample, 返回新Pose即可
Pose MakeAdditivePose(Skeleton& skeleton, Clip& clip) 
{
	Pose result = skeleton.GetRestPose();
	clip.Sample(result, clip.GetStartTime());
	return result;
}

// 其实就是算出DeltaTransform加在原本的每个Joint上, 很简单
void Add(Pose& inAndOutPose, const Pose& addPose, const Pose& basePose, int blendroot) 
{
	unsigned int numJoints = addPose.Size();
	for (unsigned int i = 0; i < numJoints; ++i) 
	{
		Transform input = inAndOutPose.GetLocalTransform(i);
		Transform additiveBase = basePose.GetLocalTransform(i);
		Transform additiveCur = addPose.GetLocalTransform(i);
		
		// SubTree的mask
		if (blendroot >= 0 && !IsInHierarchy(addPose, blendroot, i)) 
			continue;

		// outPose = inPose + (addPose - basePose)
		Transform result(
			input.position + (additiveCur.position - additiveBase.position),		// Delta Position
			normalized(input.rotation * (inverse(additiveBase.rotation) * additiveCur.rotation)),	// Delta Rotation
			input.scale + (additiveCur.scale - additiveBase.scale));// 这里的Scale居然是减, 为啥不是除
		
		inAndOutPose.SetLocalTransform(i, result);
	}
}

Additive animations are most often used to create new animation variants

Additive动画经常用于创建新的动画variants，这里的Additive不一定非得是动画，也可以是一个Pose，常见的比如说，拿一个蹲着的pose，然后作为Additive Animation(或者说Pose)，去跟其他的动画Blend，比如说跟Walking动画进行Additive Blending，就可以得到一个蹲着走的动画（感谢好像是一种程序性动画）

总结

本章主要是以下内容：

• Blending的本质，就是对相同Skeleton的两个Pose的相同关节进行Transform数据的插值
• Cross Fade的本质就是在一段较短时间内，逐帧调用Blend函数，然后不断改变之间动画的权重值，也就是插值的权重
• 还有一种特殊的Blend，叫做Additive Blending，是把一个动画的从第n帧到第0帧的offset，附加在已有的正常播放的动画上

Chapter 13: Implementing Inverse Kinematics

By using IK, you can figure out how to rotate the character’s shoulder, elbow, and wrist in a way that the character’s finger is always touching a specific point.

注意，IK的算法应该只涉及Rotation

常见的IK算法是CCD和FABRIK算法，这章的主要内容：

• CCD IK算法和实现CCD solver
• FABRIK IK算法和实现FABRIK solver
• 实现ball-and-socket constraints
• 实现hinge constraints
• 理解IK solvers应该用在动画流程的哪个地方，以及应该怎么使用(Understand where and how IK solvers fit into an animation pipeline)

Creating a CCD solver

CCD: Cyclic Coordinate Descent，算法有三个重要概念：

• goal: 也就是目标点
• IK chain：所有的joints的链表
• end effector：chain最尾部的joint

CCD算法的伪代码如下：

// 从倒数第二个Joint开始，反向遍历chain里的所有joints（倒数第一个节点是end effector）
foreach joint in ikchain.reverse() 
{
	// 算出joint指向end effector的向量，也就是当前的实际尾部
	jointToEffector = effector.position - joint.position
	// 算出joint指向goal的向量，也就是当前的预期尾部
	jointToGoal = goal.position - joint.position
	
	// 为了让此时的joint、Effector和Goal三点一线，注意这里算的DeltaRotation是基于WorldPosition
	// 来的，所以应该是GlobalRotation, 乘在左边
	joint.rotation = fromToRotation(jointToEffector, jointToGoal) * joint.rotation
}

思路是这样的：
从尾部节点开始逼近Goal，既然Effector的旋转改变也不影响最终的结果，那么直接从倒数第二个节点开始处理，其他的就跟代码里写的一样，简单来说，CCD就是从倒数第二个节点开始逆序遍历，改变遍历的节点的旋转，让该节点、尾部节点和Goal三点一线，遍历一次，视为一次迭代过程，过程如下图所示：

这里的CCD算法是个迭代算法，注意，这里倒数第二个Joint，算出来的DeltaRotation，会应用到Joint自身上，目的是让该Joint、Effector和Goal在一条直线上，CCD遍历到最后，最Parent的joint、Effector和goal会在一条直线上，但其他的Joint就不一定了。

声明CCD solver类

solver的意思是解决者，所以这个类其实就是用CCD算法解决IK问题的类，类声明如下：

// CCDSolver.h文件
class CCDSolver 
{
protected:
	std::vector<Transform> mIKChain;					// Joints的原始Transform, 除了root joint, 其他都存的Local数据
	unsigned int mNumSteps;								// 算法的迭代次数, 设置它可以有效的防止Chain的错误导致死循环
	float mThreshold;									// 用于浮点数比较的误差
	
public:
	CCDSolver();// default ctor
	unsigned int Size();								// 返回IK Chain的joint个数
	void Resize(unsigned int newSize);
	Transform& operator[](unsigned int index);			// 获取第i个节点的localTrans数据
	Transform GetGlobalTransform(unsigned int index);	// 获取第i个节点的globalTrans数据
	unsigned int GetNumSteps();							// 迭代次数的get函数
	void SetNumSteps(unsigned int numSteps);			// 迭代次数的set函数
	float GetThreshold();
	void SetThreshold(float value);
	// 如果IK Chain问题解决了, 返回true
	bool Solve(vec3 goalPos);							// gloal就是方程的解, IK问题的goal
}

The mNumSteps variable is used to make sure the solver doesn’t fall into an infinite loop. There is no guarantee that the end effector will ever reach the goal.

Implementing the CCD solver

下面是具体类的实现函数：

// 前面都是一些简单的Get, Set函数
CCDSolver::CCDSolver() 
{
	mNumSteps = 15;
	mThreshold = 0.00001f;
}

...//省略一些简单的Get和Set函数

// 算出Ik Chain第x个joint的GlobalTransform
Transform CCDSolver::GetGlobalTransform(unsigned int x) 
{
	unsigned int size = (unsigned int)mIKChain.size();
	Transform world = mIKChain[x];
	// 一级一级的左乘parent的transform
	for (int i = (int) x - 1; i >= 0; --i) 
		world = combine(mIKChain[i], world);
	
	return world;
}

// 唯一有点价值的函数, 就是把上面的伪代码实现了出来
bool CCDSolver::Solve(vec3 goalPos) 
{
	unsigned int size = Size();
	if (size == 0)
		return false;
		
	unsigned int effectorId = size - 1;
	float thresholdSq = mThreshold * mThreshold;
	// 注意, CCD是把这个IK循环过程迭代了很多次, 每次迭代都会逆序遍历一次Chain
	for (unsigned int i = 0; i < mNumSteps; ++i) 
	{
		// 在每次迭代的开始, 判断是否已经解决了IK问题
		vec3 effector = GetGlobalTransform(effectorId).position;
		
		// goalPose和effector几乎相同时, IK问题得到了解决, 返回true
		if (lenSq(goalPos - effector) < thresholdSq) 
			return true;
		
		for (int j = (int)size - 2; j >= 0; --j) 
		{
			// 算出effector的GlobalPos
			effector = GetGlobalTransform(effectorId).position;

			// 算出当前Joint对应的GlobalPosition和GlobalRotation
			Transform world = GetGlobalTransform(j);
			vec3 worldPosition = world.position;
			quat worldRotation = world.rotation;

			// 算出deltaRotation, 但是这个deltaRotation属于GLobalDeltaRotation
			vec3 toEffector = effector - worldPosition;// 这里都是Global的Pos
			vec3 toGoal = goalPos - worldPosition;
			quat deltaRot;
			if (lenSq(toGoal) > 0.00001f)
				deltaRot = fromTo(toEffector, toGoal);

			// 正常的写法
			mIKChain[j].rotation = deltaRot * mIKChain[j].rotation;

			// 书中的写法(我不明白为什么这么写)？？？？？？？？
			//quat newWorldRotation = worldRotation * deltaRot;
			//quat localRotation = newWorldRotation * inverse(worldRotation);
			//mIKChain[j].rotation = localRotation * mIKChain[j].rotation;

			effector = GetGlobalTransform(effectorId).position;
			if (lenSq(goalPos - effector) < thresholdSq)
				return true;
	}	
}

我对书里写的CCD算法，感到很疑惑，不过我看了下别人的CCD算法，感觉我写的没啥问题，为了防止错误，举几个别人代码里的CCD算法：

Unity官方给的CCD算法代码

参考：https://www.youtube.com/watch?v=MA1nT9RAF3k&ab_channel=Unity

他这里的代码加了点小优化，传统的CCD算法每次迭代是逆序遍历所有的Joint(除了Effector)，这里改进后，一次不会遍历所有的Joint，它第一次迭代只会逆序遍历两个Joint，然后不断递增。举个例子，假设一个Chain里，从Effector开始倒数，ID分别为0、1、2…，那么遍历顺序前后的变化如下图所示：

代码如下：

void Solve()
{
	Vector3 goalposition = goal.position;
	Vector3 effectorPosition = m_Bones[0].position;

	Vector3 targetPosition = Vector3.Lerp(effectorPosition, goalPosition, weight);
	float sqrDistance;
	int iterationcount = 0;

	do
	{
		for (int i = 0; i < m_Bones.count - 2; i++)
		{
			// 初始遍历两个Joint, 然后不断增加
			for (int j = 1; j < i + 3 && j < m_Bones.count; j++)
			{
				RotateBone(m_Bones[0], m_Bones[j], targetPosition);
				sqrDistance = (m_Bones[0].position - targetPosition).sqrMagnitude;
				if (sqrDistance <= sqrDistError)
					return;
			}
		}

		sqrDistance = (m_Bones[0].position - targetPosition).sqrMagnitude;
		iterationCount++;
	}
	while (sqrDistance > sqrDistError && iterationCount <= maxIterationCount);

}

public static void RotateBone(Transform effector, Transform bone, Vector3 goalPosition)
{
	// 先算DeltaGlobalRotation
	Vector3 effectorPosition = effector.position;
	Vector3 bonePosition = bone.position;
	Vector3 boneToEffector = effectorPosition - bonePosition;
	Vector3 boneToGoal = goalPosition - bonePosition;
	Quaternion fromToRotation = Quaternion.FromToRotation(boneToEffector, boneToGoal);

	bone.rotation = fromToRotation * bone.rotation;
}

Unity Final IK插件里的CCD算法代码

这个CCD IK算法加了个Joint的权重，以及Joint旋转的Limitations

// 位于IKSolverCCD.cs文件内
private void Solve(Vector3 targetPosition)
{
	// 2D
	if (XY)
	{
		...// 省略2D部分
	}
	else
	{
		for (int i = bones.Length - 2; i > -1; i--)
		{
			// 这里joint有个权重, 当权重为1时, 跟前面的IK算法是一样的
			// Slerp if weight is < 0
			//CCD tends to overemphasise the rotations of the bones closer to the target position. Reducing bone weight down the hierarchy will compensate for this effect.
			float w = bones[i].weight * IKPositionWeight;

			if (w > 0f)
			{
				Vector3 toLastBone = bones[bones.Length - 1].transform.position - bones[i].transform.position;
				Vector3 toTarget = targetPosition - bones[i].transform.position;

				// Get the rotation to direct the last bone to the target
				Quaternion targetRotation = Quaternion.FromToRotation(toLastBone, toTarget) * bones[i].transform.rotation;

				if (w >= 1) 
					bones[i].transform.rotation = targetRotation;
				// 如果权重值在0,1之间, 则需要对原本的rotation和新的rotation进行插值
				else 
					bones[i].transform.rotation = Quaternion.Lerp(bones[i].transform.rotation, targetRotation, w);
			}

			// 计算完了新的LocalRotation后, 施加rotationLimit
			if (useRotationLimits && bones[i].rotationLimit != null) 
				bones[i].rotationLimit.Apply();
		}
	}
}

还有很多类似的CCD IK算法，就不贴出来了，我都没有看到书里这么奇怪的转换代码，具体为啥这么写，好像是为了平稳过渡IK结果把，我也不是特别清楚，Remain。

FABRIK

FABRIK (Forward And Backward Reaching Inverse Kinematics): 这种IK算法看上去效果更好，收敛结果更接近人类的IK效果，与CCD IK算法一样，也是分为Goal、Effector和IK Chain三个内容。CCD IK是通过Rotation起效的，而FABRIK是通过position起效的，这种算法更容易理解。

FABRIK算法思路，分为两个Pass：

1. 第一个Pass，从Effector倒数往Root开始遍历，不断让遍历的Joint逼近Goal
2. 计算每个Joint相对于Effector的相对Pos
3. 第二个Pass，从Root再顺序遍历到Effector，其他的步骤与第一个Pass相同

算法思路如下图所示，第一个Pass，可以理解为像铁臂阿童木的手臂一样，从最外部手臂节点开始，依次发射自己一节节的Bone。这样Bone两端对应的Joints，一端的坐标就已经是目标点了，只需要求出另外一端joint的坐标即可。如下图a、b、c所示，相当于把p3p4旋转对转goal目标后，发射给goal点。这里具体的计算方法，就不需要用到旋转了，其实就是单纯的点的坐标计算。Goal点坐标是已知的，p3的坐标点也是已知的，p3p4这段Bone的长度也是已知的，假设为l3，那么新的p3’点的坐标为：

boneOldHead = p3;
boneTail = goal;// 即p4'点
// 计算方向, 对应的向量, 平移加上去即可
direction = normalize(boneOldHead - boneTail);// p4'p3对应的方向
offset = direction * boneLength;// 算出对应的offset
boneNewHead = boneTail + offset;// 新的p3点的位置，即p3'
bones[bones.size() - 2] = boneNewHead;

第一个Pass接下来的操作，跟上面介绍的差不多了，无非是Goal点坐标变了。在第二个Pass里，又会反向收回一节节的手臂，如图e和f所示：

伪代码大概是这样：

// 代表一次迭代过程
void Iterate(const Transform& goal) 
{
	startPosition = chain[0];
	// 从End向Root遍历, 这里不用管方向, 只需要计算每个Joint的Global位置即可
	// 遍历之前, 最后的joint的位置已经计算好了, 就是goal
	chain[size - 1] = goal.position;
	for (i = size - 2; i >= 0; --i) 
	{
		// 每次遍历的目的, 是为了计算chain[i]节点的位置, 从倒数第二个节点位置开始计算
		
		// 每次取两个连续的joint, 代表一块Bone
		head = chain[i];
		tail = chain[i + 1];
		// 计算方向, 对应的向量, 平移加上去即可
		direction = normalize(head - tail);
		offset = direction * length[i + 1];
		chain[i] = tail + offset;
	}

	// Iterate forwards
	chain[0] = startPosition;
	for (i = 1; i < size; ++i)
	{
		current = chain[i];
		prev = chain[i - 1];
		direction = normalize(current - prev);
		offset = direction * length[i];
		chain[i] = prev + offset;
	}
}

类声明如下：

class FABRIKSolver 
{
protected:
	std::vector<Transform> mIKChain;
	unsigned int mNumSteps;
	float mThreshold;
	
	// 与CCDIKReSolver相比, 多了俩数组
	std::vector<vec3> mWorldChain;	//这里的计算都是用的WorldPosition, mWorldChain与mIKChain里的Joints一一对应
	std::vector<float> mLengths;	// 记录所有Bone的长度, 就是Joint到其Parent Joint的距离, mLengths[0] = 0f
protected:
	void CalcWorldChainAndBoneLengthsFromIKChain();
	void IterateForward(const vec3& goal);
	void IterateBackward(const vec3& base);
	void WorldToIKChain();
public:
	FABRIKSolver();

	unsigned int Size();
	void Resize(unsigned int newSize);

	Transform GetLocalTransform(unsigned int index);
	void SetLocalTransform(unsigned int index, const Transform& t);
	Transform GetGlobalTransform(unsigned int index);

	unsigned int GetNumSteps();
	void SetNumSteps(unsigned int numSteps);

	float GetThreshold();
	void SetThreshold(float value);

	bool Solve(const Transform& target);
};

类实现如下：

#include "FABRIKSolver.h"

// 省略了一些简单的Get和Set函数
FABRIKSolver::FABRIKSolver() 
{
	mNumSteps = 15;
	mThreshold = 0.00001f;
}

unsigned int FABRIKSolver::Size() 
{
	return mIKChain.size();
}

void FABRIKSolver::Resize(unsigned int newSize) 
{
	mIKChain.resize(newSize);
	mWorldChain.resize(newSize);
	mLengths.resize(newSize);
}

Transform FABRIKSolver::GetLocalTransform(unsigned int index) 
{
	return mIKChain[index];
}

void FABRIKSolver::SetLocalTransform(unsigned int index, const Transform& t) 
{
	mIKChain[index] = t;
}

Transform FABRIKSolver::GetGlobalTransform(unsigned int index) 
{
	unsigned int size = (unsigned int)mIKChain.size();
	Transform world = mIKChain[index];
	for (int i = (int)index - 1; i >= 0; --i) 
		world = combine(mIKChain[i], world);
	
	return world;
}

// 很简单, 根据mIKChain算出WorldChain和每个joint与其parent的距离
void FABRIKSolver::CalcWorldChainAndBoneLengthsFromChainList() 
{
	unsigned int size = Size();
	for (unsigned int i = 0; i < size; ++i) 
	{
		// 遍历joint, 算出每个joint的GlobalTransform
		Transform world = GetGlobalTransform(i);
		mWorldChain[i] = world.position;

		// mLengths记录的是每个joint与其parent的距离
		if (i >= 1) 
		{
			vec3 prev = mWorldChain[i - 1];
			mLengths[i] = len(world.position - prev);
		}
	}
	if (size > 0) 
		mLengths[0] = 0.0f;
}


// 这个函数复杂一些, 它是根据WorldChain反过来推导IKChain
// WorldToIKChain这个函数作者写的有bug
void FABRIKSolver::WorldToIKChain() 
{
	unsigned int size = Size();
	if (size == 0)
		return; 

	// 顺序根据一系列Joints的Global坐标算出各自Local的Transform
	for (unsigned int i = 0; i < size - 1; ++i) 
	{
		// 作者是这么写的, 感觉很离谱, 有bug
		//Transform world = GetGlobalTransform(i);
		//Transform next = GetGlobalTransform(i + 1);
		// 应该这么写
		Transform world = mWorldChain[i];
		Transform next = mWorldChain[i + 1];
		// 获取当前节点的GlobalPos和GlobalRot
		vec3 position = world.position;
		quat rotation = world.rotation;

		// 把这个DeltaGlobalPos变换到LocalSpace下
		// toNext是原本的LocalPos
		vec3 toNext = next.position - position;
		toNext = inverse(rotation) * toNext;

		// toDesired是新的LocalPos
		vec3 toDesired = mWorldChain[i + 1] - position;
		toDesired = inverse(rotation) * toDesired;

		// 为啥又是左乘? 因为这个DeltaRot是用于curJoint的LocalPos上的, 所以应该是更Parent的一级
		quat delta = fromTo(toNext, toDesired);
		mIKChain[i].rotation = delta * mIKChain[i].rotation;
	}
}

void FABRIKSolver::IterateBackward(const vec3& goal)
{
	int size = (int)Size();
	if (size > 0) 
		mWorldChain[size - 1] = goal;
	

	for (int i = size - 2; i >= 0; --i) 
	{
		vec3 direction = normalized(mWorldChain[i] - mWorldChain[i + 1]);
		vec3 offset = direction * mLengths[i + 1];
		mWorldChain[i] = mWorldChain[i + 1] + offset;
	}
}

void FABRIKSolver::IterateForward(const vec3& base) 
{
	unsigned int size = Size();
	if (size > 0) 
		mWorldChain[0] = base;

	for (int i = 1; i < size; ++i) 
	{
		vec3 direction = normalized(mWorldChain[i] - mWorldChain[i - 1]);
		vec3 offset = direction * mLengths[i];
		mWorldChain[i] = mWorldChain[i - 1] + offset;
	}
}

bool FABRIKSolver::Solve(const Transform& target) 
{
	unsigned int size = Size();
	if (size == 0)
		return false;
	unsigned int last = size - 1;
	float thresholdSq = mThreshold * mThreshold;

	// 根据IKChain算出WorldChain和BoneLength数组
	IKChainToWorld();
	vec3 goal = target.position;
	vec3 base = mWorldChain[0];

	// TODO: 其实应该判断一下总长度, 是否小于base到goal的距离


	for (unsigned int i = 0; i < mNumSteps; ++i) 
	{
		vec3 effector = mWorldChain[last];
		if (lenSq(goal - effector) < thresholdSq) 
		{
			WorldToIKChain();
			return true;
		}

		IterateBackward(goal);
		IterateForward(base);
	}

	WorldToIKChain();
	vec3 effector = GetGlobalTransform(last).position;
	if (lenSq(goal - effector) < thresholdSq)
		return true;

	return false;
}

别人写的FabricIK算法

参考：https://www.youtube.com/watch?v=qqOAzn05fvk&t=1217s&ab_channel=DitzelGames

别人写的FabricIK算法

Final IK插件里的：

using UnityEngine;
using System.Collections;
using System;

namespace RootMotion.FinalIK 
{
	[System.Serializable]
	public class IKSolverFABRIK : IKSolverHeuristic 
	{
		// 算法阶段一
		public void SolveForward(Vector3 position) 
		{
			OnPreSolve();	
			ForwardReach(position);
		}
		
		// 算法阶段二
		public void SolveBackward(Vector3 position) 
		{
			BackwardReach(position);
			OnPostSolve();
		}

		public override Vector3 GetIKPosition() 
		{
			if (target != null) 
				return target.position;
			
			return IKPosition;
		}

		// Called before each iteration of the solver.
		public IterationDelegate OnPreIteration;

		private bool[] limitedBones = new bool[0];
		private Vector3[] solverLocalPositions = new Vector3[0];

		protected override void OnInitiate() 
		{
			if (firstInitiation || !Application.isPlaying) IKPosition = bones[bones.Length - 1].transform.position;

			for (int i = 0; i < bones.Length; i++) 
			{
				bones[i].solverPosition = bones[i].transform.position;
				bones[i].solverRotation = bones[i].transform.rotation;
			}
			
			limitedBones = new bool[bones.Length];
			solverLocalPositions = new Vector3[bones.Length];
			
			InitiateBones();

			for (int i = 0; i < bones.Length; i++) 
			{
				solverLocalPositions[i] = Quaternion.Inverse(GetParentSolverRotation(i)) * (bones[i].transform.position - GetParentSolverPosition(i));
			}
		}
		
		protected override void OnUpdate() 
		{
			if (IKPositionWeight <= 0) return;
			IKPositionWeight = Mathf.Clamp(IKPositionWeight, 0f, 1f);
			
			OnPreSolve();

			if (target != null) IKPosition = target.position;
			if (XY) IKPosition.z = bones[0].transform.position.z;

			Vector3 singularityOffset = maxIterations > 1? GetSingularityOffset(): Vector3.zero;

			// Iterating the solver
			for (int i = 0; i < maxIterations; i++) {
				// Optimizations
				if (singularityOffset == Vector3.zero && i >= 1 && tolerance > 0 && positionOffset < tolerance * tolerance) break;
				lastLocalDirection = localDirection;

				if (OnPreIteration != null) OnPreIteration(i);
				
				Solve(IKPosition + (i == 0? singularityOffset: Vector3.zero));
			}
			
			OnPostSolve();
		}
		
		/*
		 * If true, the solver will work with 0 length bones
		 * */
		protected override bool boneLengthCanBeZero { get { return false; }} // Returning false here also ensures that the bone lengths will be calculated

		/*
		 * Interpolates the joint position to match the bone's length
		*/
		private Vector3 SolveJoint(Vector3 pos1, Vector3 pos2, float length) 
		{
			if (XY) pos1.z = pos2.z;
			
			return pos2 + (pos1 - pos2).normalized * length;
		}

		/*
		 * Check if bones have moved from last solved positions
		 * */
		private void OnPreSolve() 
		{
			chainLength = 0;

			for (int i = 0; i < bones.Length; i++) 
			{
				bones[i].solverPosition = bones[i].transform.position;
				bones[i].solverRotation = bones[i].transform.rotation;

				if (i < bones.Length - 1) {
					bones[i].length = (bones[i].transform.position - bones[i + 1].transform.position).magnitude;
					bones[i].axis = Quaternion.Inverse(bones[i].transform.rotation) * (bones[i + 1].transform.position - bones[i].transform.position);

					chainLength += bones[i].length;
				}

				if (useRotationLimits) solverLocalPositions[i] = Quaternion.Inverse(GetParentSolverRotation(i)) * (bones[i].transform.position - GetParentSolverPosition(i));
			}
		}
		

		// After solving the chain
		
		private void OnPostSolve() 
		{
			// Rotating bones to match the solver positions
			if (!useRotationLimits) 
				MapToSolverPositions();
			else 
				MapToSolverPositionsLimited();

			lastLocalDirection = localDirection;
		}
		
		private void Solve(Vector3 targetPosition) 
		{
			// Forward reaching
			ForwardReach(targetPosition);

			// Backward reaching
			BackwardReach(bones[0].transform.position);
		}
		
		/*
		 * Stage 1 of FABRIK algorithm
		 * */
		private void ForwardReach(Vector3 position) 
		{
			// Lerp last bone's solverPosition to position
			bones[bones.Length - 1].solverPosition = Vector3.Lerp(bones[bones.Length - 1].solverPosition, position, IKPositionWeight);

			for (int i = 0; i < limitedBones.Length; i++) limitedBones[i] = false;
			
			for (int i = bones.Length - 2; i > -1; i--) {
				// Finding joint positions
				bones[i].solverPosition = SolveJoint(bones[i].solverPosition, bones[i + 1].solverPosition, bones[i].length);
				
				// Limiting bone rotation forward
				LimitForward(i, i + 1);
			}
			
			// Limiting the first bone's rotation
			LimitForward(0, 0);
		}

		private void SolverMove(int index, Vector3 offset) 
		{
			for (int i = index; i < bones.Length; i++) 
				bones[i].solverPosition += offset;
		}

		private void SolverRotate(int index, Quaternion rotation, bool recursive) 
		{
			for (int i = index; i < bones.Length; i++) 
			{
				bones[i].solverRotation = rotation * bones[i].solverRotation;
				if (!recursive)
					return;
			}
		}

		private void SolverRotateChildren(int index, Quaternion rotation) {
			for (int i = index + 1; i < bones.Length; i++) {
				bones[i].solverRotation = rotation * bones[i].solverRotation;
			}
		}

		private void SolverMoveChildrenAroundPoint(int index, Quaternion rotation) {
			for (int i = index + 1; i < bones.Length; i++) {
				Vector3 dir = bones[i].solverPosition - bones[index].solverPosition;
				bones[i].solverPosition = bones[index].solverPosition + rotation * dir;
			}
		}

		private Quaternion GetParentSolverRotation(int index) {
			if (index > 0) return bones[index - 1].solverRotation;
			if (bones[0].transform.parent == null) return Quaternion.identity;
			return bones[0].transform.parent.rotation;
		}

		private Vector3 GetParentSolverPosition(int index) {
			if (index > 0) return bones[index - 1].solverPosition;
			if (bones[0].transform.parent == null) return Vector3.zero;
			return bones[0].transform.parent.position;
		}

		private Quaternion GetLimitedRotation(int index, Quaternion q, out bool changed) {
			changed = false;
			
			Quaternion parentRotation = GetParentSolverRotation(index);
			Quaternion localRotation = Quaternion.Inverse(parentRotation) * q;
			
			Quaternion limitedLocalRotation = bones[index].rotationLimit.GetLimitedLocalRotation(localRotation, out changed);
			
			if (!changed) return q;
			
			return parentRotation * limitedLocalRotation;
		}

		/*
		 * Applying rotation limit to a bone in stage 1 in a more stable way
		 * */
		private void LimitForward(int rotateBone, int limitBone) {
			if (!useRotationLimits) return;
			if (bones[limitBone].rotationLimit == null) return;

			// Storing last bone's position before applying the limit
			Vector3 lastBoneBeforeLimit = bones[bones.Length - 1].solverPosition;

			// Moving and rotating this bone and all its children to their solver positions
			for (int i = rotateBone; i < bones.Length - 1; i++) {
				if (limitedBones[i]) break;

				Quaternion fromTo = Quaternion.FromToRotation(bones[i].solverRotation * bones[i].axis, bones[i + 1].solverPosition - bones[i].solverPosition);
				SolverRotate(i, fromTo, false);
			}

			// Limit the bone's rotation
			bool changed = false;
			Quaternion afterLimit = GetLimitedRotation(limitBone, bones[limitBone].solverRotation, out changed);

			if (changed) {
				// Rotating and positioning the hierarchy so that the last bone's position is maintained
				if (limitBone < bones.Length - 1) {
					Quaternion change = QuaTools.FromToRotation(bones[limitBone].solverRotation, afterLimit);
					bones[limitBone].solverRotation = afterLimit;
					SolverRotateChildren(limitBone, change);
					SolverMoveChildrenAroundPoint(limitBone, change);

					// Rotating to compensate for the limit
					Quaternion fromTo = Quaternion.FromToRotation(bones[bones.Length - 1].solverPosition - bones[rotateBone].solverPosition, lastBoneBeforeLimit - bones[rotateBone].solverPosition);

					SolverRotate(rotateBone, fromTo, true);
					SolverMoveChildrenAroundPoint(rotateBone, fromTo);

					// Moving the bone so that last bone maintains its initial position
					SolverMove(rotateBone, lastBoneBeforeLimit - bones[bones.Length - 1].solverPosition);
				} else {
					// last bone
					bones[limitBone].solverRotation = afterLimit;
				}
			}

			limitedBones[limitBone] = true;
		}
		
		/*
		 * Stage 2 of FABRIK algorithm
		 * */
		private void BackwardReach(Vector3 position) {
			if (useRotationLimits) BackwardReachLimited(position);
			else BackwardReachUnlimited(position);
		}
		
		/*
		 * Stage 2 of FABRIK algorithm without rotation limits
		 * */
		private void BackwardReachUnlimited(Vector3 position) {
			// Move first bone to position
			bones[0].solverPosition = position;
			
			// Finding joint positions
			for (int i = 1; i < bones.Length; i++) {
				bones[i].solverPosition = SolveJoint(bones[i].solverPosition, bones[i - 1].solverPosition, bones[i - 1].length);
			}
		}
		
		/*
		 * Stage 2 of FABRIK algorithm with limited rotations
		 * */
		private void BackwardReachLimited(Vector3 position) {
			// Move first bone to position
			bones[0].solverPosition = position;

			// Applying rotation limits bone by bone
			for (int i = 0; i < bones.Length - 1; i++) {
				// Rotating bone to look at the solved joint position
				Vector3 nextPosition = SolveJoint(bones[i + 1].solverPosition, bones[i].solverPosition, bones[i].length);

				Quaternion swing = Quaternion.FromToRotation(bones[i].solverRotation * bones[i].axis, nextPosition - bones[i].solverPosition);
				Quaternion targetRotation = swing * bones[i].solverRotation;

				// Rotation Constraints
				if (bones[i].rotationLimit != null) {
					bool changed = false;
					targetRotation = GetLimitedRotation(i, targetRotation, out changed);
				}

				Quaternion fromTo = QuaTools.FromToRotation(bones[i].solverRotation, targetRotation);
				bones[i].solverRotation = targetRotation;
				SolverRotateChildren(i, fromTo);

				// Positioning the next bone to its default local position
				bones[i + 1].solverPosition = bones[i].solverPosition + bones[i].solverRotation * solverLocalPositions[i + 1];
			}

			// Reconstruct solver rotations to protect from invalid Quaternions
			for (int i = 0; i < bones.Length; i++) {
				bones[i].solverRotation = Quaternion.LookRotation(bones[i].solverRotation * Vector3.forward, bones[i].solverRotation * Vector3.up);
			}
		}

		/*
		 * Rotate bones to match the solver positions when not using Rotation Limits
		 * */
		private void MapToSolverPositions() {
			bones[0].transform.position = bones[0].solverPosition;
			
			for (int i = 0; i < bones.Length - 1; i++) {
				if (XY) {
					bones[i].Swing2D(bones[i + 1].solverPosition);
				} else {
					bones[i].Swing(bones[i + 1].solverPosition);
				}
			}
		}

		/*
		 * Rotate bones to match the solver positions when using Rotation Limits
		 * */
		private void MapToSolverPositionsLimited() {
            bones[0].transform.position = bones[0].solverPosition;

			for (int i = 0; i < bones.Length; i++) {
				if (i < bones.Length - 1) bones[i].transform.rotation = bones[i].solverRotation;
			}
		}
	}
}

参考：https://forum.unity.com/threads/ik-chain-constraints-fabrik-algorithm.209306/

using UnityEngine;
using System.Collections;
 
/**
 * FABRIK Solver based on paper found here - www.andreasaristidou.com/publications/FABRIK.pdf  
 *
 * http://forum.unity3d.com/threads/187838-INVERSE-KINEMATICS-Scripting-Tutorial-Searching?p=1283005&viewfull=1#post1283005
*/
 
public class FABRIK : MonoBehaviour
{
    public int maxSolverIterations = 20; // 15 iterations is average solve time
    public float solveAccuracy = 0.001f;
   
    public IKChain myChain;
   
    void Start()
    {
        this.myChain.Init();
    }
   
    void Update()
    {
        if (this.myChain.target != null)
        {
            this.Solve(this.myChain);
        }
    }
   
    void Solve(IKChain chain)
    {
        var joints = chain.joints;
        if (joints.Length < 2)
            return;
       
        var rootToTargetDist = Vector3.Distance(joints[0].position, chain.target.position);
        var lambda = 0f;
       
        // Target unreachable, chain.length记录了chain的总长度
        if (rootToTargetDist > chain.length)
        {
        	// 遍历每段Bone
            for (int i = 0; i < joints.Length - 1; i++)
            {
            	// 计算这段Bone的长度, 与剩余的长度(包含Bone)的比例
                lambda = chain.segLengths[i] / Vector3.Distance(joints[i].position, chain.target.position);
                // 按照长度比例进行累加
                joints[i+1].position = (1 - lambda) * joints[i].position + lambda * chain.target.position;
            }
        }
        else // Target within reach
        {
            chain.Reset();
           
            var rootInitial = joints[0].position;
            var tries = 0;
            var targetDelta = Vector3.Distance(joints[joints.Length-1].position, chain.target.position);
           
            while (targetDelta > this.solveAccuracy  tries < this.maxSolverIterations)
            {
                // Forward reaching phase
               
                joints[joints.Length-1].position = chain.target.position;
               
                for (int i = joints.Length - 2; i > 0; i--)
                {
                    lambda = chain.segLengths[i] / Vector3.Distance(joints[i+1].position, joints[i].position);
                    var pos = (1 - lambda) * joints[i+1].position + lambda * joints[i].position;
                    joints[i].position = pos;
                    joints[i].position = this.Constraints(joints[i+1], joints[i]);
                }
               
                // Backward reaching phase
               
                joints[0].position = rootInitial;
               
                for (int i = 0; i < joints.Length - 1; i++)
                {
                    lambda = chain.segLengths[i] / Vector3.Distance(joints[i+1].position, joints[i].position);
                    var pos = (1 - lambda) * joints[i].position + lambda * joints[i+1].position;
                    joints[i+1].position = pos;
                    joints[i+1].position = this.Constraints(joints[i], joints[i+1]);
                }
 
                targetDelta = Vector3.Distance(joints[joints.Length-1].position, chain.target.position);
                tries++;
            }
        }
    }
   
    Vector3 Constraints(IKJoint j, IKJoint j_1)
    {
        return j_1.position;
    }
}
 

附录

Joint的WorldRotation与LocalRotation的互换

假设有Joint X，其Parent是A，A的Parent是Root R，他们的LocalRotation都知道，那么X的WorldRotation很好算：

WorldRotationX = LocalRotationR * LocalRotationA * LocalRotationX;

但假设我知道X、A和R的WorldRotation，如何求X的LocalRotation呢？

可以写个公式先看看：

WorldRotationA * LocalRotationX = WorldRotationX;

那么结果很明显：

LocalRotationX = WorldRotationA.Inverse() * WorldRotationX;

所以说，要想求某个Joint的Local旋转矩阵，用其Parent的世界矩阵的逆左乘该Joint的世界旋转矩阵即可。

根据Joint的DeltaGlobalRotation获取其newLocalRotation

其实就是需要改变Joint原本的LocalRotation而已，Global的DeltaRotation是左乘，所以有

newLocalRotation = DeltaGlobalRotation * LocalRotation;

别想太复杂了

旋转矩阵之间的乘法满足交换律吗

Are rotation matrices commutative?
The two-dimensional case is the only non-trivial (i.e. not one-dimensional) case where the rotation matrices group is commutative, so that it does not matter in which order multiple rotations are performed.

只有2D的旋转矩阵的乘法满足交换律，其他维度的不满足

如何通过WorldChain获得LocalChain

其实可以拆分为一个个子问题，假设A和B的WorldTrasform知道， 那么如何算B在A的LocalTransform?

这个很简单，WorldA * LocalB = WorldB，所以LocalB = WorldA.Inverse() * WorldB