The basic concept

Minkowski difference

Convex bodyand

Two bodies intersect if and only if their Minkowski difference contains the origin.

Simplex

This is equivalent to iterating within the Minkowski difference to form a polyhedron containing the origin as much as possible. If it contains the origin, then the Minkowski difference contains the origin and the objects intersect.

Support functions

The object is in the directionThe farthest point on

A theorem

algorithm

functionGJK_intersection(shape p, shape q, vector initial_axis): Vector A := Support(p, initial_axis) − Support(q, initial_axis) simplex s := {A} vector D := −A loop: A := Support(p, D) − Support(q, D)ifDot (A, D) < 0: reject S := s union A s, D, contains_origin := SimplexOrigin(s)if contains_origin:
            accept
Copy the code

The origin determine

Line segment

triangle

The origin in theRegion, if and only if.The direction of theta is perpendicular to thetaPoint to the origin for the next search.

Tetrahedron

The origin is in the tetrahedral body if and only if the origin is also in the plane, faceandInternal area. More pointThe projection and point of the vector to each vertex on the normal direction of the face opposite the vertexThe projection of the vector to the origin upward in this method determines whether the origin is in the inner region of the plane.

The origin in theIn-plane region if and only if

If the origin is not in the tetrahedral body, it must be in one of the out-of-plane regions, delete the vertices of the face, the rest into the previous step of the triangle determination procedure.

The program

constexpr float EPSILON = 1e-6f;

Vector3f MinkowskiDifferenceSupport(const PolyhedralConvexShape &shape1, 
    const PolyhedralConvexShape &shape2, const Vector3f &dir)
{
    Vector3f a = shape1.localGetSupportingVertex(dir);
    Vector3f b = shape2.localGetSupportingVertex(-dir);
    return a - b;
}

bool Simplex2(vector<Vector3f> &s, Vector3f &d)
{
    Vector3f A = s[1];
    Vector3f B = s[0];
    Vector3f AB = B - A;
    Vector3f AO =   - A;
    Vector3f ABOO = AB.cross(AO).cross(AO); // norm to AB toward origin

    if(ABOO.norm() <= EPSILON) // origin lies on
    {
        return true;
    }
    else
    {
        d = ABOO;
        return false; }}bool Simplex3(vector<Vector3f> &s, Vector3f &d)
{
    Vector3f A = s[2];
    Vector3f B = s[1];
    Vector3f C = s[0];
    Vector3f AB = B - A;
    Vector3f AC = C - A;
    Vector3f AO =   - A;
    Vector3f ABC = AB.cross(AC);
    Vector3f ACBB = AC.cross(AB).cross(AB); // norm to AB toward far from C
    Vector3f ABCC = AB.cross(AC).cross(AC); // norm to AC toward far from B

    if(ACBB.dot(AO) > EPSILON) // origin lies on outside of AB
    {
        d = ACBB;
        return false;
    }

    if (ABCC.dot(AO) > EPSILON) // origin lies on outside of AC
    {
        d = ABCC;
        return false;
    }

    float dot = ABC.dot(AO); // origin lies in ABC region
    if(dot > EPSILON)
    {
        d = ABC;
        return false;
    }
    else if(dot < EPSILON)
    {
        d = -ABC;
        return false;
    }
    else // origin lies in ABC triangle
    {
        return true; }}bool Simplex4(vector<Vector3f> &s, Vector3f &d)
{
    Vector3f A = s[3];
    Vector3f B = s[2];
    Vector3f C = s[1];
    Vector3f D = s[0];

    Vector3f AO =   - A;
    Vector3f AB = B - A;
    Vector3f AC = C - A;
    Vector3f AD = D - A;

    Vector3f ABC = AB.cross(AC); // normal to ABC
    Vector3f ACD = AC.cross(AD); // normal to ACD
    Vector3f ADB = AD.cross(AB); // normal to ADB

    float AD_ABC = ABC.dot(AD); // AD project on normal of ABC
    float AB_ACD = ACD.dot(AB); // AB project on normal of ACD
    float AC_ADB = ADB.dot(AC); // AC project on normal of ADB

    float AO_ABC = ABC.dot(AO); // AO project on normal of ABC
    float AO_ACD = ACD.dot(AO); // AO project on normal of ACD
    float AO_ADB = ADB.dot(AO); // AO project on normal of ADB

    bool inside_ABC = AD_ABC * AO_ABC > EPSILON;
    bool inside_ACD = AB_ACD * AO_ACD > EPSILON;
    bool inside_ADB = AC_ADB * AO_ADB > EPSILON;

    if(inside_ABC && inside_ACD && inside_ADB) // origin inside tetrahedron
    {
        return true;
    }
    else if(! inside_ABC)// origin outside ABC
    {
        // remove D
        s[0] = s[1];
        s[1] = s[2];
        s[2] = s[3];
        s.pop_back();
    }
    else if(! inside_ACD)// origin outside ACD
    {
        // remove B
        s[2] = s[3];
        s.pop_back();
    }
    else // origin outside ADB
    {
        // remove C
        s[1] = s[2];
        s[2] = s[3];
        s.pop_back();
    }

    return Simplex3(s, d);
}

bool SimplexOrigin(vector<Vector3f> &s, Vector3f &d)
{
    bool contain_origin = false;
    switch (s.size())
    {
    case 2: // line segment
        contain_origin = Simplex2(s, d);
        break;
    case 3: // triangle
        contain_origin = Simplex3(s, d);
        break;
    case 4: // tetrahedron
        contain_origin = Simplex4(s, d);
        break;
    default:
        break;
    }

    return contain_origin;
}

bool gjkAlgorithm(const PolyhedralConvexShape &shape1, 
    const PolyhedralConvexShape &shape2)
{
    // center difference as initial direction
    Vector3f dir = shape1.getCenter() - shape2.getCenter();
    if(dir.norm() < 0.001 f)
        dir = Vector3f{1.0 f.0.f.0.f};
    dir.normalize();

    // init
    vector<Vector3f> simplex;
    simplex.reserve(4);

    Vector3f simplex_point = MinkowskiDifferenceSupport(shape1, shape2, dir);
    simplex.push_back(simplex_point);
    dir = -simplex_point;

    // main loop
    int max_iteration = max(shape1.getNumVertices(), shape2.getNumVertices());
    while(max_iteration-- > 0)
    {
        simplex_point = MinkowskiDifferenceSupport(shape1, shape2, dir);

        if(simplex_point.dot(dir) < 0)
            return false;

        simplex.push_back(simplex_point);

        if(SimplexOrigin(simplex, dir))
            return true;
    }

    return false;
}
Copy the code

test

reference

[1] dyn4j: GJK (Gilbert – Johnson – Keerthi)
[2] BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp
[3] Wiki: Gilbert — Johnson — Keerthi Distance algorithm
[4] E. G. Gilbert, D. W. Johnson and S. S. Keerthi, “A fast procedure for computing the distance between complex objects in three-dimensional space,” in IEEE Journal on Robotics and Automation, Vol. 4, No.2, pp. 193-203, April 1988, doi: 10.1109/56.2083
[5] Christer Ericson. 2004. Real-Time Collision Detection. CRC Press, Inc. USA. Chapter 9.5, pp. 399-410.

mo4tech.com (Moment For Technology) is a global community with thousands techies from across the global hang out!Passionate technologists, be it gadget freaks, tech enthusiasts, coders, technopreneurs, or CIOs, you would find them all here.

Collision detection — GJK algorithm

The basic concept

Minkowski difference

Simplex

Support functions

algorithm