前回

IK, Append 変換

IK(Inverse Kinematics)

IKについて話しましょう。

まずIKを説明する前に、FK（Forward Kinematics）について話します。

FKは親関節が動くと子関節も動きます。

キーフレームアニメーションが代表的なFK方式です。

IKはその逆です。

IKは関節の先端（手や足）の目標位置を設定すると、その位置に到達するために関節が回転します。

親関節が子関節に従って動くということです。

ゲームでも特定の状況において、FKよりも自然なポーズを作るために多く使用されます。

IKを実装する方法はいくつかありますが、私たちはCCD IKを使用します。

CCD IK(Cyclic Coordinate Descent Inverse Kinematics)

CCD IKは、ボーンを順番に回転させて目標の位置に到達させる方法です。
この過程を図を使って説明しましょう。

上の図は、ルート関節を含む3つの関節を持つ腕です。
腕の先が目標位置に到達できるように、CCDIK動作を適用してみましょう。

最も上にある関節の位置からターゲットに向かうベクトルを求めます。

腕がベクトルと同じ方向になるように関節を回転させます。

次の関節でも、ターゲットの位置に向かうベクトルを求めます。

今回も腕がベクトルと同じ方向になるように関節を回転させます。

この過程を腕の先端が目標位置に到達するまで繰り返します。

下の動画を見るとより理解しやすいです。
https://youtu.be/MvuO9ZHGr6k

このCCDIKを実装してみましょう。

VMDIKKey

IKもアニメーションと同様に、フレームごとの情報があります。

struct VMDIKkey
{
	unsigned int frameNo;
	bool enable;

	VMDIKkey(unsigned int frameNo, bool enable) :
		frameNo(frameNo),
		enable(enable)
	{}
};

VMDKeyみたいにframeNoを持っていますが、他のメンバーには特に特別なものはありません。

該当フレームでIKが有効かどうかの情報のみを持っています。

BoneNodeにVMDIKkeyも持つようにメンバーとして追加しましょう。

std::vector<VMDKey> _motionKeys;
std::vector<VMDIKkey> _ikKeys;

VMDIKkeyを追加できるようにメソッドも追加します。

void BoneNode::AddIKkey(unsigned int& frameNo, bool& enable)
{
	_ikKeys.emplace_back(frameNo, enable);
}

PMXActorでVMDファイルの内容を設定する部分でIKデータも追加するように修正しましょう。

void PMXActor::InitAnimation(VMDFileData& vmdFileData)
{
	for (auto& motion : vmdFileData.motions)
	{
		auto boneNode = _nodeManager.GetBoneNodeByName(motion.boneName);
		if (boneNode == nullptr)
		{
			continue;
		}

		boneNode->AddMotionKey(motion.frame,
			motion.quaternion,
			motion.translate,
			XMFLOAT2(static_cast<float>(motion.interpolation[3]) / 127.0f, static_cast<float>(motion.interpolation[7]) / 127.0f),
			XMFLOAT2(static_cast<float>(motion.interpolation[11]) / 127.0f, static_cast<float>(motion.interpolation[15]) / 127.0f));
	}

	for (VMDIK& ik : vmdFileData.iks)
	{
		for (VMDIKInfo& ikInfo : ik.ikInfos)
		{
			auto boneNode = _nodeManager.GetBoneNodeByName(ikInfo.name);
			if (boneNode == nullptr)
			{
				continue;
			}

			bool enable = ikInfo.enable;
			boneNode->AddIKkey(ik.frame, enable);
		}
	}

	_nodeManager.SortKey();
}

VMDFileDataはVMDIKをstd::vectorとして持っていました。

VMDIKのVMDIKInfoを順に巡回しながら、ボーンにデータを入れます。

IKSolver

IK演算を処理する最も重要なクラスを作成します。

class IKSolver
{
public:
	IKSolver(BoneNode* node, BoneNode* targetNode, unsigned int iterationCount, float limitAngle);

	void Solve();

	void AddIKChain(BoneNode* linkNode, bool enableAxisLimit, const DirectX::XMFLOAT3& limitMin, const DirectX::XMFLOAT3 limitMax)
	{
		_ikChains.emplace_back(linkNode, enableAxisLimit, limitMin, limitMax);
	}

	const std::wstring& GetIKNodeName() const;

	bool GetEnable() const { return _enable; }
	void SetEnable(bool enable) { _enable = enable; }

	BoneNode* GetIKNode() const { return _ikNode; }
	BoneNode* GetTargetNode() const { return _targetNode; }

	const std::vector<IKChain>& GetIKChains() const { return _ikChains; }

	unsigned int GetIterationCount() const { return _ikIterationCount; }
	float GetLimitAngle() const { return _ikLimitAngle; }

private:
	void SolveCore(unsigned int iteration);
	void SolvePlane(unsigned int iteration, unsigned int chainIndex, SolveAxis solveAxis);
	XMFLOAT3 Decompose(const XMMATRIX& m, const XMFLOAT3& before);
	float NormalizeAngle(float angle);
	float DiffAngle(float a, float b);

private:
	bool _enable;

	BoneNode* _ikNode;
	BoneNode* _targetNode;

	std::vector<IKChain> _ikChains;

	unsigned int _ikIterationCount;
	float _ikLimitAngle;
};

各メンバーの詳細は、順番に説明していきます。

まず、IKChainを追加する部分は、インラインで作成します。

IKChainも作成しましょう。

struct IKChain
{
	BoneNode* boneNode;
	bool enableAxisLimit;
	DirectX::XMFLOAT3 limitMin;
	DirectX::XMFLOAT3 limitMax;
	DirectX::XMFLOAT3 prevAngle;
	DirectX::XMFLOAT4 saveIKRotation;
	float planeModeAngle;

	IKChain(BoneNode* linkNode, bool axisLimit, const DirectX::XMFLOAT3& limitMinimum, const DirectX::XMFLOAT3 limitMaximum)
	{
		boneNode = linkNode;
		enableAxisLimit = axisLimit;
		limitMin = limitMinimum;
		limitMax = limitMaximum;
		saveIKRotation = DirectX::XMFLOAT4(0.f, 0.f, 0.f, 0.f);
	}
};

ターゲットボーンとルートボーンを結ぶボーンを表す構造体です。

