충돌 알고리즘 - GJK, EPA

https://winter.dev/articles/gjk-algorithm
위 블로그를 참고해서 2D상에서 충돌을 검출하고 간단한 탄성충돌을 적용했다.

1. 코드

CollisionData CollisionManager::GJK(Collider* _iCollider, Collider* _jCollider)
{		
	//Simplex는 포인트(vertex)들의 배열로, 최대 갯수는 3개이다.
	Simplex2D points;
	
	//임의의 방향벡터(1,0,0)으로부터 초기 supportingPoint를 구한다.
	Vector3 support = SupportingPoint(_iCollider, _jCollider,Vector3(1,0,0));
	points.push_front(support);

	//새로운 방향벡터는 원점을 향한다.
	Vector3 direction = (support * -1).normalize();

	//다른 supporting Point들을 구한다. 
	//(탈출 조건 : finds a vertex that was already the furthest one along it.)
	while (true)
	{
		support = SupportingPoint(_iCollider, _jCollider, direction);
		if (SignedDot(support, direction) <= 0)
			return CollisionData(false); //no collision
		
		points.push_front(support);

		if (NextSimplex2D(points, direction))
		{
			vector<Vector2> polytope;
			for (int i = 0; i < 3; i++)
			{
				polytope.push_back(Vector2(points[i].x, points[i].y));
			}

			Vector2 collisionPoint = EPA(polytope, _iCollider, _jCollider);
			return CollisionData(true, collisionPoint);
		}	
	}
}

struct Simplex2D
{
private:
	std::array<Vector3, 3> m_points;
	unsigned m_size;

public:
	Simplex2D()
		:m_points({ Vector3(),Vector3(),Vector3() })
		, m_size(0)
	{
	}

	Simplex2D& operator = (std::initializer_list<Vector3> list)
	{
		for (auto v = list.begin(); v != list.end(); v++)
		{
			m_points[std::distance(list.begin(), v)] = *v;
		}
		m_size = list.size();

		return *this;
	}

	void push_front(Vector3 _point)
	{
		m_points = { _point, m_points[0], m_points[1] };
		m_size = min(m_size + 1, 3u);
	}

	Vector3& operator[](unsigned i) { return m_points[i]; }
	unsigned size() const { return m_size; }
	auto begin() const { return m_points.begin(); }
	auto end() const { return m_points.end() - (3 - m_size); }
};

/// <summary>
/// a x b x c의 삼중곱
/// </summary>
Vector3 TripleProduct(Vector3 _a, Vector3 _b, Vector3 _c)
{
	Vector3 first = Cross(_a, _b);
	Vector3 second = Cross(first, _c);

	return second;
}

/// <summary>
/// a x (b x c)의 삼중곱
/// </summary>
Vector3 TripleProduct2(Vector3 _a, Vector3 _b, Vector3 _c)
{
	Vector3 first = Cross(_b, _c);
	Vector3 second = Cross(_a, first);

	return second;
}

/// <summary>
/// Convex형태의 d방향으로 가장 멀리 있는 점
/// </summary>
Vector3 convexMaxPoint(vector<Vector2> s1, Vector2 d)
{
	Vector2 maxPoint;
	float maxDistance = numeric_limits<float>::lowest();
	for (Vector2 vertex : s1)
	{
		float distance = SignedDot(vertex, d);
		if (distance > maxDistance)
		{
			maxDistance = distance;
			maxPoint = vertex;
		}
	}
	return maxPoint;
}

/// <summary>
///Circle형태의 d방향으로 가장 멀리 있는 점
/// </summary>
Vector3 circleMaxPoint(Vector2 pos, float r, Vector2 d)
{
	return pos + d.normalize() * r;
}

/// <summary>
/// 민코스키 차로 구해지는 두 도형 D방향으로의 최대 거리
/// </summary>
Vector3 SupportingPoint(Collider* c1, Collider* c2, Vector3 d)
{
	Vector3 maxPoint1, maxPoint2;

	if (c1->getType() == COLLIDER_TYPE::CIRCLE)
		maxPoint1 = circleMaxPoint(c1->getPosition(), c1->getRadius(), Vector2(d.x, d.y).normalize());
	else
		maxPoint1 = convexMaxPoint(c1->getVertexs(), Vector2(d.x, d.y).normalize());

	if (c2->getType() == COLLIDER_TYPE::CIRCLE)
		maxPoint2 = circleMaxPoint(c2->getPosition(), c2->getRadius(), Vector2(d.x, d.y).normalize() * -1);
	else
		maxPoint2 = convexMaxPoint(c2->getVertexs(), (Vector2(d.x, d.y).normalize()) * -1);


	return maxPoint1 - maxPoint2;
}

