順運動学と逆運動学

ロボットや 3D モデルの関節を制御する方法として順運動学（Forward Kinematics）と逆運動学（Inverse Kinematics）がある。

順運動学 … 関節の角度から指先やつま先の位置が決まる
逆運動学 … 指先やつま先の位置から関節の角度が決まる

3D モデルのモーションデータは、スケルトンの各関節の SRT についてのカーブであり、これをスケルトンに適用することでモデルの姿勢が決まる。これは順運動学。

ロボットアームに何かを掴ませたり、3D モデルに何かを拾わせたりする場合、腕の先端（手）を目標の位置に持っていくために、肩や肘の姿勢をどのようにすればよいかを求める必要がある。これは逆運動学。

順運動学の実装

デモ

https://keitanxkeitan.github.io/TentacleFK/
腕が揺れます。

逆運動学の実装

デモ

https://keitanxkeitan.github.io/TentacleIK/
アローキーで腕の目標位置を操作できます。

アルゴリズム

Cyclic-Coordinate-Descent 法（CCD 法）と呼ばれるアルゴリズムを使っています。
詳細はこちらを御覧ください。

長所: ほかの IK アルゴリズムと比べて計算量が小さく、アルゴリズムもシンプル
短所: 関節の回転可動域を設定したり、エフェクタの位置と方向を同時に指定した計算が苦手

このような特徴があるため、人間の腕や脚のような可動域が明確に決まっている場合は使いにくいかもしれません。
一方で、イカやタコの触腕、生物の尻尾のような自由な可動域を持つ関節とは相性が良さそうです。

ソースコード

https://github.com/keitanxkeitan/Tentacle
肝となるのは ArmController.cs です。

ArmController.cs（一部抜粋）

// CCD アルゴリズムで IK
if (armParts.Count >= 1) {
	GameObject armPart_Last = armParts [armParts.Count - 1];

	for (int i = 0; i < ccdIterNum; ++i) {
		// 末端から根本へイテレートする
		for (int iArmPart = armParts.Count - 1; iArmPart >= 0; --iArmPart) {
			GameObject armPart = armParts [iArmPart];

			// エフェクタ位置
			Vector3 posEffect = armPart_Last.transform.position + armPart_Last.transform.forward * 5.0f;
			// -MEMO- 値を決め打ちにしているが本当はロケータを置くのがよい

			// エフェクタ方向
			Vector3 dirEffect;
			{
				dirEffect = posEffect - armPart.transform.position;
				dirEffect.Normalize ();
			}

			// 目標方向
			Vector3 dirTarget;
			{
				dirTarget = ikTarget.position - armPart.transform.position;
				dirTarget.Normalize ();
			}

			// 回転角
			float rotateRad;
			{
				float dot = Vector3.Dot (dirEffect, dirTarget);
				rotateRad = Mathf.Acos (dot);
			}

			if (rotateRad > 0.001f) {
				// 回転軸
				Vector3 rotateAxis = Vector3.Cross (dirEffect, dirTarget);
				rotateAxis.Normalize ();

				// 回転
				armPart.transform.Rotate (rotateAxis, rotateRad * Mathf.Rad2Deg);
			}
		}
	}
}