[番外編 #2] 関節を動かしてみた

はじめに

皆様お疲れ様です。
今回の 「番外編」 では 「関節」 についての記事を作成して参ります。

顔文字だけの私達からしたら興味深いですね！
あくまで疑似的な動きですがとある方法を用いれば案外シンプルにできることが期待されます。それでは早速見ていきましょう！

関節運動の仕組み

関節のイメージをみていきましょう
図のように関節運動を実現するために 「始点」「関節」「末端」 この3つが大きく関わってきます。赤点の「末端の移動」に合わせて 「関節位置」を計算します。

こちらの腕イラストをイメージとして見ても良いかなと

では次に 「関節の位置」 を計算するためにはどうすれば良いかみていきましょう。

余弦定理

この「関節の位置」の計算には 「余弦定理」を使用します
　数学で出てきましたね！「Cosθ」「コサイン」とかのやつですね
　正確には 「第2余弦定理」 を活用します。どのような公式か見ていきましょう。

　こんな図を昔に見たことがありましたねぇ！
この公式を利用して、予め定数で設定した辺「a」と辺「b」の距離と、末端の移動によって伸び縮みする辺「c」の距離を代入すると、下の図のように「CosAの値」が求まるようになります。

そしてこの「CosAの値」を利用して「関節の位置」を求めましょう！
…というのが今回検証する関節の位置を計算する手順になります。

この式も数学で見たことありますね！

ちなみにですが図のように赤の末端の位置は、関節の構造上 「始点を中心」 として一定の範囲でしか移動できません（人間も同様に関節の可動範囲の制限が無い人はいないかなと思います）

作成したコード

作成したコードの紹介をいたします。
今回はWASDキー操作で末端を移動させてみたいと思います。

作成する環境は何でも良いと思います（Unity、C#やアンリアルエンジンのBP（ブループリント）、JavaScriptなど・・・）。

今回もC++のDxLibを用いて作成してみたいと思います。
C++とは？
Dxライブラリについて
DXライブラリの使い方

// main.h（ヘッダーファイル）
#include "DxLib.h"

// 定数値（お好みで）.
const float LINEA = 150;		// 定数a.
const float LINEB = 150;		// 定数b.

// 変数宣言.
float s_Px, s_Py;		// 始点座標.
float end_x, end_y;		// 末端座標.
float k_x, k_y;			// 関節座標.

// 調整必要.
float speed = 2.5f;		// 移動速度.

float cosA;				// コサインA.
float sinA;				// サインA.

// ベクトル用.
float v1X;
float v1Y;

float v2X;
float v2Y;

// ヘッダファイルを読み込む.
#include "DxLib.h"
#include "stdio.h"
#include "math.h"    // 数学関数（pow（累乗）関数などが使えます）.
#include "main.h"

int WINAPI WinMain(HINSTANCE, HINSTANCE, LPSTR, int) 
{
	ChangeWindowMode(TRUE); // ウィンドウモードに設定
	DxLib_Init();   // DXライブラリ初期化処理
	SetDrawScreen(DX_SCREEN_BACK); //描画先を裏画面に設定.
	SetGraphMode(1280, 700, 32);	// 画面モードの設定.
	SetBackgroundColor(100, 100, 100);
	SetMouseDispFlag(0);
	
// 初期化.
// 始点（青）.
s_Px = 640;
s_Py = 350;

// 末端（赤）.
end_x = 940;
end_y = 350;

while (!ScreenFlip() && !ProcessMessage() && !ClearDrawScreen())
{
	if (CheckHitKey(KEY_INPUT_ESCAPE) != 0)		// ESCAPEキーが押されているかを調べる.
	{
		break;	 // 押されていたらメインループを抜ける.				
	}
		
//------------------------------[ＣｏｓＡを用いた関節計算]---------------------------
// ①：余弦定理でCosAの値を取得.
float line_c = sqrt((end_x - s_Px) * (end_x - s_Px) + (end_y - s_Py) * (end_y - s_Py));
	cosA = ((pow(LINEB, 2) + pow(line_c, 2) - pow(LINEA, 2))) / (2.0 * LINEB * line_c);

// ②：始点から末端を指す単位ベクターV1を取得.
v1X = (end_x - s_Px) / line_c;
v1Y = (end_y - s_Py) / line_c;

// ③：CosAとSinAを用いてV1をV2に回転.
//-------------------------------------------[ ＣｏｓＡの値を用いてV1を回転させる手順  ]----------------------------------------------------.
// 1 : CosAの値を元にsinAの値を求めます
// 右足.
sinA = sqrtf(1 - pow(cosA, 2));

// 2 : SinAを求めたら、座標回転式の各項にV1のXY成分、CosA、SinAの値を代入するとV1の方向をV2に方向転換できます。
// 右足.
v2X = (v1X * cosA) - (v1Y * sinA);
v2Y = (v1X * sinA) + (v1Y * cosA);
//------------------------------------------------------------------------------------------------------------------------------------------.

// ④：V2に辺ｂの長さをかけてV2’を作る.
v2X = v2X * LINEB;
v2Y = v2Y * LINEB;

// ⑤：始点の座標にV2’を加算すると… ⑥：関節の座標が求められる.
k_x = s_Px + v2X;
k_y = s_Py + v2Y;

//-------------------------------[描画処理]---------------------------
// 始点の描画.
DrawCircle(s_Px, s_Py, 5, GetColor(0, 0, 255));

// 末端の描画.
DrawCircle(end_x, end_y, 5, GetColor(255, 0, 0));

// 関節の描画.
DrawCircle(k_x, k_y, 5, GetColor(0, 255, 0));

// 末端から関節部分を線描画（線？骨組？）
DrawLine(end_x, end_y, k_x, k_y, GetColor(255, 255, 255), 5);

// 関節部分から始点の描画.
DrawLine(k_x, k_y, s_Px, s_Py, GetColor(255, 255, 255), 5);

//--------------------------------------------------------------------

//--------------[移動処理]-----------------
// Dキー入力で右に移動.
if (CheckHitKey(KEY_INPUT_D) != 0)
	{
		// 末端補正処理.
		line_c = sqrt(((end_x + speed) - s_Px) * ((end_x + speed) - s_Px) + (end_y - s_Py) * (end_y - s_Py));
		if (line_c <= LINEA + LINEB)
		{
			end_x += speed;
		}
	}
	// Aキー入力で左に移動.
	if (CheckHitKey(KEY_INPUT_A) != 0)
	{
		line_c = sqrt(((end_x - speed) - s_Px) * ((end_x - speed) - s_Px) + (end_y - s_Py) * (end_y - s_Py));
		if (line_c <= LINEA + LINEB)
		{
			end_x -= speed;
		}
	}

	// Wキー入力で上に移動.
	if (CheckHitKey(KEY_INPUT_W) != 0)
	{
		line_c = sqrt((end_x - s_Px) * (end_x - s_Px) + ((end_y - speed) - s_Py) * ((end_y - speed) - s_Py));
		if (line_c <= LINEA + LINEB)
		{
			end_y -= speed;
		}
	}
	// Sキー入力で下に移動.
	if (CheckHitKey(KEY_INPUT_S) != 0)
	{
		line_c = sqrt((end_x - s_Px) * (end_x- s_Px) + ((end_y + speed) - s_Py) * ((end_y + speed) - s_Py));
		if (line_c <= LINEA + LINEB)
		{
			end_y += speed;
		}
	}
}
DxLib_End();		// DXライブラリ終了処理.
return 0;
}

動作確認

上記で作成したコードを実行してみます。

これで疑似的な関節の動きができました。末端の初期値などはお好みで調整してみてください。気になる方は実装して確認してみると良いと思います。

あくまで一例のやり方です。参考程度としてください

おわりに

ベクトルの仕組みは私も完璧ではないところが多々あります。
今回は 「関節」 の動きについて紹介いたしました。

この[番外編]では今後もちょっと変わった記事を作成して参ります。
最後まで読んで頂きありがとうございました！

「サイト」
IK（インバースキネマティクス）

「Qiita」
【プログラマーのための数学】余弦定理