【C++】OBBと球体の衝突面算出プログラム作ってみた【衝突判定】

# はじめに
　これは「他のクラスメイトがやってないことをしたい！」、「3Dゲームでは当たり判定は必須だよな」と思い3D空間での衝突判定に興味を持った私が、OBBの衝突面を求めることは可能なのかという小さな疑問から始まった制作の記録です。
※開発環境はVisualStudio、DirectXTKを使用しています。

# 判定の流れ
　この衝突判定を作り始めた時はオブジェクト同士の位置関係から算出することができないだろうかと考えましたが、残念ながら私にはその方法を思いつくことができませんでした。最初から判定方法に躓いた私は、視点を変えてOBBとしてではなく、**六枚の平面**として**分解**してみれば判定をとれるのではないかと考えました。

　大まかな流れとしては
1. OBBと球の衝突判定をとる
2. 六枚の平面と球の衝突判定をとる
3. 衝突面を返す
となります。

　OBBと球の衝突判定は[まるぺけつくろーどっとコムさん](http://marupeke296.com/index.html)を参考にさせていただいているので今回は割愛させていただきます。


# 限定的な範囲の平面の判定をとるには
　平面と球体の衝突判定を行うには、球体の衝突判定なので中心点と平面の最短距離が半径より短ければ当たっていると言えます。なので平面と点の距離を求めましょう。

　平面と点の距離を求める式はこちら
```math
L = \frac{|ax+by+cz+d|} {\sqrt{a^2+b^2+c^2}} 
```

　この式は平面の方程式を使用した平面と点の距離の求め方で、xyzが点の座標で、abcが平面の法線ベクトルで、dが平面と座標原点との距離になっています。

　この式をプログラムになおすとこのようになります。
```c++
// 無限平面の法線と座標Aの計算を行う
float planeD = -((nomalizeCross.x * plane.vertex[0].x) + (nomalizeCross.y * plane.vertex[0].y) +
		(nomalizeCross.z * plane.vertex[0].z));

// 線分と平面の垂直距離を求める
float L = abs((nomalizeCross.x * pos.x + nomalizeCross.y * pos.y + nomalizeCross.z * pos.z) + planeD) /
		sqrtf(pow(nomalizeCross.x, 2.0f) + pow(nomalizeCross.y, 2.0f) + pow(nomalizeCross.z, 2.0f));
```

　これで平面と点の距離を求められるのですが、この式は平面が無限に広がることが前提となっています。何故なら点から平面に垂線を下すことができるときにしか式が成立しないからです。

　では、垂線が下せないときはどうするのか。また私は平面としてではなく、**四本の線**として**分解**すれば、判定が取れるのではと考えました。

　点と線の最短距離は垂線が下せるときと下せないときで式が変わります。以下の図は垂線が下せる時の式です。
![図1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3219317/b9dd8de9-9f04-9006-52aa-a8758e1f2f5b.png)

　この式をプログラムになおすとこのようになります。
```C++
// ベクトルVの算出
DirectX::SimpleMath::Vector3 V = Pe - Ps;
DirectX::SimpleMath::Vector3 vNorm = V;
vNorm.Normalize();

// ベクトルPPsの算出
DirectX::SimpleMath::Vector3 PPs = P - Ps;

// 係数tの算出
float t = vNorm.Dot(PPs) / V.Length();

// 垂直距離を算出
DirectX::SimpleMath::Vector3 h = V * t - PPs;
```

　垂線が下せない場合は以下の図のようになります。
![図2.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3219317/3f132295-3618-98bd-cd5c-5180d803bb40.jpeg)

　下せない場合は単純で点Peか点Psのどちらか近い方と点Pのベクトルが最短距離になります。

　垂線が下せるか下ろせないかは係数tを見ることでわかります。下せる場合は係数ｔが0～1の間にあります。下ろせない場合は係数tが0未満だとPs側で、1より大きいとPe側になります。

これらをまとめてプログラムになおすとこのようになります。
```C++
// 
// ベクトルVの算出
DirectX::SimpleMath::Vector3 V = Pe - Ps;
DirectX::SimpleMath::Vector3 vNorm = V;
vNorm.Normalize();

// ベクトルPPsの算出
DirectX::SimpleMath::Vector3 PPs = P - Ps;

// 係数tの算出
float t = vNorm.Dot(PPs) / V.Length();

float result = 0.0f;

// 垂直におろせるなら垂直距離を、おろせないなら近い側の線分の頂点との距離を返す
if (t < 0)
{
	result = PPs.Length();
}
else if (t > 1)
{
	// ベクトルPPe
	DirectX::SimpleMath::Vector3 PPe = P - Pe;
	result = PPe.Length();
}
else
{
    // 垂直距離を算出
	DirectX::SimpleMath::Vector3 h = V * t - PPs;
	result = h.Length();
}

return result;
```

# まとめ
　以上のことをまとめるとOBBの衝突面を求めるには
1. OBBの衝突判定をとる
2. 当たっていたら平面と点の距離を計算する
3. 平面と点が垂直にあるかを調べる
4. 垂直にあれば平面と点の距離を返す、垂直になければ線と点の最短距離を返す
を行えば求めることができます。

ですが、あくまでこれは一つの方法であってもっと計算コストが安く済む方法があるかもしれません。

<details><summary>サンプルコード</summary>

```C++:Collision.cpp
/*===============================================================================================================
	機能	：	傾いた直方体の衝突面判定
	引数	：	立方体の位置と方向ベクトルとベクトルの長さ、球2のマトリクスと半径
	戻り値	：	int型(1：上面が当たっている	2：下面が当たっている	4：左面が当たっている
					  8：右面が当たっている	16：前面が当たっている	32：背面が当たっている
					  64：当っていない)
===============================================================================================================*/
int Collision::OBBHitFace(OBB& obb, Sphere& sphere)
{
	if (!OBBToSphereCollision(obb, sphere))
	{
		return IMPINGEMENT_FACE::NOT_HIT;
	}

	int result = 0;

	for (int i = 0; i < 6; i++)
	{
		(PlaneToSphere(obb.plane[i], sphere))
			? result += 1 << i : false;
	}

	return result;
}

/*===============================================================================================================
	機能	：	傾いた立方体と球の衝突判定を取る
	引数	：	立方体の位置と方向ベクトルとベクトルの長さ、球2のマトリクスと半径
	戻り値	：	bool型(true:当たっている	false:当っていない)
===============================================================================================================*/
bool Collision::OBBToSphereCollision(OBB& obb, Sphere& sphere)
{
	float length = LenOBBToPoint(obb, sphere.pos);

	return (length <= sphere.radius) ? true : false;
}

/*===============================================================================================================
	機能	：	傾いた立方体と点の最短距離を求める
	引数	：	立方体の位置と方向ベクトルとベクトルの長さ、点の座標
	戻り値	：	float型(傾いた直方体と点の最短距離)
===============================================================================================================*/
float Collision::LenOBBToPoint(OBB& obb, DirectX::SimpleMath::Vector3& p)
{
	DirectX::SimpleMath::Vector3 Vec(0.0f, 0.0f, 0.0f);   //　最終的に長さを求めるベクトル

	// 各軸についてはみ出た部分のベクトルを算出
	for (int i = 0; i < 3; i++)
	{
		float L = obb.length[i] / 2;

		// L=0は計算できない
		if (L <= 0) continue;

		float s = obb.normaDirect[i].Dot(p - obb.pos) / L;

		// sの値から、はみ出した部分があればそのベクトルを加算
		s = fabs(s);
		if (s > 1)
		{
			// はみ出した部分のベクトル算出
			Vec = Vec + ((1 - s) * L * obb.normaDirect[i]);
		}
	}

	return Vec.Length();   // 長さを出力
}

/*===============================================================================================================
	機能	:	有限平面と球の衝突判定
	引数	:	有限平面、球の位置、半径
	戻り値	:	bool型(true:当たっている	false:当っていない)
===============================================================================================================*/
bool Collision::PlaneToSphere(Plane plane, Sphere& sphere)
{
	float angle = 0.0f;

	float len = PlaneToPointLeng(plane, sphere.pos, sphere.radius, angle);

	return (len <= sphere.radius) ? true : false;
}

/*===============================================================================================================
	機能	:	有限平面と点の最短距離を求める
	引数	:	有限平面、球の位置
	戻り値	:	float型(有限平面と点の最短距離)
===============================================================================================================*/
float Collision::PlaneToPointLeng(Plane plane, DirectX::SimpleMath::Vector3 pos, float radius, float& angle)
{
	// 平面のベクトル
	DirectX::SimpleMath::Vector3 vec1 = plane.vertex[1] - plane.vertex[0];

	DirectX::SimpleMath::Vector3 vec2 = plane.vertex[3] - plane.vertex[0];

	// 平面の法線ベクトル
	DirectX::SimpleMath::Vector3 cross = vec2.Cross(vec1);

	// ベクトルの正規化
	DirectX::SimpleMath::Vector3 nomalizeCross;
	cross.Normalize(nomalizeCross);

	// 無限平面の法線と座標Aの計算を行う
	float planeD = -((nomalizeCross.x * plane.vertex[0].x) + (nomalizeCross.y * plane.vertex[0].y) +
		(nomalizeCross.z * plane.vertex[0].z));

	// 線分と平面の垂直距離を求める
	float L = abs((nomalizeCross.x * pos.x + nomalizeCross.y * pos.y + nomalizeCross.z * pos.z) + planeD) /
		sqrtf(pow(nomalizeCross.x, 2.0f) + pow(nomalizeCross.y, 2.0f) + pow(nomalizeCross.z, 2.0f));

	// 面と球のベクトル
	DirectX::SimpleMath::Vector3 v2 = pos - plane.vertex[0];
	v2.Normalize();

	// 平面の上下判定
	angle = acosf(v2.Dot(nomalizeCross));

	bool result = false;

	// 平面との内外判定を行う
	if (angle < DirectX::XM_PIDIV2)
	{
		result |= PlaneToPointInside(plane, pos - (nomalizeCross * L));
	}
	else if (angle > DirectX::XM_PIDIV2)
	{
		result |= PlaneToPointInside(plane, pos + (nomalizeCross * L));
	}
	else
	{
		result |= PlaneToPointInside(plane, pos);
	}

	// 平面内ならそのまま垂直距離を返す
	if (result)
	{
		return L;
	}

	// 平面の各辺との最短距離を求め最小直を返す
	L = LineToPointLeng(plane.vertex[0], plane.vertex[1], pos);
	L = std::min(LineToPointLeng(plane.vertex[1], plane.vertex[2], pos), L);
	L = std::min(LineToPointLeng(plane.vertex[2], plane.vertex[3], pos), L);
	L = std::min(LineToPointLeng(plane.vertex[3], plane.vertex[0], pos), L);

	return L;
}

/*===============================================================================================================
	機能	:	有限平面と点の内外判定
	引数	:	有限平面、点の位置
	戻り値	:	bool型(true:中にある	false:外にある)
===============================================================================================================*/
bool Collision::PlaneToPointInside(Plane plane, DirectX::SimpleMath::Vector3 pos)
{
	// 有限平面を二枚の三角ポリゴンにする

	// 三角ポリゴン1
	std::vector<DirectX::SimpleMath::Vector3> vertex1;

	vertex1.push_back(plane.vertex[0]);
	vertex1.push_back(plane.vertex[3]);
	vertex1.push_back(plane.vertex[1]);

	// 三角ポリゴン2
	std::vector<DirectX::SimpleMath::Vector3> vertex2;

	vertex2.push_back(plane.vertex[2]);
	vertex2.push_back(plane.vertex[1]);
	vertex2.push_back(plane.vertex[3]);

	bool result = false;

	// 三角ポリゴンと点の内外判定を行う
	result |= PolygonToPointInside(vertex1, pos);
	result |= PolygonToPointInside(vertex2, pos);

	return result;
}

/*===============================================================================================================
	機能	:	三角ポリゴンと点の内外判定
	引数	:	三角ポリゴンの頂点情報、点の位置
	戻り値	:	bool型(true:中にある	false:外にある)
===============================================================================================================*/
bool Collision::PolygonToPointInside(std::vector<DirectX::SimpleMath::Vector3> polygonPos, DirectX::SimpleMath::Vector3 pos)
{
	// 交点とポリゴンの頂点との外積結果を入れるための変数
	DirectX::SimpleMath::Vector3 normalA;
	DirectX::SimpleMath::Vector3 normalB;
	DirectX::SimpleMath::Vector3 normalC;

	// 一つ目の三角形法線の算出
	DirectX::SimpleMath::Vector3 vec_PTo1 = polygonPos[0] - pos;
	DirectX::SimpleMath::Vector3 vec_1To2 = polygonPos[1] - polygonPos[0];

	normalA.x = vec_PTo1.y * vec_1To2.z - vec_PTo1.z * vec_1To2.y;
	normalA.y = vec_PTo1.z * vec_1To2.x - vec_PTo1.x * vec_1To2.z;
	normalA.z = vec_PTo1.x * vec_1To2.y - vec_PTo1.y * vec_1To2.x;

	// 二つ目の三角形法線の算出
	vec_PTo1 = polygonPos[1] - pos;
	vec_1To2 = polygonPos[2] - polygonPos[1];

	normalB.x = vec_PTo1.y * vec_1To2.z - vec_PTo1.z * vec_1To2.y;
	normalB.y = vec_PTo1.z * vec_1To2.x - vec_PTo1.x * vec_1To2.z;
	normalB.z = vec_PTo1.x * vec_1To2.y - vec_PTo1.y * vec_1To2.x;

	// 三つ目の三角形法線の算出
	vec_PTo1 = polygonPos[2] - pos;
	vec_1To2 = polygonPos[0] - polygonPos[2];

	normalC.x = vec_PTo1.y * vec_1To2.z - vec_PTo1.z * vec_1To2.y;
	normalC.y = vec_PTo1.z * vec_1To2.x - vec_PTo1.x * vec_1To2.z;
	normalC.z = vec_PTo1.x * vec_1To2.y - vec_PTo1.y * vec_1To2.x;

	//内積で順方向か逆方向か調べる
	float dot1 = normalA.Dot(normalB);
	float dot2 = normalA.Dot(normalC);

	return (dot1 >= 0 && dot2 >= 0) ? true : false;
}

/*===============================================================================================================
	機能	:	線分と点の最短距離
	引数	:	線分の頂点1、線分の頂点2、点の位置
	戻り値	:	bool型(true:中にある	false:外にある)
===============================================================================================================*/
float Collision::LineToPointLeng(DirectX::SimpleMath::Vector3 lineVertex1, DirectX::SimpleMath::Vector3 lineVertex2,
	DirectX::SimpleMath::Vector3 pointPos)
{
	DirectX::SimpleMath::Vector3 v = lineVertex1 - lineVertex2;
	DirectX::SimpleMath::Vector3 vNorm = v;
	vNorm.Normalize();

	// 線分の頂点1との距離
	DirectX::SimpleMath::Vector3 v1 = pointPos - lineVertex2;

	float t = vNorm.Dot(v1) / v.Length();

	float result = 0.0f;

	// 垂直におろせるなら垂直距離を、おろせないなら近い側の線分の頂点との距離を返す
	if (t < 0)
	{
		result = v1.Length();
	}
	else if (t > 1)
	{
		// 線分の頂点2との距離
		DirectX::SimpleMath::Vector3 v2 = pointPos - lineVertex1;
		result = v2.Length();
	}
	else
	{
		// 垂直距離を求める
		DirectX::SimpleMath::Vector3 h = v * t - v1;
		result = h.Length();
	}

	return result;
}
```
</details>


# 制作を終えて
　今回の制作は公開されているライブラリを使用すれば、すぐに実装できることができるような内容です。すぐに実装できるようなことに時間を使うのは、効率が悪いと思います。ですが、自身で一から必要な情報を収集し、それを元に仮説を立てて検証することを何度も繰り返すといった自由に開発することができる学生生活でしか得られない経験をすることができたと思っています。この経験を生かしてこれからも技術成長に注力し、**業界で活躍できるようなクリエイター**を目指していきます。


# 参考サイト様
[OBBと球の衝突判定](http://marupeke296.com/COL_3D_No12_OBBvsPoint.html "まるぺけつくろーどっとコム")
[平面と点の距離](https://hiraocafe.com/note/plane_equation.html "おいしい数学")
[点と線の最短距離](http://marupeke296.com/COL_3D_No1_PointToLine.html "まるぺけつくろーどっとコム")