implementing GJK in 3D

Gilbert - Johnson - Keerthi algorithm. 세 사람의 이름을 따 지어졌다.

GJK는 본래 두 convex shape 사이의 최소거리를 구하기 위해 고안되었다. 이를 활용해서 두 물체가 충돌하는지 처리할 수 있고, 3D 상의 복잡한 convex에 대해서는 SAT보다 좋은 성능을 보여 실시간 충돌처리에 많이 사용된다.

 

GJK에 대한 기초적인 이론은 Christer Ericson의 <Real-Time Collision Detection>과 William Bittle씨의 블로그 그리고 이분이 소개해준 동영상(http://mollyrocket.com/849)을 통해 익혔고, 이를 확장하여 3D에서의 GJK를 구현하였다.

 

주요 자료구조와 함수에 대한 설명만으로 구현설명을 대신하겠다.

Simplex

사면체(tetrahedron)를 이루는 네 개의 정점으로 이루어진다. GJK를 구하기 위해 이 자료구조가 사용될 때는 마지막에 추가한 정점으로부터 A,B,C,D의 순서를 갖는다고 가정한다. 물론 이건 GJK 처리과정에서의 가정이고 GJK의 결과로 도출되는 simplex에서는 이 정점순서를 어찌하든 상관이 없다. EPA에 이어 사용하기 위해서 가변배열을 사용하면 좋다.

 

Vector3 FarthestPoint(set, direction)

주어진 set에서 direction 방향으로의 가장 멀리있는 정점을 반환한다. 모든 정점을 direction에 투영하여 가장 큰 값을 내는 정점을 취하면 된다.

 

Vector3 SupportFunc(setA, setB, direction)

두 정점집합 setA와 setB의 민코스키섬 집합에서 direction 방향으로의 가장 멀리있는 정점을 반환한다. [math]FarthestPoint(A_i - B_j, \mathbf{\vec{d}}) = FarthestPoint(A_i, \mathbf{\vec{d}}) - FarthestPoint(B_j, \mathbf{-\vec{d}})[/math]라는 사실로부터 모든 민코스키섬 집합의 원소를 구하지 않고 필요한 값만을 계산해온다.

위 식이 성립함은 두 정점집합의 민코스키섬 집합을 그려보면 아주 명확하게 이해된다.

 

bool UpdateSimplex(simplex, direction)

GJK 처리과정에서 현재 simplex로부터 원점을 포함하기 위해 발버둥치는 과정이다. simplex가 원점을 포함한다면 true값을 반환한다. 핵심은 보로노이 영역(voronoi region)으로 후보를 나누어 다음 처리시 원점을 포함할 가능성이 가장 큰 direction을 구하는것이다.

direction은 이전 반복에서의 방향값인 동시에 갱신 후, SupportFunc를 위한 방향값이 된다.

 

simplex가 갱신되는 흐름은 다음과 같다.

// 0. 준비

// direction은 이전 갱신상태에서 현재의 정점 A를 찾기 위한 support direction이다.

// AB는 A to B의 벡터를, ABC는 삼각형 ABC의 노멀벡터(오른손 좌표계)를 나타낸다.

 

// 1. 현재 simplex의 정점 A가 BCD의 위쪽에 있는지, 아래쪽에 있는지 확인한다.

// 이 과정으로 처리해야 하는 후보삼각형과 그 와인딩(winding)을 가려낼 수 있다.

// 이 전 direction이 원점을 포함하는 방향벡터라는 사실로부터 direction의 반대방향으로의 보로노이 영역은 후보에서 제외된다.

// 또한 정점 A에 해당되는 보로노이 영역은, SupportFunc가 가장 멀리있는 정점을 반환한다는 사실로부터 역시 제외될 수 있다.

if dot(direction, BCD) then,

A is above BCD. ABC, ADB and ACD are candidates.

else

A is under BCD. ACB, ABD, and ADC are candidates.

 

// 2. (1)의 결과, 가능성 있는 삼각형들에 대해서 원점이 삼각형을 포함하는 평면의 위에있는지, 아래있는지 검사한다.

// 만약 원점이 후보 삼각형의 위에있다면, simplex에서 후보삼각형의 밑에놓인 정점을 제거하고 함수를 종료한다.

// (P는 후보삼각형의

for each candidate triangles,

if O is above the candidate triangle then,

remove a point under the candidate triangle from simplex.

return false.

 

// 3. (2)의 과정으로부터 선택되는 후보삼각형이 하나도 없다면, 원점은 simplex 안에 위치한다. 즉 두 convex shape은 충돌한다.

if there is no appropriate candidate triangle then,

two convex shapes are intersecting.

return true.

 

bool DoGJK(setA, setB, out_simplex)

두 정점집합 setA, setB에 대해 GJK를 수행한다. 초기 simplex를 구하는 과정은 원점을 포함할 가능성이 가장 큰 벡터를 취해가는 식으로 구현하였다. GJK를 처음다뤄보는거라 아래 방법이 최선일것이라고는 생각하지 않는다.

 

GJK가 수행되는 과정은 다음과 같다. 아래 과정에서 direction은 굳이 정규화 될 필요는 없다.

// 1. 초기 simplex를 구한다.

// 1-1. 처음 direction을 정한 후 서포트포인트를 구한다. 여러 방향이 선택 가능하지만, 여기서는 (1,0,0)의 우측벡터를 사용하였다.

// 그런데 원점이 direction 방향으로 support point보다 멀리있다면 더 확인할 것도 없이 이 민코스키집합은 원점을 포함할 수 없다.

// 즉 두 convex shape은 충돌하지 않을것이다.

direction = Right

simplex.Add(SupportFunc(setA, setB, direction)) //-- point D

if dot(direction, C) < 0 then //-- (a)

two convex shapes can not intersect.

return false.

 

// 1-2. 이번에는 DO (D to O)를 방향벡터로 하여 다음 정점을 구한다.

// (1-1)과 마찬가지로 선분 넘어서는 보로노이 영역을 검사하여 원점 포함 가능성을 확인한다.

direction = -direction

simplex.Add(SupportFunc(~)) //-- point C

if dot(direction, C) < 0 then //-- (a)

two convex shapes can not intersect.

return false.

 

// 1-3. CD와 CO에 수직한 벡터를 다음 방향벡터로 해보자.

// CD와 CO가 평행하다면 원점이 CD선상에 존재한다는 것인데, 1-2의 (a)로부터 원점은 CD를 넘어설 수 없다. 이 경우 두 convex shape은

// 충돌할 수 밖에 없다. EPA 수행을 위해 일련의 과정을 통해 simplex를 완성한다. (3D에선 사면체)

direction = CD x CO

if direction is zero then,

two convex shapes are intersecting.

complete simplex for EPA input.

return true.

simplex.Add(SupportFunc(~)) //-- point B

 

// 1-4. 마지막으로 BCD의 노멀벡터를 사용한다.

// 이때 원점과 BCD를 포함하는 평면 사이의 관계로부터 어느 방향으로의 벡터를 선택할지 결정하는데,

// 간혹 원점이 BCD 평면상에 존재할 수 있다. 이런 경우는 BCD의 노멀벡터로 B를 다시 구하고 (1-4)를 반복한다.

// 정점 A는 (2) 과정의 시작과 함께 구해진다.

direction = BC x BD

if Origin is on the plane of BCD then,

change B as SupportFunc(setA, setB, BCD) and repeat 1-4.

else if Origin is under the plane of BCD then,

direction = -direction.

 

// 2. (1)로부터의 simplex를 초기값으로 UpdateSimplex를 반복한다.

// 현재 direction에 대한 서포트포인트를 구하고 이 포인트의 보로노이 영역을 검사한다.

// UpdateSimplex에서는 현재 상태에서 simplex가 원점을 포함하는지 검사하고 simplex와 direction을 갱신한다.

loop:

V = SupportFunc(setA, setB, direction)

if dot(direction, V) < 0 then,

return false.

simplex.Add(V)

if UpdateSimplex(simplex, direction) returns true then,

simplex contains origin. it means two shapes are intersecting.

return true.

 

써놓고 보니 참 더럽다... 그래도 잘 돌아가니 다행이다.

아래는 구현한 코드와 테스트 결과이다. 충돌이라 판정되면 테두리를 빨갛게 하였다.

 

 /*

code by alleysark

*/

class Simplex{
private:
    std::vector<Vector3> points;

 

public:
    Simplex(){}
    ~Simplex(){}

 

    Vector3& operator[](uinteger idx){
        return points[idx];
    }

    const uinteger& GetSize() const{ return points.size(); }

 

    void Add(const Vector3& p){
        points.push_back(p);
    }

 

    void Remove(uinteger at){

        points.erase(points.begin()+at);
    }

 

    Vector3& Last(){ return points.back(); }
};

 

const Vector3 FarthestPointInDirection(const std::vector<Vector3>& pointSet, const Vector3& direction){
    integer idx = 0;
    real maxd = -REAL_MAX;
    uinteger size = pointSet.size();
    for (uinteger i = 0; i < size; i++){
        real d = direction.Dot(pointSet[i]);
        if (maxd < d){
            maxd = d;
            idx = i;
        }
    }
    return pointSet[idx];
}

 

// return the farthest point in direction at the set of minkowski sum of two set A and B.
const Vector3 SupportFunc(const std::vector<Vector3>& setA, const std::vector<Vector3>& setB, const Vector3& direction){
    const Vector3 farA = FarthestPointInDirection(setA, direction);
    const Vector3 farB = FarthestPointInDirection(setB, -direction);

    return farA - farB;
}

 

// check whether the simplex contains origin or not. if it doesn't, update simplex in a possible way.

bool UpdateSimplex(Simplex& simplex, Vector3& dir){
    const Vector3& A = simplex[3];
    const Vector3& B = simplex[2];
    const Vector3& C = simplex[1];
    const Vector3& D = simplex[0];
    Vector3 AO = -A;
    Vector3 AB = B - A;
    Vector3 AC = C - A;
    Vector3 AD = D - A;

    if (dir.Dot((C - B).Cross(D - B)) > 0){
        // A is above BCD. normal of ABC, ADB, ACD are candidates
        dir = AB.Cross(AC);
        if (AO.Dot(dir) > 0){
            simplex.Remove(0);
        }
        else{
            dir = AD.Cross(AB);
            if (AO.Dot(dir) > 0){
                simplex.Remove(1);
            }
            else{
                dir = AC.Cross(AD);
                if (AO.Dot(dir) > 0){
                    simplex.Remove(2);
                }
                else{
                    // origin is in the tetrahedron ABCD
                    return true;
                }
            }
        }
    }
    else{
        // A is under BCD. normal of ACB, ABD, ADC are candidates
        dir = AC.Cross(AB);
        if (AO.Dot(dir) > 0){
            simplex.Remove(0);
        }
        else{
            dir = AB.Cross(AD);
            if (AO.Dot(dir) > 0){
                simplex.Remove(1);
            }
            else{
                dir = AD.Cross(AC);
                if (AO.Dot(dir) > 0){
                    simplex.Remove(2);
                }
                else{
                    // origin is in the tetrahedron ABCD
                    return true;
                }
            }
        }
    }
    return false;
}

 

// do GJK process.
// it returns true if two convex shape(in world space) A and B are intersected each other.
// and the last simplex will be stored in 'out_simplex' buffer on true cases.
bool DoGJK(const std::vector<Vector3>& setA, const std::vector<Vector3>&setB, Simplex& out_simplex){
    Simplex& simplex = out_simplex;

    // initialize simplex
    // point D
    Vector3 dir = Vector3::Right;
    simplex.Add(SupportFunc(setA, setB, dir));

    if (dir.Dot(simplex.Last()) < 0)

        return false;

 

    // point C
    dir = -simplex[0];
    simplex.Add(SupportFunc(setA, setB, dir));
    if (dir.Dot(simplex.Last()) < 0)
        return false;
   
    // point B
    //dir = (D - C) x (O - C)
    dir = (simplex[0] - simplex[1]).Cross(-simplex[1]);
    if (dir == Vector3(0, 0, 0)){

        // * you should to complete the simplex if you want to

        // do EPA input as this simplex.
        return true;
    }
    simplex.Add(SupportFunc(setA, setB, dir));
   

    // point A (compute direction only)
    do{
        // dir = (C - B) x (D - B)
        dir = (simplex[1] - simplex[2]).Cross(simplex[0] - simplex[2]);

        // check location of origin
        int8 loc = PointLocationByPlane(Vector3(0), dir, simplex[2]);
        if (loc == 0){
            // adjust point B
            simplex[2] = SupportFunc(setA, setB, dir);
            continue;
        }
        if (loc < 0){
            dir = -dir;
        }
        break;
    } while (true);
   
    // iteration
    while (true){
        simplex.Add(SupportFunc(setA, setB, dir));
        if (dir.Dot(simplex.Last()) < 0){
            return false;
        }

        if (UpdateSimplex(simplex, dir)){
            return true;
        }
    }
}

 

 

 

 

GJK 수행 후 simplex를 결과로 반환한다. 이는 일반적으로 반복되는 충돌검사는 프레임 사이에 비슷한 상태를 검사하게 되고, 같은 simplex를 결과로 가질 확률이 높다는 사실로 부터 충돌검사 성능을 높일 수 있는 요소로 사용할 수 있기 때문이다.

또한 GJK만으론 구할 수 없는 특징들을 구하기 위해 보충 알고리즘인 EPA(Expanding Polytope Algorithm)를 수행할 수 있는데, GJK의 결과는 EPA의 초기값으로 사용하기에 적절하다.