/// <summary>
/// 
/// </summary>
bool NextSimplex2D(Simplex2D& _points, Vector3& _direction)
{
	switch (_points.size())
	{
	case 2:		return GJKLine(_points, _direction);
	case 3:		return GJKTriangle(_points, _direction);
	}

	return false; //이까지 오면 안됨
}

/// <summary>
/// 두개의 벡터가 같은 방향에 있는가?
/// </summary>
bool SameDirection(Vector3 _direction, Vector3 _ao)
{
	return SignedDot(_direction, _ao) > 0;
}

/// <summary>
/// Simplex의 점들 중 2개를 찾았을 때
/// </summary>
bool GJKLine(Simplex2D& _points, Vector3& _direction)
{
	Vector3 a = _points[0];
	Vector3 b = _points[1];

	Vector3 ab = b - a;
	Vector3 ao = a * -1;

	if (SameDirection(ab, ao))
	{
		_direction = TripleProduct(ab, ao, ab).normalize();
		//return true; 
	}
	else
	{
		_points = { a };
		_direction = ao;
	}
	return false;
}

/// <summary>
/// Simplex의 점 3개로 원점을 포함하는지 판단
/// </summary>
bool GJKTriangle(Simplex2D& _points, Vector3& _direction)
{
	Vector3 a = _points[0];
	Vector3 b = _points[1];
	Vector3 c = _points[2];

	Vector3 ab = b - a;
	Vector3 ac = c - a;
	Vector3 ao = a * -1;

	Vector3 abc = Cross(ab, ac);

	Vector3 r = TripleProduct(ab, ac, ac);

	if (SameDirection(TripleProduct(ab, ac, ac), ao))
	{
		if (SameDirection(ac, ao))
		{
			_points = { a,c };
			_direction = TripleProduct(ac, ao, ac).normalize();
		}
		else
		{
			return GJKLine(_points = { a,b }, _direction);
		}
	}
	else
	{
		if (SameDirection(TripleProduct2(ab, ab, ac), ao))
		{
			return GJKLine(_points = { a,b }, _direction);
		}
		else
		{
			return true;
		}
	}
	return false;
}

/// <summary>
/// 침투점 계산
/// </summary>
Vector2 EPA(vector<Vector2>& polytope, Collider* shapeA, Collider* shapeB)
{
	int minIndex = 0;
	float minDistance = std::numeric_limits<float>::infinity();
	Vector2 minNormal;

	while (minDistance == std::numeric_limits<float>::infinity())
	{
		for (size_t i = 0; i < polytope.size(); i++)
		{
			size_t j = (i + 1) % polytope.size();

			Vector2 vertexI = polytope[i];
			Vector2 vertexJ = polytope[j];

			Vector2 ij = vertexJ - vertexI;

			Vector2 normal = Vector2(ij.y, -ij.x).normalize();
			float distance = SignedDot(normal, vertexI);

			if (distance < 0)
			{
				distance *= -1;
				normal = normal * -1;
			}

			if (distance < minDistance)
			{
				minDistance = distance;
				minNormal = normal;
				minIndex = j;
			}
		}

		Vector3 support3 = SupportingPoint(shapeA, shapeB, minNormal);
		Vector2 support = Vector2(support3.x, support3.y);
		float sDistance = SignedDot(minNormal, support);

		if (abs(sDistance - minDistance) > 0.001)
		{
			minDistance = std::numeric_limits<float>::infinity();
			polytope.insert(polytope.begin() + minIndex, support);
			//minIndex위치에 support를 추가한다. 
		}
	}

	return minNormal * (minDistance + (float)0.001);
}

2. 이론

