EPA는 Expanding Polytope Algorithm의 줄임말로, 교차판정에서 한발 나아가 '충돌'을 해결하기 위한 정보들을 구할 수 있는 알고리즘 중 하나이다.
충돌 해결을 위해서는 충돌 시점의 여러 상태정보가 필요한데, EPA를 통해 contact normal과 penetration depth를 구할 수 있다.
사실 EPA는 두 convex 물체의 교차시, 두 물체를 분리할 수 있는 최소한의 움직임(MTV: Minimum Translation Vector)을 구하는 알고리즘이다. 그런데 충돌해결을 위해서 이 MTV값을 충돌시의 노멀과 교차된 두 물체의 관통깊이값으로 사용하는것이다.
EPA의 동작은, 이름에서 말하듯 polytope을 확장해가며 최종 결과를 계산해가는 알고리즘이다.
두 물체 A와 B의 민코스키섬 집합에서 점들이 이룰 수 있는 원점을 포함하는 polytope 중, polytope의 face와 원점 사이의 최소 거리가 가장 큰 값, 그리고 이 face normal(원점과 멀어지는 방향)이 두 물체의 penetration depth, vector가 된다는 것이다.
* 조금 더 상세한 설명.
설명이 난해한데, 아래 그림을 보자. 민코스키섬 집합을 2차원 상에 표현한 것이다.
연두색은 원점을 포함하는 polytope을 나타내고, 주황색 선은 polytope의 face와 원점 사이의 거리이다.
각 polytope에서 face와 원점 사이의 최소거리는 빨간색 실선으로 표시되었고, 이때 보라색으로 표시된 face가 closest face가 된다.
가능한 polytope 중 face와 원점 사이의 최소거리가 가장 큰 값이란 것은, 위 예에서 오른쪽의 빨간실선을 선택하는 것을 말한다.
그런데 위 그림을 유심히 보면, 오른쪽의 polytope은 왼쪽의 polytope에 아래쪽 점을 추가하여 확장한 polytope임을 알 수 있다. 그리고 이렇게 확장된 polytope의 closest face는 갱신되었다.
이처럼 polytope을 계속 확장하며 maximum closest face를 찾아가는 과정이 EPA 동작의 핵심이라 할 수 있다.
여기에 좌측점 하나를 더 추가하여 아래와 같은 polytope을 형성한다면, 이때의 closest face는 민코스키섬 집합의 convex 표면임을 알 수 있다.
polytope을 확장하다 이렇게 convex hull face에 다다르면, 더 이상 polytope을 확장해도 closest face는 변함이 없을것이다. 즉, 이 결과가 maximum closest face가 되는것이다.
* EPA 수행 절차.
위 과정을 정리하여 EPA가 수행되는 절차를 표현하여보았다.
0. 두 물체 A와 B의 민코스키섬 집합에서, 원점을 포함하는 polytope을 구한다. GJK의 결과가 가장 적절하다.
1. 현재 polytope의 closest face를 구한다.
2. closest face의 face normal 방향으로 가장 먼 점 P를 구한다.
3. 원점으로부터 P가 closest face 보다 가까이 있다면, 현재 closest face는 convex hull face이므로 동작을 멈추고 MTV 정보를 반환한다.
4. P를 볼 수 있는 face들을 구하여 polytope을 확장한다.
5. (1)을 다시 수행한다.
이 과정에서, 점 P를 구하기 위해 GJK의 Support 함수를 동일하게 사용할 수 있다. 문제는 (4)의 polytope 확장 부분인데, 구현하면서도 실수가 가장 많았던 부분이다.
/*
Face type of polytope
*/
class Face{ public: union{ struct{ uinteger a, b, c; }; struct{ uinteger idx[3]; }; }; Vector3 normal; real distance;
// it initialize normal and distance as ccw order of given a,b,c. Face(uinteger _a, uinteger _b, uinteger _c, const Vector3& _normal, const real& _distance) : a(_a), b(_b), c(_c), normal(_normal), distance(_distance){}
Vector3 P = SupportPoint(convexA, convexB, closest->normal);
real d = P.Dot(closest->normal); if (d - closest->distance < 0.00001f){ // closest face is really closest and we're done. if (out_normal != NULL) *out_normal = closest->normal; if (out_depth != NULL) *out_depth = closest->distance; return true; } else{ // add point and make sure that face list to be convex. // note that added point p can't be placed on the any tip of voronoi regions // because the points of simplex are already farthest points of // minkowski set. simplex.push_back(P);
polytope의 바깥에 있는 한 점 P를 추가하여 polytope이 확장되는 상황은 여러 형태가 있을 수 있다. 단 하나의 face만이 해당될 수도 있고, 여러 face가 해당될 수도 있고, 하나의 꼭지점이 잠식되어버릴 수도 있다. 이런 상황들을 머릿속에 그려보다 보면, 한가지 중요한 사실이 보이는데, 점 P가 위치할 수 있는 영역은, 현재 polytope의 voronoi region이라는 것이다
여기에 더해, EPA 수행 절차의 (2)가 반복되므로 polytope의 정점들은 convex hull의 정점임이 보장되고, 이는 새로운 P가 voronoi region의 꼭지점 영역(.. 이라 표현하는게 맞는지는 모르겠다.)에는 절대 존재할 수 없음을 보장한다.
즉, 점 P가 추가되어 polytope이 확장 유형은,
1. 단 하나의 face만 P를 볼 수 있다
2. 두개 이상의 face가 P를 볼 수 있지만, 이들 face는 한 꼭지점을 둘러싸지는 않는다.
두 가지로 나뉘게 된다.
첫번째 유형은 어렵지 않게 생각할 수 있다.
유형 1:
a. 점 P와 face의 각 모서리를 이어 새로운 세 개의 face를 생성한다.
b. 기존의 face를 제거한다.
하지만 두번째는 생각만큼 간단하지가 않았다. 아래는 혼자 정신노동하여 고안한 방법이지, Expanding을 위한 일반적인 방법이거나.. 웰메이드 알고리즘이 아니므로, 성능 및 안정성이 보장되지 않는다.
설명을 위해 점 P를 보는 face들을 target face라 하겠다.
유형 2:
a. target face의 정점들을 중복없이 나열한다. 이를 UniqVert라 하자.
b. UniqVert를 CW 혹은 CCW의 일정한 순서로 정렬하여 circuit strip을 구성한다.
c. 정렬된 UniqVert를 연결시킨 strip의 모서리와 점 P를 이어 새로운 face들을 생성한다.
d. 기존의 target face들을 제거한다.
다른 단계는 어찌저찌 한다손 쳐도, b의 과정은 만만치 않았다.
나는 이를 해결하기 위해서 특정한 평면을 정한 후 UniqVert를 투영하고, 중심점 CP를 구한 후 CP와 투영된 UniqVert 사이의 각도로 이들을 CCW 순으로 정렬하였다.
target face들은 서로 최소 한 모서리씩 고유하며 한 꼭지점을 둘러싸지 않아, 연결된 형태가 아주 괴상하지 않을것이란 가정하에.. 생각한 방법이지만, 결과적으론 잘 동작한다. (물론 모든 convex 물체에 대해 테스트한것은 아니다.)
설명이 길었는데, 세세한 구현은 아래 코드와 주석을 보며 이해해 주길 바란다.
(실제 구현에서는 target face가 두 개일 때 특별한 처리를 해 주었다.)
// expand simplex to be containing the origin // as far as possible. void EPA_Expand(const Simplex& simplex, FaceList& faces, const std::vector<FaceList::iterator>& targets) { uinteger pidx = simplex.size() - 1; uinteger szTarget = targets.size(); if (szTarget == 0){
} else if (szTarget == 1){ // point is in a certain face side of voronoi regions. // there are 3 faces should be added and a face should be removed. faces.push_back(Face(pidx, targets[0]->a, targets[0]->b, simplex)); faces.push_back(Face(pidx, targets[0]->b, targets[0]->c, simplex)); faces.push_back(Face(pidx, targets[0]->c, targets[0]->a, simplex)); faces.erase(targets[0]); } else if (szTarget == 2){ // point is in a certain edge side of voronoi regions. // there are 4 faces should be added and two faces should be removed. // find unique and one way ordered indices of two target faces. uinteger uniqIdxs[4]; for (uinteger i = 0; i < 3; i++){ uinteger j = 0; for (; j < 3; j++){ if (targets[0]->idx[i] == targets[1]->idx[j]) break; } if (j != 3){ // it can be guaranteed that the target faces are orderd by ccw. // (they were sorted when compute those face normals) // so, (i+1)---(j+2) or (i+2)---(j+1) is the common edge. if (targets[0]->idx[(i + 1) % 3] == targets[1]->idx[(j + 2) % 3]){ // (i)--(j+1)--(i+1)--(i+2) order uniqIdxs[0] = targets[0]->idx[i]; uniqIdxs[1] = targets[1]->idx[(j + 1) % 3]; uniqIdxs[2] = targets[0]->idx[(i + 1) % 3]; uniqIdxs[3] = targets[0]->idx[(i + 2) % 3]; break; } else{ //if (i+2)---(j+1) // (i)--(i+1)--(i+2)--(j+2) uniqIdxs[0] = targets[0]->idx[i]; uniqIdxs[1] = targets[0]->idx[(i + 1) % 3]; uniqIdxs[2] = targets[0]->idx[(i + 2) % 3]; uniqIdxs[3] = targets[1]->idx[(j + 2) % 3]; break; } } }
// 1. push indices of targets[0] into idxs. uniqIdxs.push_back(targets[0]->a); uniqIdxs.push_back(targets[0]->b); uniqIdxs.push_back(targets[0]->c); uinteger szIdxs = uniqIdxs.size();
// 2. gather the indices from targets uniquely. uinteger i, j, k; for (i = 1; i < szTarget; i++){ // push indices from target[i] into idxs uniquely. for (j = 0; j < 3; j++){ bool unique = true; for (k = 0; k < szIdxs; k++){ if (targets[i]->idx[j] == uniqIdxs[k]){ unique = false; break; } } if (unique){ uniqIdxs.push_back(targets[i]->idx[j]); szIdxs++; } } }
// 3. compute the center point of unique vertices. Vector3 CP(0); for (i = 0; i < szIdxs; i++) CP += simplex[uniqIdxs[i]]; CP /= (real)szIdxs;
// 4. project unique vertices into a certain plane. // at this point, i choose the plane which has CP-P as plane normal // and is passing the origin. Vector3 norm = simplex[pidx] - CP; norm.Normalize();
Vector3 projCP = CP - CP.Dot(norm)*norm; std::vector<Vector3> projPoints(szIdxs); for (i = 0; i < szIdxs; i++){ const Vector3& Q = simplex[uniqIdxs[i]]; projPoints[i] = Q - Q.Dot(norm)*norm; }
// 5. compute angles of CP-projPoints axis as plane normal. std::vector<real> angles(szIdxs); Vector3 standAxis = projPoints[0] - projCP; standAxis.Normalize();
angles[0] = 0; for (i = 1; i < szIdxs; i++){ Vector3 line = (projPoints[i] - projCP).Normal(); Vector3 perp = standAxis.Cross(line); angles[i] = math::acos(standAxis.Dot(line)); // if sin(θ) <0 then angle is over PI if (perp.Dot(norm) < 0){ angles[i] = math::PI*2.f - angles[i]; } }
// 6. sort angles with indices ordered by angles ascent. // algorithm from wikipedia. real tmp; for (i = 1; i<szIdxs; i++){ j = i; tmp = angles[j]; k = uniqIdxs[j];