回転の最大値、最小値とIK演算を進める間に保存する回転値を持っています。

IKSolver::IKSolver(BoneNode* node, BoneNode* targetNode, unsigned iterationCount, float limitAngle) :
	_ikNode(node),
	_targetNode(targetNode),
	_ikIterationCount(iterationCount),
	_ikLimitAngle(limitAngle),
	_enable(true)
{
}

これはIKSolverのコンストラクタです。

各メンバーを初期化します。

ルートボーン、ターゲットボーン、反復回数、制限角度、有効フラグを初期化しています。

Solve

IK演算を外部から呼び出す最も重要なメソッドです。

void IKSolver::Solve()
{
	if (_enable == false)
	{
		return;
	}

	if (_ikNode == nullptr || _targetNode == nullptr)
	{
		return;
	}

	for (IKChain& chain : _ikChains)
	{
		chain.prevAngle = DirectX::XMFLOAT3(0.f, 0.f, 0.f);
		chain.boneNode->SetIKRotation(XMMatrixIdentity());
		chain.planeModeAngle = 0.f;

		chain.boneNode->UpdateLocalTransform();
		chain.boneNode->UpdateGlobalTransform();
	}

	float maxDistance = std::numeric_limits<float>::max();
	for (unsigned int i = 0; i < _ikIterationCount; i++)
	{
		SolveCore(i);

		XMVECTOR targetPosition = _targetNode->GetGlobalTransform().r[3];
		XMVECTOR ikPosition = _ikNode->GetGlobalTransform().r[3];
		float dist = XMVector3Length(targetPosition - ikPosition).m128_f32[0];

		if (dist < maxDistance)
		{
			maxDistance = dist;
			for (IKChain& chain : _ikChains)
			{
				XMStoreFloat4(&chain.saveIKRotation, XMQuaternionRotationMatrix(chain.boneNode->GetIKRotation()));
			}
		}
		else
		{
			for (IKChain& chain : _ikChains)
			{
				chain.boneNode->SetIKRotation(XMMatrixRotationQuaternion(XMLoadFloat4(&chain.saveIKRotation)));
				chain.boneNode->UpdateLocalTransform();
				chain.boneNode->UpdateGlobalTransform();
			}
			break;
		}
	}
}

for (IKChain& chain : _ikChains)
{
	chain.prevAngle = DirectX::XMFLOAT3(0.f, 0.f, 0.f);
	chain.boneNode->SetIKRotation(XMMatrixIdentity());
	chain.planeModeAngle = 0.f;

	chain.boneNode->UpdateLocalTransform();
	chain.boneNode->UpdateGlobalTransform();
}

まず、IK回転値を初期値に設定し、ボーンを更新します。

float maxDistance = std::numeric_limits<float>::max();
for (unsigned int i = 0; i < _ikIterationCount; i++)
{
	SolveCore(i);

	XMVECTOR targetPosition = _targetNode->GetGlobalTransform().r[3];
	XMVECTOR ikPosition = _ikNode->GetGlobalTransform().r[3];
	float dist = XMVector3Length(targetPosition - ikPosition).m128_f32[0];

	if (dist < maxDistance)
	{
		maxDistance = dist;
		for (IKChain& chain : _ikChains)
		{
			XMStoreFloat4(&chain.saveIKRotation, XMQuaternionRotationMatrix(chain.boneNode->GetIKRotation()));
		}
	}
	else
	{
		for (IKChain& chain : _ikChains)
		{
			chain.boneNode->SetIKRotation(XMMatrixRotationQuaternion(XMLoadFloat4(&chain.saveIKRotation)));
			chain.boneNode->UpdateLocalTransform();
			chain.boneNode->UpdateGlobalTransform();
		}
		break;
	}
}

CCDIKの内容を思い出してみましょう。

チェーンの回転がすべて終わり、ターゲットボーンとルートボーンの距離が前回よりも小さくならなければ、反復を終了してIK回転を適用すればいいんです。

SolveCore はまだ作成していませんが、このメソッドで連結されているチェーンボーンの回転が行われます。

SolveCore が完了すると、ターゲットボーンとルートボーンの距離を求め、maxDistance と比較して前回の距離値と変わったかどうかを比較します。

変わっていれば、まだ IK 演算を進める必要があるため、IKChain の saveIKRotation に現在の IK 回転値を保存します。

変わっていなければ、IK 演算を終了します。

すべてのチェーンボーンを巡回しながら、saveIKRotation に保存していた値をボーンに適用し、更新します。

簡単にもう一度並べてみましょうか。

ターゲットボーンとルートボーンの距離を計算
前回の距離と比較
変化があれば結果を保存し、変化がなければボーンに回転を適用して反復を終了
上記の内容を繰り返す

SolveCore

チェーンボーンのIK回転を計算するメソッドです。

Solveと比較するとより複雑です。