두 볼록 다각형(Convex)에 대한, 민코스키 차를 구하면, 두 볼록 다각형이 서로 쳡치는지 아니면, 최소 거리가 얼마인지를 알 수가 있는데, GJK 알고리즘은 전체 민코스키 차를 구하지 않고, 일부분만 구해서 두 다각형의 충돌을 판정할 수 있는 알고리즘이다.

(1) 용어 정리 및 기초

1) Simplex

• 0-Simplex는 점
• 1-Simplex는 선분
• 2-Simplex는 삼각형
• 3-Simplex는 사면체를 의미한다.
  
  

2) Supporting Point

어떠한 방향 d에서 가장 멀리 떨어진 점을 의미한다.

3) 민코스키 차 ( Minkowski difference )

민코스키 차는 다음과 같이 정의된다.

𝐴−𝐵 = { x − y ∣ x ∈ 𝐴, y ∈ 𝐵 }

즉, 집합 𝐴의 모든 점과 집합 𝐵의 모든 점 간의 차를 계산한 결과가 민코스키 차를 이루는 집합이 된다.

민코스키 차는 다음 두가지 중요한 특징을 가지고 있다.

• 두 도형이 모두 convex라면 민코스키 차 또한 convex이다.
• 두 도형이 충돌하고 있다면 민코스키 차는 (0, 0)을 포함한다.

두 삼각형에 대한 민코스키 차를 구하는 예시를 들어보자면,

 삼각형 A { (0,0), (2,0), (1,2) }
 삼각형 B { (1,1), (3,1), (2,3) } 

1. A의 점 (0,0)에 대해서 B의 모든 점을 뺀다.

 (0,0) - (1,1) = (-1,-1)
 (0,0) - (3,1) = (-3, -1)
 (0,0) - (2,3) = (-2, -3)

2. 위 과정을 A의 점(2,0), (1,2) 에 대해서도 수행한다.

3. 결과로 얻어진 점들 중 중복된 점들을 제외한 결과는 다음과 같다.

   {(-1,1), (-3,-1), (-2,-3), (1,-1), (0,-3), (0,1), (-2,1)}

4. 위 점들로 convex hull(최외각 점으로 이루어진 도형)을 구한다.
   Graham Scan 알고리즘, Jarvis March 알고리즘 등으로 구할 수 있다.

   {(−2,−3),(0,−3),(1,−1),(0,1),(−2,1),(−3,−1)}

기본적은 이론은 위와 같다.

그렇다면 민코스키 차가 원점(0,0)을 포함한다는 것이 어떻게 충돌과 연결이 될까?
민코스키 차가 원점(0,0)을 포함한다는 것은 다음 벡터 방정식이 성립한다는 것이다.

0 ∈ A−B 즉,    0 = a−b for some a∈A, b∈B

풀어서 설명하자면, 도형 A에 포함된 무수한 점들 중 하나인 x와, 또 B에 포함된 y가 있다고 했을 때,
a - b = 0 이 의미하는 것은 a와 b가 같은 위치에 있는 점이라는 것이다. 즉 두 도형이 어떤 부분에서 서로 겹친다는 것을 의미한다.

(2) GJK 알고리즘

최적화 없이 이론 그대로 충돌을 계산하기에는 엄청나게 무겁기때문에 GJK 알고리즘은 전체 민코스키 차를 구하지 않고, 일부분만 구해서 충돌을 판정한다.

1) 첫 Supporting Point 구하기

먼저 임의의 초기 방향벡터를 이용해서 Supporting Point를 구한다.

• Supporting Point는 A 도형의 점들 중 d방향으로 가장 멀리 있는 점을 구하고,
• B에 대해서는 -d방향으로 가장 멀리 있는 점을 구한 다음,
• 두 Supporting Point를 빼면 민코스키 차로 만들어지는 도형 외각의 점을 구할 수 있다.

아무 방향벡터나 사용해도 되며, 코드에서는 벡터 (1,0,0)을 이용했다. 2차원이지만 외적 계산의 편의성을 위해 Vector3를 사용했다.

원은 원의 중심에서 반지름 크기만큼의 방향벡터를 더해주면 되고,
다각형은 모든 Vertex(정점)에 대해 반복문을 돌면서 방향벡터와 Vertex를 내적했을 때 최대값이 나오는 것이 바로 Supporting Point 이다.

2) 2번째 Supporting Point 구하기

다음으로 사용할 방향벡터로는 이전에 구했던 Supporting Point에서 원점을 향하는 벡터를 사용한다.
Supporting Point를 구한 뒤 방금 사용했던 방향벡터와 내적해준다.
여기서 내적 값이 0보다 작다면 민코스키 차가 원점을 포함하지 않기 때문에 서로 충돌하고 있지 않음을 알 수 있다. 반대의 경우라면 테스트를 더 진행해야 한다.

3) 3번째 Supporting Point 구하기

다음은 하나의 Vertex를 더 찾아 원점을 포함하는 삼각형을 만드는 것이 목표이다. 따라서 앞서 찾은 두 Supporting Point를 연결한 것과 수직인 벡터이면서 원점을 향하는 벡터를 이용해 다시 Supporting Point를 구해준다. 수직인 벡터는 벡터의 삼중곱을 이용해 구할 수 있다.

그리고 2번과 마찬가지로 내적해서 0보다 작은지 확인해준다. 0보다 크다면 다음 과정으로 넘어간다.

Note. 왜 (𝑎𝑏 × 𝑎𝑜) × 𝑎𝑏 를 하면 원점을 향하는 노말 벡터가 나올까?
(𝑎𝑏 × 𝑎𝑜)가 의미하는 것은, 두 벡터 (𝑎𝑏 × 𝑎𝑜) 가 있는 평면에 대해 수직인 벡터이다. 따라서 외적하면 v(0, 0, ?)과 같은 값이 나온다. "?"의 크기는 ab와 ao로 이루어진 평행사각형의 넓이 이고, 부호는 ab에 대해서 ao가 시계 방향에 있다면 양수가, 반시계 방향에 있다면 음수가 나온다. 만약 (𝑎𝑏 × 𝑎𝑜)를 다시 𝑎𝑏와 곱한다면, 이 새로운 외적 (𝑎𝑏 × 𝑎𝑜) × 𝑎𝑏는 원래의 ab와 수직이면서 (𝑎𝑏 × 𝑎𝑜)와도 수직인 벡터가 된다. (ba x bo) x ba와 결과적으로 완전히 동일하다.

4) 삼각형이 원점을 포함하는지 확인하기

Ra : (2) 과정에서 c점에서 원점을 향하는 벡터를 구했기 때문에 원점을 포함시킬 수 없다.

Rb : (2) 과정에서 원점을 지나도록 b점을 구했기 때문에 원점을 포함시킬 수 없다.

Rab : (3) 과정에서 원점을 향해 수직인 벡터를 구했기 때문에 원점을 포함시킬 수 없다.

Rc : (3) 과정에서 원점을 지나도록 a점을 구했기 때문에 원점을 포함시킬 수 없다.

이제 남은 구역은 `Rbc`, `Rac`, `Rabc` 다.

Rbc 영역 안에 원점이 있는지 확인하는 방법은 다음과 같다.

Rac 영역 안에 원점이 있는지 확인하는 방법은 다음과 같다.

Rbc, Rac에 모두 원점이 포함되어 있지 않다면 두 도형이 충돌하고 있음을 알 수 있다.

(2) EPA 알고리즘

GJK알고리즘은 두 콜라이더가 충돌했는지 여부만 판단할 수 있다. EPA(Expanding Polytope Algorithm)는 두 콜라이더 간의 충돌 깊이와 충돌 방향을 계산하기 위한 알고리즘이다. EPA는 앞서 GJK로 얻어낸 3개의 Vertex, 즉 Polytope으로 시작한다.

• 이 삼각형의 가장자리에 대해 법선 벡터와 원점까지의 최소 거리를 계산한다.
• 최소 거리를 구했던 법선 벡터로 새로운 Supporting Point를 계산한다.
• 새로운 Supporting Point 와 최소 거리를 구했던 법선 벡터를 내적하여 거리를 구한다.
• 새로운 Supporting Point의 거리와 이전 최소 거리의 차이가 일정 오차 이하로 작아지면 종료한다. 그렇지 않다면 Polytope에 새로운 Supporting Point를 추가하고 앞의 과정을 반복한다.