void IKSolver::SolveCore(unsigned iteration)
{
	XMVECTOR ikPosition = _ikNode->GetGlobalTransform().r[3];
	for (unsigned int chainIndex = 0; chainIndex < _ikChains.size(); chainIndex++)
	{
		IKChain& chain = _ikChains[chainIndex];
		BoneNode* chainNode = chain.boneNode;
		if (chainNode == nullptr)
		{
			continue;
		}

		if (chain.enableAxisLimit == true)
		{
			if ((chain.limitMin.x != 0 || chain.limitMax.x != 0) &&
				(chain.limitMin.y == 0 || chain.limitMax.y == 0) &&
				(chain.limitMin.z == 0 || chain.limitMax.z == 0))
			{
				SolvePlane(iteration, chainIndex, SolveAxis::X);
				continue;
			}
			else if ((chain.limitMin.y != 0 || chain.limitMax.y != 0) &&
				(chain.limitMin.x == 0 || chain.limitMax.x == 0) &&
				(chain.limitMin.z == 0 || chain.limitMax.z == 0))
			{
				SolvePlane(iteration, chainIndex, SolveAxis::Y);
				continue;
			}
			else if ((chain.limitMin.z != 0 || chain.limitMax.z != 0) &&
				(chain.limitMin.x == 0 || chain.limitMax.x == 0) &&
				(chain.limitMin.y == 0 || chain.limitMax.y == 0))
			{
				SolvePlane(iteration, chainIndex, SolveAxis::Z);
				continue;
			}
		}

		XMVECTOR targetPosition = _targetNode->GetGlobalTransform().r[3];

		XMVECTOR det = XMMatrixDeterminant(chain.boneNode->GetGlobalTransform());
		XMMATRIX inverseChain = XMMatrixInverse(&det, chain.boneNode->GetGlobalTransform());

		XMVECTOR chainIKPosition = XMVector3Transform(ikPosition, inverseChain);
		XMVECTOR chainTargetPosition = XMVector3Transform(targetPosition, inverseChain);

		XMVECTOR chainIKVector = XMVector3Normalize(chainIKPosition);
		XMVECTOR chainTargetVector = XMVector3Normalize(chainTargetPosition);

		float dot = XMVector3Dot(chainTargetVector, chainIKVector).m128_f32[0];
		dot = MathUtil::Clamp(dot, -1.f, 1.f);

		float angle = std::acos(dot);
		float angleDegree = XMConvertToDegrees(angle);
		if (angleDegree < 1.0e-3f)
		{
			continue;
		}

		angle = MathUtil::Clamp(angle, -_ikLimitAngle, _ikLimitAngle);
		XMVECTOR cross = XMVector3Normalize(XMVector3Cross(chainTargetVector, chainIKVector));
		XMMATRIX rotation = XMMatrixRotationAxis(cross, angle);

		XMMATRIX chainRotation = rotation * chainNode->GetAnimateRotation() * chainNode->GetIKRotation();
		if (chain.enableAxisLimit == true)
		{
			XMFLOAT3 rotXYZ = Decompose(chainRotation, chain.prevAngle);

			XMFLOAT3 clampXYZ = MathUtil::Clamp(rotXYZ, chain.limitMin, chain.limitMax);
			float invLimitAngle = -_ikLimitAngle;
			clampXYZ = MathUtil::Clamp(MathUtil::Sub(clampXYZ, chain.prevAngle), invLimitAngle, _ikLimitAngle);
			clampXYZ = MathUtil::Add(clampXYZ, chain.prevAngle);

			chainRotation = XMMatrixRotationRollPitchYaw(clampXYZ.x, clampXYZ.y, clampXYZ.z);
			chain.prevAngle = clampXYZ;
		}

		XMVECTOR det1 = XMMatrixDeterminant(chain.boneNode->GetAnimateRotation());
		XMMATRIX inverseAnimate = XMMatrixInverse(&det1, chain.boneNode->GetAnimateRotation());

		XMMATRIX ikRotation = inverseAnimate * chainRotation;
		chain.boneNode->SetIKRotation(ikRotation);

		chain.boneNode->UpdateLocalTransform();
		chain.boneNode->UpdateGlobalTransform();
	}
}

	XMVECTOR ikPosition = _ikNode->GetGlobalTransform().r[3];
	for (unsigned int chainIndex = 0; chainIndex < _ikChains.size(); chainIndex++)
	{
		IKChain& chain = _ikChains[chainIndex];
		BoneNode* chainNode = chain.boneNode;
		if (chainNode == nullptr)
		{
			continue;
		}

ルートボーンの位置を取得します。

そして、すべてのチェーンボーンを巡回します。

if (chain.enableAxisLimit == true)
{
			if ((chain.limitMin.x != 0 || chain.limitMax.x != 0) &&
				(chain.limitMin.y == 0 || chain.limitMax.y == 0) &&
				(chain.limitMin.z == 0 || chain.limitMax.z == 0))
			{
				SolvePlane(iteration, chainIndex, SolveAxis::X);
				continue;
			}
			else if ((chain.limitMin.y != 0 || chain.limitMax.y != 0) &&
				(chain.limitMin.x == 0 || chain.limitMax.x == 0) &&
				(chain.limitMin.z == 0 || chain.limitMax.z == 0))
			{
				SolvePlane(iteration, chainIndex, SolveAxis::Y);
				continue;
			}
			else if ((chain.limitMin.z != 0 || chain.limitMax.z != 0) &&
				(chain.limitMin.x == 0 || chain.limitMax.x == 0) &&
				(chain.limitMin.y == 0 || chain.limitMax.y == 0))
			{
				SolvePlane(iteration, chainIndex, SolveAxis::Z);
				continue;
			}
}

enableAxisLimitは特定の軸を基準に回転を制限するかどうかを示します。

limitMinとlimitMaxからどの軸を使用するか推測できます。

SolvePlaneは特定の軸でチェーンボーンの回転を計算します。

XMVECTOR targetPosition = _targetNode->GetGlobalTransform().r[3];

XMVECTOR det = XMMatrixDeterminant(chain.boneNode->GetGlobalTransform());
XMMATRIX inverseChain = XMMatrixInverse(&det, chain.boneNode->GetGlobalTransform());

XMVECTOR chainIKPosition = XMVector3Transform(ikPosition, inverseChain);
XMVECTOR chainTargetPosition = XMVector3Transform(targetPosition, inverseChain);

ターゲットボーンの位置、ルートボーンの位置をチェーンボーンの座標系に変換するために、ボーンの変換行列の逆行列を計算します。

そして、この行列を掛けて両方の位置をボーンの座標系に変換します。

XMVECTOR chainIKVector = XMVector3Normalize(chainIKPosition);
XMVECTOR chainTargetVector = XMVector3Normalize(chainTargetPosition);

float dot = XMVector3Dot(chainTargetVector, chainIKVector).m128_f32[0];
dot = MathUtil::Clamp(dot, -1.f, 1.f);

float angle = std::acos(dot);
float angleDegree = XMConvertToDegrees(angle);
if (angleDegree < 1.0e-3f)
{
	continue;
}

angle = MathUtil::Clamp(angle, -_ikLimitAngle, _ikLimitAngle);
XMVECTOR cross = XMVector3Normalize(XMVector3Cross(chainTargetVector, chainIKVector));
XMMATRIX rotation = XMMatrixRotationAxis(cross, angle);

両方とも骨の座標系における位置なので、両方とも原点が同じベクトルとして見なすことができます。

正規化して内積すると、二つのベクトルのコサイン値を得ることができます。

アークコサイン関数で二つのベクトルの角度を求めます。

_ikLimitAngleで制限された角度を超えないようにし、

2つのベクトルの外積を計算して回転軸を求めます。

回転させる軸と角度があれば、XMMatrixRotationAxisで回転行列を生成できます。

XMMATRIX chainRotation = rotation * chainNode->GetAnimateRotation() * chainNode->GetIKRotation();

チェーンボーンの最終的な回転を求める必要があるため、アニメーションの回転行列と既に適用されている可能性のあるIK回転行列を取得し、すべて掛け合わせます。

	if (chain.enableAxisLimit == true)
	{
		XMFLOAT3 rotXYZ = Decompose(chainRotation, chain.prevAngle);

		XMFLOAT3 clampXYZ = MathUtil::Clamp(rotXYZ, chain.limitMin, chain.limitMax);
		float invLimitAngle = -_ikLimitAngle;
		clampXYZ = MathUtil::Clamp(MathUtil::Sub(clampXYZ, chain.prevAngle), invLimitAngle, _ikLimitAngle);
		clampXYZ = MathUtil::Add(clampXYZ, chain.prevAngle);

		chainRotation = XMMatrixRotationRollPitchYaw(clampXYZ.x, clampXYZ.y, clampXYZ.z);
		chain.prevAngle = clampXYZ;
	}

上記でenableAxisLimitがtrueであるがSolvePlaneで処理されなかった場合、この部分で回転制限を適用します。

Decomposeは回転行列の各軸の回転値のみを抽出する関数です。

コードの解説は特にしません。

XMFLOAT3 IKSolver::Decompose(const XMMATRIX& m, const XMFLOAT3& before)
{
	XMFLOAT3 r;
	float sy = -m.r[0].m128_f32[2];
	const float e = 1.0e-6f;

	if ((1.0f - std::abs(sy)) < e)
	{
		r.y = std::asin(sy);
		float sx = std::sin(before.x);
		float sz = std::sin(before.z);
		if (std::abs(sx) < std::abs(sz))
		{
			float cx = std::cos(before.x);
			if (cx > 0)
			{
				r.x = 0;
				r.z = std::asin(-m.r[1].m128_f32[0]);
			}
			else
			{
				r.x = XM_PI;
				r.z = std::asin(m.r[1].m128_f32[0]);
			}
		}
		else
		{
			float cz = std::cos(before.z);
			if (cz > 0)
			{
				r.z = 0;
				r.x = std::asin(-m.r[2].m128_f32[1]);
			}
			else
			{
				r.z = XM_PI;
				r.x = std::asin(m.r[2].m128_f32[1]);
			}
		}
	}
	else
	{
		r.x = std::atan2(m.r[1].m128_f32[2], m.r[2].m128_f32[2]);
		r.y = std::asin(-m.r[0].m128_f32[2]);
		r.z = std::atan2(m.r[0].m128_f32[1], m.r[0].m128_f32[0]);
	}

	const float pi = XM_PI;
	XMFLOAT3 tests[] =
	{
		{ r.x + pi, pi - r.y, r.z + pi },
		{ r.x + pi, pi - r.y, r.z - pi },
		{ r.x + pi, -pi - r.y, r.z + pi },
		{ r.x + pi, -pi - r.y, r.z - pi },
		{ r.x - pi, pi - r.y, r.z + pi },
		{ r.x - pi, pi - r.y, r.z - pi },
		{ r.x - pi, -pi - r.y, r.z + pi },
		{ r.x - pi, -pi - r.y, r.z - pi },
	};

	float errX = std::abs(DiffAngle(r.x, before.x));
	float errY = std::abs(DiffAngle(r.y, before.y));
	float errZ = std::abs(DiffAngle(r.z, before.z));
	float minErr = errX + errY + errZ;
	for (const auto test : tests)
	{
		float err = std::abs(DiffAngle(test.x, before.x))
			+ std::abs(DiffAngle(test.y, before.y))
			+ std::abs(DiffAngle(test.z, before.z));
		if (err < minErr)
		{
			minErr = err;
			r = test;
		}
	}
	return r;
}

float IKSolver::NormalizeAngle(float angle)
{
	float ret = angle;
	while (ret >= XM_2PI)
	{
		ret -= XM_2PI;
	}
	while (ret < 0)
	{
		ret += XM_2PI;
	}

	return ret;
}

float IKSolver::DiffAngle(float a, float b)
{
	float diff = NormalizeAngle(a) - NormalizeAngle(b);
	if (diff > XM_PI)
	{
		return diff - XM_2PI;
	}
	else if (diff < -XM_PI)
	{
		return diff + XM_2PI;
	}
	return diff;
}

SolveCore に戻りましょう。

XMVECTOR det1 = XMMatrixDeterminant(chain.boneNode->GetAnimateRotation());
XMMATRIX inverseAnimate = XMMatrixInverse(&det1, chain.boneNode->GetAnimateRotation());

XMMATRIX ikRotation = inverseAnimate * chainRotation;
chain.boneNode->SetIKRotation(ikRotation);

chain.boneNode->UpdateLocalTransform();
chain.boneNode->UpdateGlobalTransform();

ボーンのアニメーション回転行列を取得し、その逆行列を計算します。

そして、チェーンボーンの回転と掛け合わせた結果を最終的にボーンに設定します。

この方法を取る理由は、BoneNodeのUpdateLocalTransformでアニメーションの回転に関する処理が行われるため、以前に乗算しておいたアニメーション回転行列の変換をキャンセルするためです。

ここまでで、チェーンボーンに対するIK回転の計算が完了します。

もちろん、これ1回で終わるわけではなく、SolveでSolveCoreが目標を達成するまで何度も呼び出されることになります。

今回は先ほど過ごしたSolvePlaneを実装します。

SolvePlane

enum class SolveAxis
{
	X,
	Y,
	Z
};

void IKSolver::SolvePlane(unsigned iteration, unsigned chainIndex, SolveAxis solveAxis)
{
	XMFLOAT3 rotateAxis;
	float limitMinAngle;
	float limitMaxAngle;

	IKChain& chain = _ikChains[chainIndex];

	switch (solveAxis)
	{
	case SolveAxis::X:
		limitMinAngle = chain.limitMin.x;
		limitMaxAngle = chain.limitMax.x;
		rotateAxis = XMFLOAT3(1.f, 0.f, 0.f);
		break;
	case SolveAxis::Y:
		limitMinAngle = chain.limitMin.y;
		limitMaxAngle = chain.limitMax.y;
		rotateAxis = XMFLOAT3(0.f, 1.f, 0.f);
		break;
	case SolveAxis::Z:
		limitMinAngle = chain.limitMin.z;
		limitMaxAngle = chain.limitMax.z;
		rotateAxis = XMFLOAT3(0.f, 0.f, 1.f);
		break;
	}

	XMVECTOR ikPosition = _ikNode->GetGlobalTransform().r[3];
	XMVECTOR targetPosision = _targetNode->GetGlobalTransform().r[3];

	XMVECTOR det = XMMatrixDeterminant(chain.boneNode->GetGlobalTransform());
	XMMATRIX inverseChain = XMMatrixInverse(&det, chain.boneNode->GetGlobalTransform());

	XMVECTOR chainIKPosition = XMVector3Transform(ikPosition, inverseChain);
	XMVECTOR chainTargetPosition = XMVector3Transform(targetPosision, inverseChain);

	XMVECTOR chainIKVector = XMVector3Normalize(chainIKPosition);
	XMVECTOR chainTargetVector = XMVector3Normalize(chainTargetPosition);

	float dot = XMVector3Dot(chainTargetVector, chainIKVector).m128_f32[0];
	dot = MathUtil::Clamp(dot, -1.f, 1.f);

	float angle = std::acos(dot);
	angle = MathUtil::Clamp(angle, -_ikLimitAngle, _ikLimitAngle);

	XMVECTOR rotation1 = XMQuaternionRotationAxis(XMLoadFloat3(&rotateAxis), angle);
	XMVECTOR targetVector1 = XMVector3Rotate(chainTargetVector, rotation1);
	float dot1 = XMVector3Dot(targetVector1, chainIKVector).m128_f32[0];

	XMVECTOR rotation2 = XMQuaternionRotationAxis(XMLoadFloat3(&rotateAxis), -angle);
	XMVECTOR targetVector2 = XMVector3Rotate(chainTargetVector, rotation2);
	float dot2 = XMVector3Dot(targetVector2, chainIKVector).m128_f32[0];

	float newAngle = chain.planeModeAngle;
	if (dot1 > dot2)
	{
		newAngle += angle;
	}
	else
	{
		newAngle -= angle;
	}

	if (iteration == 0)
	{
		if (newAngle < limitMinAngle || newAngle > limitMaxAngle)
		{
			if (-newAngle > limitMinAngle && -newAngle < limitMaxAngle)
			{
				newAngle *= -1;
			}
			else
			{
				float halfRadian = (limitMinAngle + limitMaxAngle) * 0.5f;
				if (abs(halfRadian - newAngle) > abs(halfRadian + newAngle))
				{
					newAngle *= -1;
				}
			}
		}
	}

	newAngle = MathUtil::Clamp(newAngle, limitMinAngle, limitMaxAngle);
	chain.planeModeAngle = newAngle;

	XMVECTOR det1 = XMMatrixDeterminant(chain.boneNode->GetAnimateRotation());
	XMMATRIX inverseAnimate = XMMatrixInverse(&det1, chain.boneNode->GetAnimateRotation());

	XMMATRIX ikRotation = inverseAnimate * XMMatrixRotationAxis(XMLoadFloat3(&rotateAxis), newAngle);

	chain.boneNode->SetIKRotation(ikRotation);

	chain.boneNode->UpdateLocalTransform();
	chain.boneNode->UpdateGlobalTransform();
}

  switch (solveAxis)
	{
	case SolveAxis::X:
		limitMinAngle = chain.limitMin.x;
		limitMaxAngle = chain.limitMax.x;
		rotateAxis = XMFLOAT3(1.f, 0.f, 0.f);
		break;
	case SolveAxis::Y:
		limitMinAngle = chain.limitMin.y;
		limitMaxAngle = chain.limitMax.y;
		rotateAxis = XMFLOAT3(0.f, 1.f, 0.f);
		break;
	case SolveAxis::Z:
		limitMinAngle = chain.limitMin.z;
		limitMaxAngle = chain.limitMax.z;
		rotateAxis = XMFLOAT3(0.f, 0.f, 1.f);
		break;
	}

回転軸がすでに決まっているため、パラメータとして渡されたSolveAxis列挙体に基づいて回転軸と制限角度を初期化します。

この後はSolveCoreと変わりませんが、回転を正と負の両方向で計算しています。

float angle = std::acos(dot);
angle = MathUtil::Clamp(angle, -_ikLimitAngle, _ikLimitAngle);

XMVECTOR rotation1 = XMQuaternionRotationAxis(XMLoadFloat3(&rotateAxis), angle);
XMVECTOR targetVector1 = XMVector3Rotate(chainTargetVector, rotation1);
float dot1 = XMVector3Dot(targetVector1, chainIKVector).m128_f32[0];

XMVECTOR rotation2 = XMQuaternionRotationAxis(XMLoadFloat3(&rotateAxis), -angle);
XMVECTOR targetVector2 = XMVector3Rotate(chainTargetVector, rotation2);
float dot2 = XMVector3Dot(targetVector2, chainIKVector).m128_f32[0];

両方向の回転に対するコサイン値を求める理由は、目標に到達する際に可能な限り最小限の回転を適用するためです。

つまり、二つの回転方向のうち、より少ない回転で済む方を選択するのです。

float newAngle = chain.planeModeAngle;
if (dot1 > dot2)
{
	newAngle += angle;
}
else
{
	newAngle -= angle;
}

内積の値がより大きい方が回転角度が小さいため、より大きい方の結果を加算します。

	if (iteration == 0)
	{
		if (newAngle < limitMinAngle || newAngle > limitMaxAngle)
		{
			if (-newAngle > limitMinAngle && -newAngle < limitMaxAngle)
			{
				newAngle *= -1;
			}
			else
			{
				float halfRadian = (limitMinAngle + limitMaxAngle) * 0.5f;
				if (abs(halfRadian - newAngle) > abs(halfRadian + newAngle))
				{
					newAngle *= -1;
				}
			}
		}
	}

初回だけ、最大回転角度の処理を行います。

回転角度が制限角度を超える可能性がある場合、符号を反転させることで制限範囲内に収めることができるようにします。

反転させても制限角度を超える場合は、現在の角度と中間角度を比較して符号反転の可否を判断します。

この後の処理はSolveCoreと変わりません。

IKSolverの準備が完了しました。BoneNodeで使用するように追加してみましょう。

IKSolver* _ikSolver = nullptr;

BoneNodeにメンバーを追加してください。

IKSolver* GetIKSolver() const { return _ikSolver; }
void SetIKSolver(IKSolver* ikSolver);

IKSolverのゲッターとセッターを追加します。

void BoneNode::AnimateIK(unsigned frameNo)
{
	if (_motionKeys.size() <= 0 || _ikSolver == nullptr)
	{
		return;
	}

	auto rit = std::find_if(_ikKeys.rbegin(), _ikKeys.rend(),
		[frameNo](const VMDIKkey& key)
		{
			return key.frameNo <= frameNo;
		});

	if (rit == _ikKeys.rend())
	{
		return;
	}

	_ikSolver->SetEnable(rit->enable);
}

現在のフレームのVMDIKkeyを設定するメソッドを追加します。

アニメーションのキーフレームとは異なり、有効化の状態のみを使用するため、補間は全く必要ありません。

void BoneNode::UpdateLocalTransform()
{
	XMMATRIX scale = XMMatrixIdentity();

	XMMATRIX rotation = _animateRotation;
	if (_enableIK == true)
	{
		rotation = rotation * _ikRotation;
	}

	XMVECTOR t = XMLoadFloat3(&_animatePosition) + XMLoadFloat3(&_position)

	XMMATRIX translate = XMMatrixTranslationFromVector(t);

	_localTransform = scale * rotation * translate;
}

_enableIKがtrueの場合、_ikRotationが適用されるように追加します。

BoneNodeでのIK関連の内容が準備完了しました。

次は、IKSolverを生成してBoneNodeに設定し、IKが使用されるようにNodeManagerを修正します。

NodeManager の修正

std::vector<IKSolver*> _ikSolvers;

IKSolver ポインタのリストをメンバーとして追加します。

void NodeManager::Init(const std::vector<PMXBone>& bones)
{
	_boneNodeByIdx.resize(bones.size());
	_sortedNodes.resize(bones.size());

	for (int index = 0; index < bones.size(); index++)
	{
		const PMXBone& currentBoneData = bones[index];
		_boneNodeByIdx[index] = new BoneNode(index, currentBoneData);
		_boneNodeByName[_boneNodeByIdx[index]->GetName()] = _boneNodeByIdx[index];
		_sortedNodes[index] = _boneNodeByIdx[index];
	}

	for (int index = 0; index < _boneNodeByIdx.size(); index++)
	{
		//Parent Bone Set
		BoneNode* currentBoneNode = _boneNodeByIdx[index];

		unsigned int parentBoneIndex = currentBoneNode->GetParentBoneIndex();
		if (parentBoneIndex != 65535 && _boneNodeByIdx.size() > parentBoneIndex)
		{
			currentBoneNode->SetParentBoneNode(_boneNodeByIdx[currentBoneNode->GetParentBoneIndex()]);
		}

		const PMXBone& currentPmxBone = bones[index];

		//IK Solver Setting
		if (((uint16_t)currentPmxBone.boneFlag & (uint16_t)PMXBoneFlags::IK) && currentPmxBone.ikTargetBoneIndex < _boneNodeByIdx.size())
		{
			BoneNode* targetNode = _boneNodeByIdx[currentPmxBone.ikTargetBoneIndex];
			unsigned int iterationCount = currentPmxBone.ikIterationCount;
			float limitAngle = currentPmxBone.ikLimit;

			_ikSolvers.push_back(new IKSolver(currentBoneNode, targetNode, iterationCount, limitAngle));

			IKSolver* solver = _ikSolvers[_ikSolvers.size() - 1];

			for (const PMXIKLink& ikLink : currentPmxBone.ikLinks)
			{
				if (ikLink.ikBoneIndex < 0 || ikLink.ikBoneIndex >= _boneNodeByIdx.size())
				{
					continue;
				}

				BoneNode* linkNode = _boneNodeByIdx[ikLink.ikBoneIndex];
				if (ikLink.enableLimit == true)
				{
					solver->AddIKChain(linkNode, ikLink.enableLimit, ikLink.limitMin, ikLink.limitMax);
				}
				else
				{
					solver->AddIKChain(
						linkNode,
						ikLink.enableLimit,
						XMFLOAT3(0.5f, 0.f, 0.f),
						XMFLOAT3(180.f, 0.f, 0.f));
				}
				linkNode->SetIKEnable(true);
			}
			currentBoneNode->SetIKSolver(solver);
		}
	}

	for (int index = 0; index < _boneNodeByIdx.size(); index++)
	{
		BoneNode* currentBoneNode = _boneNodeByIdx[index];

		if (currentBoneNode->GetParentBoneNode() == nullptr)
		{
			continue;
		}

		XMVECTOR pos = XMLoadFloat3(&bones[currentBoneNode->GetBoneIndex()].position);
		XMVECTOR parentPos = XMLoadFloat3(&bones[currentBoneNode->GetParentBoneIndex()].position);

		XMFLOAT3 resultPos;
		XMStoreFloat3(&resultPos, pos - parentPos);

		currentBoneNode->SetPosition(resultPos);
	}

	std::stable_sort(_sortedNodes.begin(), _sortedNodes.end(), 
		[](const BoneNode* left, const BoneNode * right)
	{
			return left->GetDeformDepth() < right->GetDeformDepth();
	});
}

IK関連の処理がInitメソッドに追加されました。

if (((uint16_t)currentPmxBone.boneFlag & (uint16_t)PMXBoneFlags::IK) && currentPmxBone.ikTargetBoneIndex < _boneNodeByIdx.size())
{

ボーンフラグでIKを使用しているかチェックします。

BoneNode* targetNode = _boneNodeByIdx[currentPmxBone.ikTargetBoneIndex];
unsigned int iterationCount = currentPmxBone.ikIterationCount;
float limitAngle = currentPmxBone.ikLimit;

_ikSolvers.push_back(new IKSolver(currentBoneNode, targetNode, iterationCount, limitAngle));

PMXBoneからIK計算の繰り返し回数、制限角度を取得できます。

ルートボーンは現在のボーンで、ターゲットボーンはPMXBoneからインデックスを取得できます。

これらを使ってIKSolverオブジェクトを生成します。

IKSolver* solver = _ikSolvers[_ikSolvers.size() - 1];

for (const PMXIKLink& ikLink : currentPmxBone.ikLinks)
{
		if (ikLink.ikBoneIndex < 0 || ikLink.ikBoneIndex >= _boneNodeByIdx.size())
		{
			continue;
		}

		BoneNode* linkNode = _boneNodeByIdx[ikLink.ikBoneIndex];
		if (ikLink.enableLimit == true)
		{
			solver->AddIKChain(linkNode, ikLink.enableLimit, ikLink.limitMin, ikLink.limitMax);
		}
		else
		{
			solver->AddIKChain(
				linkNode,
				ikLink.enableLimit,
				XMFLOAT3(0.5f, 0.f, 0.f),
				XMFLOAT3(180.f, 0.f, 0.f));
		}
		linkNode->SetIKEnable(true);
}
currentBoneNode->SetIKSolver(solver);

PMXBoneからPMXIKLinkのリストを取得できます。

PMXIKLinkを使用してIKChainを生成することになります。

PMXIKLinkからチェーンボーンとして使用されるボーンのインデックスを取得し、ボーンを探します。

角度制限に応じてAddIKChainを呼び出すと、IKSolver内部でIKChainオブジェクトが生成され、追加されます。

IKSolverにすべてのチェーンボーンが設定されたら、現在のボーンにIKSolverを設定します。

void NodeManager::UpdateAnimation(unsigned int frameNo)
{
	for (BoneNode* curNode : _boneNodeByIdx)
	{
		curNode->AnimateMotion(frameNo);
		curNode->AnimateIK(frameNo);
		curNode->UpdateLocalTransform();
	}

	if (_boneNodeByIdx.size() > 0)
	{
		_boneNodeByIdx[0]->UpdateGlobalTransform();
	}

	for (BoneNode* curNode : _sortedNodes)
	{
		IKSolver* curSolver = curNode->GetIKSolver();
		if (curSolver != nullptr)
		{
			curSolver->Solve();
			curNode->UpdateGlobalTransform();
		}
	}
}

すべてのボーンのAnimateIKを呼び出し、IKSolverのSolveも呼び出してIKを使用するようにします。

以前に_deformDepthの値で並べ替えた_sortedNodesをここで使用することになりますね。

ボーンはツリー構造であるため、IK演算を行って_boneNodeByIdxの順序で更新してはいけません。

正しい更新順序はすでにPMXファイルに_deformDepthとして持っていますね。

_sortedNodesで巡回しながらIK演算とボーンの更新を進めれば問題ありません。

ビルドして結果を見てみましょう。

腕が正常に動くようになりましたね。

おそらくこのモデルは手だけにアニメーションキーフレームを設定し、腕や肘は手に従ってIKで動くように制作されたものだと思います。

あしがまだ正しく動いていないようですね。

これを解決するためにはAppend変換を実装する必要があります。

Append 変換

Append変換はPMXモデルのアニメーション変換方法の一種です。

あるボーンが他のボーンの変換（移動および回転）の影響を受けて自身を変換する方法です。

もっと簡単に言えば、自分一人で動くのではなく、他のボーンの動きによって動くということです。

ミクさんのあしにあるボーンのほとんどがAppend変換を必要とするため、動いていないのでしょう。

それでは、BoneNodeでAppend変換ができるように修正しましょう。

	void SetEnableAppendRotate(bool enable) { _isAppendRotate = enable; }
	void SetEnableAppendTranslate(bool enable) { _isAppendTranslate = enable; }
	void SetEnableAppendLocal(bool enable) { _isAppendLocal = enable; }
	void SetAppendWeight(float weight) { _appendWeight = weight; }
	float GetAppendWeight() const { return _appendWeight; }
	void SetAppendBoneNode(BoneNode* node) { _appendBoneNode = node; }
	BoneNode* GetAppendBoneNode() const { return _appendBoneNode; }
	const XMMATRIX& GetAppendRotation() const { return _appendRotation; }
	const XMFLOAT3& GetAppendTranslate() const { return _appendTranslate; }

Append変換に必要なパラメータを設定したり取得したりするメソッドはすでに作成済みでしたね。

GetAppendBoneNodeでAppend変換における親ボーンを設定できます。

少し後で NodeManager でこれらのメソッドを使用して初期化できるように追加します。

今回はAppend変換を行うメソッドを追加します。

void BoneNode::UpdateAppendTransform()
{
	if (_appendBoneNode == nullptr)
	{
		return;
	}

	XMMATRIX appendRotation;
	if (_isAppendRotate == true)
	{
		if (_isAppendLocal == true)
		{
			appendRotation = _appendBoneNode->GetAnimateRotation();
		}
		else
		{
			if (_appendBoneNode->GetAppendBoneNode() == nullptr)
			{
				appendRotation = _appendBoneNode->GetAnimateRotation();
			}
			else
			{
				appendRotation = _appendBoneNode->GetAppendRotation();
			}
		}

		if (_appendBoneNode->GetIKEnable() == true)
		{
			appendRotation = appendRotation * _appendBoneNode->GetIKRotation();
		}

		XMVECTOR appendRotationQuaternion = XMQuaternionRotationMatrix(appendRotation);
		appendRotationQuaternion = XMQuaternionSlerp(XMQuaternionIdentity(), appendRotationQuaternion, _appendWeight);

		_appendRotation = XMMatrixRotationQuaternion(appendRotationQuaternion);
	}

	XMVECTOR appendTranslate = XMVectorZero();
	if (_isAppendTranslate == true)
	{
		if (_isAppendLocal == true)
		{
			appendTranslate = XMLoadFloat3(&_appendBoneNode->GetAnimatePosition());
		}
		else
		{
			if (_appendBoneNode->GetAppendBoneNode() == nullptr)
			{
				appendTranslate = XMLoadFloat3(&_appendBoneNode->GetAnimatePosition());
			}
			else
			{
				appendTranslate = XMLoadFloat3(&_appendBoneNode->GetAppendTranslate());
			}
		}

		XMStoreFloat3(&_appendTranslate, appendTranslate);
	}

	UpdateLocalTransform();
}

回転を処理する部分だけ抜き出して見てみましょう。

if (_isAppendRotate == true)
	{
		if (_isAppendLocal == true)
		{
			appendRotation = _appendBoneNode->GetAnimateRotation();
		}
		else
		{
			if (_appendBoneNode->GetAppendBoneNode() == nullptr)
			{
				appendRotation = _appendBoneNode->GetAnimateRotation();
			}
			else
			{
				appendRotation = _appendBoneNode->GetAppendRotation();
			}
		}

		if (_appendBoneNode->GetIKEnable() == true)
		{
			appendRotation = appendRotation * _appendBoneNode->GetIKRotation();
		}

		XMVECTOR appendRotationQuaternion = XMQuaternionRotationMatrix(appendRotation);
		appendRotationQuaternion = XMQuaternionSlerp(XMQuaternionIdentity(), appendRotationQuaternion, _appendWeight);

		_appendRotation = XMMatrixRotationQuaternion(appendRotationQuaternion);
	}

_isAppendLocalによって、親ボーンのローカル座標系の回転を継承するか、グローバル座標系の回転を継承するかが決定されます。

ローカル座標系の回転を継承する場合、親ボーンのアニメーション回転を取得してAppend回転として使用します。

GetAnimateRotationで取得する変換行列はボーンのローカル座標系の行列です。

グローバル座標系の回転を継承する場合、親ボーンがさらに上位の親ボーンを持っているかどうかを確認する必要があります。

親ボーンが親ボーンを持っていない場合、ローカル座標系の回転を継承するのと変わりません。

親ボーンを持っているボーンの場合、その親ボーンの回転の影響も受ける必要があるため、グローバル座標系の回転を取得します。

現在のボーンの親ボーンであるため、すでにAppend回転として計算が完了しています。

親ボーンがIKを使用しているボーンの場合、IK回転も適用します。

最後に、PMXボーンは回転に対するウエート値も持っているため、ウエート値を使用して球面線形補間も適用します。

回転の次は移動を処理しますが、全体的なロジックは回転と変わりません。

ウエート値に対する補間やIKに関する処理だけがないですね。

すべてのAppend変換の計算が完了したら、ボーンのローカル変換行列を更新します。

Append変換が変換行列に適用されるように、UpdateLocalTransformメソッドを修正しましょう。

void BoneNode::UpdateLocalTransform()
{
	XMMATRIX scale = XMMatrixIdentity();

	XMMATRIX rotation = _animateRotation;
	if (_enableIK == true)
	{
		rotation = rotation * _ikRotation;
	}

	if (_isAppendRotate == true)
	{
		rotation = rotation * _appendRotation;
	}

	XMVECTOR t = XMLoadFloat3(&_animatePosition) + XMLoadFloat3(&_position);
	if (_isAppendTranslate == true)
	{
		t += XMLoadFloat3(&_appendTranslate);
	}

	XMMATRIX translate = XMMatrixTranslationFromVector(t);

	_localTransform = scale * rotation * translate;
}

Append変換に関するフラグがtrueの場合、回転や行列に適用されるように修正します。

NodeManagerでAppend変換を呼び出すように修正します。

void NodeManager::UpdateAnimation(unsigned int frameNo)
{
	for (BoneNode* curNode : _boneNodeByIdx)
	{
		curNode->AnimateMotion(frameNo);
		curNode->AnimateIK(frameNo);
		curNode->UpdateLocalTransform();
	}

	if (_boneNodeByIdx.size() > 0)
	{
		_boneNodeByIdx[0]->UpdateGlobalTransform();
	}

	for (BoneNode* curNode : _sortedNodes)
	{
		if (curNode->GetAppendBoneNode() != nullptr)
		{
			curNode->UpdateAppendTransform();
			curNode->UpdateGlobalTransform();
		}

		IKSolver* curSolver = curNode->GetIKSolver();
		if (curSolver != nullptr)
		{
			curSolver->Solve();
			curNode->UpdateGlobalTransform();
		}
	}
}

NodeManagerでAppend変換の設定を初期化する部分も追加しましょう。

Initメソッドで親ボーンと子ボーンを接続する部分の下に追加します。

//Parent Bone Set
BoneNode* currentBoneNode = _boneNodeByIdx[index];

unsigned int parentBoneIndex = currentBoneNode->GetParentBoneIndex();
if (parentBoneIndex != 65535 && _boneNodeByIdx.size() > parentBoneIndex)
{
	currentBoneNode->SetParentBoneNode(_boneNodeByIdx[currentBoneNode->GetParentBoneIndex()]);
}

const PMXBone& currentPmxBone = bones[index];

//Append Bone Setting
bool appendRotate = (uint16_t)currentPmxBone.boneFlag & (uint16_t)PMXBoneFlags::AppendRotate;
bool appendTranslate = (uint16_t)currentPmxBone.boneFlag & (uint16_t)PMXBoneFlags::AppendTranslate;
currentBoneNode->SetEnableAppendRotate(appendRotate);
currentBoneNode->SetEnableAppendTranslate(appendTranslate);
if ((appendRotate || appendTranslate) && currentPmxBone.appendBoneIndex < _boneNodeByIdx.size())
{
	if (index > currentPmxBone.appendBoneIndex)
	{
		bool appendLocal = (uint16_t)currentPmxBone.boneFlag & (uint16_t)PMXBoneFlags::AppendLocal;
		BoneNode* appendBoneNode = _boneNodeByIdx[currentPmxBone.appendBoneIndex];
		currentBoneNode->SetEnableAppendLocal(appendLocal);
		currentBoneNode->SetAppendBoneNode(appendBoneNode);
		currentBoneNode->SetAppendWeight(currentPmxBone.appendWeight);
	}
}

ボーンフラグでAppend変換を使用するかどうかをチェックします。

親ボーンのインデックスもPMXBoneから取得できます。

ビルドして確認してみましょう。

まとめ

ここまでで、VMDファイルに含まれるキーフレームアニメーションの処理が完了しました。

足もよく動いて、きちんと踊っているように見えますね。

まだ不足しているように見えるのは、髪の毛や服が動いていないことですね。

次の記事では、物理エンジンをプロジェクトに組み込んで、物理シミュレーションができるようにします。

ありがとうございました。

次回