3D Gilbert–Johnson–Keerthi (GJK) distance algorithm

_GJK.md

Gilbert–Johnson–Keerthi (GJK) 3D distance algorithm

The Gilbert–Johnson–Keerthi (GJK) distance algorithm is a method of determining the minimum distance between two convex sets. The algorithm's stability, speed which operates in near-constant time, and small storage footprint make it popular for realtime collision detection.

Unlike many other distance algorithms, it has no requirments on geometry data to be stored in any specific format, but instead relies solely on a support function to iteratively generate closer simplices to the correct answer using the Minkowski sum (CSO) of two convex shapes.

GJK algorithms are used incrementally. In this mode, the final simplex from a previous solution is used as the initial guess in the next iteration. If the positions in the new frame are close to those in the old frame, the algorithm will converge in one or two iterations.

Features

• GJK 3D distance algorithm implementation useful for collision detection
• No dependencies on other libraries (including standard library)
• Easy to embed (single .h and .c file)
• Unabstracted iterative API. Does not require any specific primitives to be passed. Instead just relies on provided support data structure.
• Easy to create debug functionality by storing each simplex iteration inside an array. Allows to run algorithm and draw output in single steps.
• No specific math datastructures to convert to and from. Instead everything is just float arrays

Usage

1. Fill gjk_support struct with initial vertex positions a and b one for each polyhedron.
2. Calculate distance vector between points a and b.
3. Create zeroed out gjk_simplex struct.
4. Optionally set maximum number of iterations to run. If not set will be set to default value.
5. Call function gjk in a loop until it returns 0 and pass filled out gjk_support and distance vector.
6. Call support function for first polyhedron with from gjk returned negative direction vector.
7. Store vertex furthest along the negative direction vector in first polyhedron in gjk_support.
8. Store vertex index furthest along the negative direction vector in first polyhedron into a in gjk_support.
9. Call support function for second polyhedron with returned director vector returned from gjk.
10. Store vertex furthest along the direction vector in second polyhedron into b inside gjk_support.
11. Store vertex index furthest along the direction vector in second polyhedron into gjk_support.
12. Calculate distance vector between points a and b.
13. Run next iteration of 'gjk' until it returns '0'.
14. Check gjk_simplex if we have a collision by checking variable hit is non-zero.
15. Optionally call gjk_analyze to calculate closest points and distance between polyhedrons.
16. Optionally for quadratic polyhedrons like spheres gjk_quad calculates correct closest point and distance.

Compilation

Just copy and paste the single .h and .c into your project and compile. This project also features examples showcasing how to use this implementation.

Examples

1. The first example xample_simple.c showcases a simple use cases of collision detection between a polyhedron and a capsule. It is made out of two support functions one for the polyhedron and one for the capsule as well as a function to check for a collision.

   The collision funtion itself runs the GJK algorithm to calculate the distance between the polhedron and the capsule internal line representation. However since we want to check the capsule and not just the line representation it also checks the distance between both polytopes and only notifies of a collision if the distance is smaller or equal the capsule radius.

2. The second example xample_transform.c implements a polyhedron to polyhedron collision detection check and extends the algorithm showcased in the first example by adding transformation for both polytopes with translation and rotation in form of a 3x3 rotation matrix. It is made out of a single support function for polyhedrons as well as function to calculate the transformation and inverse transformation of a point.

   The collision function is similar to the first example with one obvious difference: transformations. Instead of transforming all vertexes of a polyhedron only the currently processed vertexes are transformed.This is more performant but also requires the direction passed to the support function to be transformed as well.

3. The third example xample_xdebug.c showcases how to create debug visualization. One interesting property of having a iterative API with fixed upper bound is that each iteration output can be safed and used for debug drawing visualization without having to rely on command buffers or coroutines to draw information. The example contains three function. One for calculating the support point of polyhedron, one function to calculate the simplex of each iteration and finally a function to draw a specific iteration.

4. The final example xample_xmov.c combines the second example with moving objects by adding two additional arguments for object translation. Adding the concept of movement to GJK is relatively straight forward. For collision we just combine each object before and after applying the translation. However there is a small optimization. If we consider our translation t in context of our current direction d. Only if both point in the same direction we need to consider the translated object (Real time Collision Detection Page.: 409).

Support Functions

• Support functions are the core of the algorithm and main abstraction over convex polytopes
• Need to calculate both vertex position and index furthest along a direction.
• Polytropes like polyhedrons and AABBs only require calculating max dot product between all points and the direction (implemented by a single support function taking a number of vertexes, looping and calculating the max dot product)
• Quadric shaped collision volumes like spheres and capsules rely on the distance measured of capsule internal line or sphere center for collision testing

The reason why support functions are not part of the GJK algorithm implemenation is abstraction. One of the interesting properties of GJK is that it does not depend on any polytope abstraction implementation. Therefore it is possible for example to transform (scale, rotate, move, shear, ...) any object by just transforming the support vertexes without modifing the GJK algorithm. Which is both more performant and grants more control in total.

Implementation notes

• Main termination criteria is a duplicated simplex vertex and not a directional check.
• Caseys implementation of using planes to check for the correct voroni region is not numerical safe.
• Caseys optimizations of only taking some of the voronoi regions into account produces wrong results in some cases.
• Instead of planes this implementation uses barycentric coordinates, areas and volumes for voronoi region determination.
• In each iterations all voronoi regions are checked to be as numerical safe as possible
• While the duplication check is enough for 2D it will hit the maximum iteration in 3D. To fix that this implementation additionally validates that distance between polytopes over each iteration is getting smaller.

Debugging

Due to the iterative nature of this API it is possible to store each simplex iteration inside an array with size of the maximum number of iterations (either set directly or GJK_MAX_ITERATIONS). Each iteration step can be passed to gjk_analyze to generate information like currently proccessed points, current distance and simplex shape. All information can be used to draw debug information without having to store draw commands or do coroutine style debug drawing.

References

• Erin Cattos presentation on 2D GJK
• Randy Gauls 3D GJK implementation
• Casey Muratoris video lecture
• Christer Ericson Real Time Collision Detection

Special Thanks

• Randy Gaul (@RandyPGaul)
• Erin Catto (@erin_catto)
• Per Vognsen (@pervognsen)
• Casey Muratori (@cmuratori)

gjk.c

#include "gjk.h"
#include <assert.h>

#define GJK_FLT_MAX 3.40282347E+38F
#define GJK_EPSILON 1.19209290E-07F

static const float f3z[3];
#define fop(r,e,a,p,b,i,s) (r) e ((a) p (b)) i (s)
#define f3op(r,e,a,p,b,i,s) do {\
    fop((r)[0],e,(a)[0],p,(b)[0],i,s),\
    fop((r)[1],e,(a)[1],p,(b)[1],i,s),\
    fop((r)[2],e,(a)[2],p,(b)[2],i,s);}while(0)
#define f3cpy(d,s) (d)[0]=(s)[0],(d)[1]=(s)[1],(d)[2]=(s)[2]
#define f3add(d,a,b) f3op(d,=,a,+,b,+,0)
#define f3sub(d,a,b) f3op(d,=,a,-,b,+,0)
#define f3mul(d,a,s) f3op(d,=,a,+,f3z,*,s)
#define f3dot(a,b) ((a)[0]*(b)[0]+(a)[1]*(b)[1]+(a)[2]*(b)[2])
#define f3cross(d,a,b) do {\
    (d)[0] = ((a)[1]*(b)[2]) - ((a)[2]*(b)[1]),\
    (d)[1] = ((a)[2]*(b)[0]) - ((a)[0]*(b)[2]),\
    (d)[2] = ((a)[0]*(b)[1]) - ((a)[1]*(b)[0]);}while(0)

static inline float
f3box(const float *a, const float *b, const float *c)
{
    float n[3];
    f3cross(n, a, b);
    return f3dot(n, c);
}
static float
inv_sqrt(float n)
{
    union {unsigned u; float f;} conv; conv.f = n;
    conv.u = 0x5f375A84 - (conv.u >> 1);
    conv.f = conv.f * (1.5f - (n * 0.5f * conv.f * conv.f));
    return conv.f;
}
extern int
gjk(struct gjk_simplex *s, struct gjk_support *sup)
{
    assert(s);
    assert(sup);
    if (!s || !sup) return 0;
    if (s->max_iter > 0 && s->iter >= s->max_iter)
        return 0;

    /* I.) Initialize */
    if (s->cnt == 0) {
        s->D = GJK_FLT_MAX;
        s->max_iter = !s->max_iter ? GJK_MAX_ITERATIONS: s->max_iter;
    }
    /* II.) Check for duplications */
    for (int i = 0; i < s->cnt; ++i) {
        if (sup->aid != s->v[i].aid) continue;
        if (sup->bid != s->v[i].bid) continue;
        return 0;
    }
    /* III.) Add vertex into simplex */
    struct gjk_vertex *vert = &s->v[s->cnt];
    f3cpy(vert->a, sup->a);
    f3cpy(vert->b, sup->b);
    f3sub(vert->p, sup->b, sup->a);
    vert->aid = sup->aid;
    vert->bid = sup->bid;
    s->bc[s->cnt++] = 1.0f;

    /* IV.) Find closest simplex point */
    switch (s->cnt) {
    case 1: break;
    case 2: {
        /* -------------------- Line ----------------------- */
        float a[3]; f3cpy(a, s->v[0].p);
        float b[3]; f3cpy(b, s->v[1].p);

        /* compute barycentric coordinates */
        float ab[3]; f3sub(ab, a, b);
        float ba[3]; f3sub(ba, b, a);

        float u = f3dot(b, ba);
        float v = f3dot(a, ab);
        if (v <= 0.0f) {
            /* region A */
            s->bc[0] = 1.0f;
            s->cnt = 1;
            break;
        }
        if (u <= 0.0f) {
            /* region B */
            s->v[0] = s->v[1];
            s->bc[0] = 1.0f;
            s->cnt = 1;
            break;
        }
        /* region AB */
        s->bc[0] = u;
        s->bc[1] = v;
        s->cnt = 2;
    } break;
    case 3: {
        /* -------------------- Triangle ----------------------- */
        float a[3]; f3cpy(a, s->v[0].p);
        float b[3]; f3cpy(b, s->v[1].p);
        float c[3]; f3cpy(c, s->v[2].p);

        float ab[3]; f3sub(ab, a, b);
        float ba[3]; f3sub(ba, b, a);
        float bc[3]; f3sub(bc, b, c);
        float cb[3]; f3sub(cb, c, b);
        float ca[3]; f3sub(ca, c, a);
        float ac[3]; f3sub(ac, a, c);

        /* compute barycentric coordinates */
        float u_ab = f3dot(b, ba);
        float v_ab = f3dot(a, ab);

        float u_bc = f3dot(c, cb);
        float v_bc = f3dot(b, bc);

        float u_ca = f3dot(a, ac);
        float v_ca = f3dot(c, ca);

        if (v_ab <= 0.0f && u_ca <= 0.0f) {
            /* region A */
            s->bc[0] = 1.0f;
            s->cnt = 1;
            break;
        }
        if (u_ab <= 0.0f && v_bc <= 0.0f) {
            /* region B */
            s->v[0] = s->v[1];
            s->bc[0] = 1.0f;
            s->cnt = 1;
            break;
        }
        if (u_bc <= 0.0f && v_ca <= 0.0f) {
            /* region C */
            s->v[0] = s->v[2];
            s->bc[0] = 1.0f;
            s->cnt = 1;
            break;
        }
        /* calculate fractional area */
        float n[3]; f3cross(n, ba, ca);
        float n1[3]; f3cross(n1, b, c);
        float n2[3]; f3cross(n2, c, a);
        float n3[3]; f3cross(n3, a, b);

        float u_abc = f3dot(n1, n);
        float v_abc = f3dot(n2, n);
        float w_abc = f3dot(n3, n);

        if (u_ab > 0.0f && v_ab > 0.0f && w_abc <= 0.0f) {
            /* region AB */
            s->bc[0] = u_ab;
            s->bc[1] = v_ab;
            s->cnt = 2;
            break;
        }
        if (u_bc > 0.0f && v_bc > 0.0f && u_abc <= 0.0f) {
            /* region BC */
            s->v[0] = s->v[1];
            s->v[1] = s->v[2];
            s->bc[0] = u_bc;
            s->bc[1] = v_bc;
            s->cnt = 2;
            break;
        }
        if (u_ca > 0.0f && v_ca > 0.0f && v_abc <= 0.0f) {
            /* region CA */
            s->v[1] = s->v[0];
            s->v[0] = s->v[2];
            s->bc[0] = u_ca;
            s->bc[1] = v_ca;
            s->cnt = 2;
            break;
        }
        /* region ABC */
        assert(u_abc > 0.0f && v_abc > 0.0f && w_abc > 0.0f);
        s->bc[0] = u_abc;
        s->bc[1] = v_abc;
        s->bc[2] = w_abc;
        s->cnt = 3;
    } break;
    case 4: {
        /* -------------------- Tetrahedron ----------------------- */
        float a[3]; f3cpy(a, s->v[0].p);
        float b[3]; f3cpy(b, s->v[1].p);
        float c[3]; f3cpy(c, s->v[2].p);
        float d[3]; f3cpy(d, s->v[3].p);

        float ab[3]; f3sub(ab, a, b);
        float ba[3]; f3sub(ba, b, a);
        float bc[3]; f3sub(bc, b, c);
        float cb[3]; f3sub(cb, c, b);
        float ca[3]; f3sub(ca, c, a);
        float ac[3]; f3sub(ac, a, c);

        float db[3]; f3sub(db, d, b);
        float bd[3]; f3sub(bd, b, d);
        float dc[3]; f3sub(dc, d, c);
        float cd[3]; f3sub(cd, c, d);
        float da[3]; f3sub(da, d, a);
        float ad[3]; f3sub(ad, a, d);

        /* compute barycentric coordinates */
        float u_ab = f3dot(b, ba);
        float v_ab = f3dot(a, ab);

        float u_bc = f3dot(c, cb);
        float v_bc = f3dot(b, bc);

        float u_ca = f3dot(a, ac);
        float v_ca = f3dot(c, ca);

        float u_bd = f3dot(d, db);
        float v_bd = f3dot(b, bd);

        float u_dc = f3dot(c, cd);
        float v_dc = f3dot(d, dc);

        float u_ad = f3dot(d, da);
        float v_ad = f3dot(a, ad);

        /* check verticies for closest point */
        if (v_ab <= 0.0f && u_ca <= 0.0f && v_ad <= 0.0f) {
            /* region A */
            s->bc[0] = 1.0f;
            s->cnt = 1;
            break;
        }
        if (u_ab <= 0.0f && v_bc <= 0.0f && v_bd <= 0.0f) {
            /* region B */
            s->v[0] = s->v[1];
            s->bc[0] = 1.0f;
            s->cnt = 1;
            break;
        }
        if (u_bc <= 0.0f && v_ca <= 0.0f && u_dc <= 0.0f) {
            /* region C */
            s->v[0] = s->v[2];
            s->bc[0] = 1.0f;
            s->cnt = 1;
            break;
        }
        if (u_bd <= 0.0f && v_dc <= 0.0f && u_ad <= 0.0f) {
            /* region D */
            s->v[0] = s->v[3];
            s->bc[0] = 1.0f;
            s->cnt = 1;
            break;
        }
        /* calculate fractional area */
        float n[3]; f3cross(n, da, ba);
        float n1[3]; f3cross(n1, d, b);
        float n2[3]; f3cross(n2, b, a);
        float n3[3]; f3cross(n3, a, d);

        float u_adb = f3dot(n1, n);
        float v_adb = f3dot(n2, n);
        float w_adb = f3dot(n3, n);

        f3cross(n, ca, da);
        f3cross(n1, c, d);
        f3cross(n2, d, a);
        f3cross(n3, a, c);

        float u_acd = f3dot(n1, n);
        float v_acd = f3dot(n2, n);
        float w_acd = f3dot(n3, n);

        f3cross(n, bc, dc);
        f3cross(n1, b, d);
        f3cross(n2, d, c);
        f3cross(n3, c, b);

        float u_cbd = f3dot(n1, n);
        float v_cbd = f3dot(n2, n);
        float w_cbd = f3dot(n3, n);

        f3cross(n, ba, ca);
        f3cross(n1, b, c);
        f3cross(n2, c, a);
        f3cross(n3, a, b);

        float u_abc = f3dot(n1, n);
        float v_abc = f3dot(n2, n);
        float w_abc = f3dot(n3, n);

        /* check edges for closest point */
        if (w_abc <= 0.0f && v_adb <= 0.0f && u_ab > 0.0f && v_ab > 0.0f) {
            /* region AB */
            s->bc[0] = u_ab;
            s->bc[1] = v_ab;
            s->cnt = 2;
            break;
        }
        if (u_abc <= 0.0f && w_cbd <= 0.0f && u_bc > 0.0f && v_bc > 0.0f) {
            /* region BC */
            s->v[0] = s->v[1];
            s->v[1] = s->v[2];
            s->bc[0] = u_bc;
            s->bc[1] = v_bc;
            s->cnt = 2;
            break;
        }
        if (v_abc <= 0.0f && w_acd <= 0.0f && u_ca > 0.0f && v_ca > 0.0f) {
            /* region CA */
            s->v[1] = s->v[0];
            s->v[0] = s->v[2];
            s->bc[0] = u_ca;
            s->bc[1] = v_ca;
            s->cnt = 2;
            break;
        }
        if (v_cbd <= 0.0f && u_acd <= 0.0f && u_dc > 0.0f && v_dc > 0.0f) {
            /* region DC */
            s->v[0] = s->v[3];
            s->v[1] = s->v[2];
            s->bc[0] = u_dc;
            s->bc[1] = v_dc;
            s->cnt = 2;
            break;
        }
        if (v_acd <= 0.0f && w_adb <= 0.0f && u_ad > 0.0f && v_ad > 0.0f) {
            /* region AD */
            s->v[1] = s->v[3];
            s->bc[0] = u_ad;
            s->bc[1] = v_ad;
            s->cnt = 2;
            break;
        }
        if (u_cbd <= 0.0f && u_adb <= 0.0f && u_bd > 0.0f && v_bd > 0.0f) {
            /* region BD */
            s->v[0] = s->v[1];
            s->v[1] = s->v[3];
            s->bc[0] = u_bd;
            s->bc[1] = v_bd;
            s->cnt = 2;
            break;
        }
        /* calculate fractional volume (volume can be negative!) */
        float denom = f3box(cb, ab, db);
        float volume = (denom == 0) ? 1.0f: 1.0f/denom;
        float u_abcd = f3box(c, d, b) * volume;
        float v_abcd = f3box(c, a, d) * volume;
        float w_abcd = f3box(d, a, b) * volume;
        float x_abcd = f3box(b, a, c) * volume;

        /* check faces for closest point */
        if (x_abcd < 0.0f && u_abc > 0.0f && v_abc > 0.0f && w_abc > 0.0f) {
            /* region ABC */
            s->bc[0] = u_abc;
            s->bc[1] = v_abc;
            s->bc[2] = w_abc;
            s->cnt = 3;
            break;
        }
        if (u_abcd < 0.0f && u_cbd > 0.0f && v_cbd > 0.0f && w_cbd > 0.0f) {
            /* region CBD */
            s->v[0] = s->v[2];
            s->v[2] = s->v[3];
            s->bc[0] = u_cbd;
            s->bc[1] = v_cbd;
            s->bc[2] = w_cbd;
            s->cnt = 3;
            break;
        }
        if (v_abcd < 0.0f && u_acd > 0.0f && v_acd > 0.0f && w_acd > 0.0f) {
            /* region ACD */
            s->v[1] = s->v[2];
            s->v[2] = s->v[3];
            s->bc[0] = u_acd;
            s->bc[1] = v_acd;
            s->bc[2] = w_acd;
            s->cnt = 3;
            break;
        }
        if (w_abcd < 0.0f && u_adb > 0.0f && v_adb > 0.0f && w_adb > 0.0f) {
            /* region ADB */
            s->v[2] = s->v[1];
            s->v[1] = s->v[3];
            s->bc[0] = u_adb;
            s->bc[1] = v_adb;
            s->bc[2] = w_adb;
            s->cnt = 3;
            break;
        }
        /* region ABCD */
        assert(u_abcd >= 0.0f && v_abcd >= 0.0f && w_abcd >= 0.0f && x_abcd >= 0.0f);
        s->bc[0] = u_abcd;
        s->bc[1] = v_abcd;
        s->bc[2] = w_abcd;
        s->bc[3] = x_abcd;
        s->cnt = 4;
    } break;}

    /* V.) Check if origin is enclosed by tetrahedron */
    if (s->cnt == 4) {
        s->hit = 1;
        return 0;
    }
    /* VI.) Ensure closing in on origin to prevent multi-step cycling */
    float pnt[3], denom = 0;
    for (int i = 0; i < s->cnt; ++i)
        denom += s->bc[i];
    denom = 1.0f / denom;

    switch (s->cnt) {
    case 1: f3cpy(pnt, s->v[0].p); break;
    case 2: {
        /* --------- Line -------- */
        float a[3]; f3mul(a, s->v[0].p, denom * s->bc[0]);
        float b[3]; f3mul(b, s->v[1].p, denom * s->bc[1]);
        f3add(pnt, a, b);
    } break;
    case 3: {
        /* ------- Triangle ------ */
        float a[3]; f3mul(a, s->v[0].p, denom * s->bc[0]);
        float b[3]; f3mul(b, s->v[1].p, denom * s->bc[1]);
        float c[3]; f3mul(c, s->v[2].p, denom * s->bc[2]);

        f3add(pnt, a, b);
        f3add(pnt, pnt, c);
    } break;}

    float d2 = f3dot(pnt, pnt);
    if (d2 >= s->D) return 0;
    s->D = d2;

    /* VII.) New search direction */
    float d[3] = {0};
    switch (s->cnt) {
    default: assert(0); break;
    case 1: {
        /* --------- Point -------- */
        f3mul(d, s->v[0].p, -1);
    } break;
    case 2: {
        /* ------ Line segment ---- */
        float ba[3]; f3sub(ba, s->v[1].p, s->v[0].p);
        float b0[3]; f3mul(b0, s->v[1].p, -1);
        float t[3];  f3cross(t, ba, b0);
        f3cross(d, t, ba);
    } break;
    case 3: {
        /* ------- Triangle ------- */
        float ab[3]; f3sub(ab, s->v[1].p, s->v[0].p);
        float ac[3]; f3sub(ac, s->v[2].p, s->v[0].p);
        float n[3];  f3cross(n, ab, ac);
        if (f3dot(n, s->v[0].p) <= 0.0f)
            f3cpy(d, n);
        else f3mul(d, n, -1);
    }}
    if (f3dot(d,d) < GJK_EPSILON * GJK_EPSILON)
        return 0;

    f3mul(sup->da, d, -1.0f);
    f3cpy(sup->db, d);
    return 1;
}
extern void
gjk_analyze(struct gjk_result *res, const struct gjk_simplex *s)
{
    res->iterations = s->iter;
    res->hit = s->hit;

    /* calculate normalization denominator */
    float denom = 0;
    for (int i = 0; i < s->cnt; ++i)
        denom += s->bc[i];
    denom = 1.0f / denom;

    /* compute closest points */
    switch (s->cnt) {
    default: assert(0); break;
    case 1: {
        /* Point */
        f3cpy(res->p0, s->v[0].a);
        f3cpy(res->p1, s->v[0].b);
    } break;
    case 2: {
        /* Line */
        float as = denom * s->bc[0];
        float bs = denom * s->bc[1];

        float a[3]; f3mul(a, s->v[0].a, as);
        float b[3]; f3mul(b, s->v[1].a, bs);
        float c[3]; f3mul(c, s->v[0].b, as);
        float d[3]; f3mul(d, s->v[1].b, bs);

        f3add(res->p0, a, b);
        f3add(res->p1, c, d);
    } break;
    case 3: {
        /* Triangle */
        float as = denom * s->bc[0];
        float bs = denom * s->bc[1];
        float cs = denom * s->bc[2];

        float a[3]; f3mul(a, s->v[0].a, as);
        float b[3]; f3mul(b, s->v[1].a, bs);
        float c[3]; f3mul(c, s->v[2].a, cs);

        float d[3]; f3mul(d, s->v[0].b, as);
        float e[3]; f3mul(e, s->v[1].b, bs);
        float f[3]; f3mul(f, s->v[2].b, cs);

        f3add(res->p0, a, b);
        f3add(res->p0, res->p0, c);

        f3add(res->p1, d, e);
        f3add(res->p1, res->p1, f);
    } break;
    case 4: {
        /* Tetrahedron */
        float a[3]; f3mul(a, s->v[0].a, denom * s->bc[0]);
        float b[3]; f3mul(b, s->v[1].a, denom * s->bc[1]);
        float c[3]; f3mul(c, s->v[2].a, denom * s->bc[2]);
        float d[3]; f3mul(d, s->v[3].a, denom * s->bc[3]);

        f3add(res->p0, a, b);
        f3add(res->p0, res->p0, c);
        f3add(res->p0, res->p0, d);
        f3cpy(res->p1, res->p0);
    } break;}

    if (!res->hit) {
        /* compute distance */
        float d[3]; f3sub(d, res->p1, res->p0);
        res->distance_squared = f3dot(d, d);
    } else res->distance_squared = 0;
}
extern void
gjk_quad(struct gjk_result *res, float a_radius, float b_radius)
{
    float radius = a_radius + b_radius;
    float radius_squared = radius * radius;
    if (res->distance_squared > GJK_EPSILON &&
        res->distance_squared > radius_squared) {
        res->distance_squared -= radius_squared;

        /* calculate normal */
        float n[3]; f3sub(n, res->p1, res->p0);
        float l2 = f3dot(n, n);
        if (l2 != 0.0f) {
            float il = inv_sqrt(l2);
            f3mul(n,n,il);
        }
        float da[3]; f3mul(da, n, a_radius);
        float db[3]; f3mul(db, n, b_radius);

        /* calculate new collision points */
        f3add(res->p0, res->p0, da);
        f3sub(res->p1, res->p1, db);
    } else {
        float p[3]; f3add(p, res->p0, res->p1);
        f3mul(res->p0, p, 0.5f);
        f3cpy(res->p1, res->p0);
        res->distance_squared = 0;
        res->hit = 1;
    }
}

gjk.h

#ifndef GJK_H
#define GJK_H

#define GJK_MAX_ITERATIONS 20

struct gjk_support {
    int aid, bid;   /* in */
    float a[3];     /* in */
    float b[3];     /* in */
    float da[3];    /* out */
    float db[3];    /* out */
};
struct gjk_simplex {
    int max_iter, iter;
    int hit, cnt;
    struct gjk_vertex {
        float a[3];
        float b[3];
        float p[3];
        int aid, bid;
    } v[4];
    float bc[4], D;
};
struct gjk_result {
    int hit;
    float p0[3];
    float p1[3];
    float distance_squared;
    int iterations;
};
extern int gjk(struct gjk_simplex *s, struct gjk_support *sup);
extern void gjk_analyze(struct gjk_result *res, const struct gjk_simplex *s);
extern void gjk_quad(struct gjk_result *res, float a_radius, float b_radius);

#endif

xample_simple.c

#include "gjk.h"

static int
line_support(float *support, const float *d,
    const float *a, const float *b)
{
    int i = 0;
    if (f3dot(a, d) < f3dot(b, d)) {
        f3cpy(support, b); i = 1;
    } else f3cpy(support, a);
    return i;
}
static int
polyhedron_support(float *support, const float *d,
    const float *verts, int cnt)
{
    int imax = 0;
    float dmax = f3dot(verts, d);
    for (int i = 1; i < cnt; ++i) {
        /* find vertex with max dot product in direction d */
        float dot = f3dot(&verts[i*3], d);
        if (dot < dmax) continue;
        imax = i, dmax = dot;
    } f3cpy(support, &verts[imax*3]);
    return imax;
}
static int
polyhedron_intersect_capsule(const float *verts, int cnt,
    const float *ca, const float *cb, float cr)
{
    /* initial guess */
    struct gjk_support s = {0};
    f3cpy(s.a, verts);
    f3cpy(s.b, ca);

    /* run gjk algorithm */
    struct gjk_simplex gsx = {0};
    while (gjk(&gsx, &s)) {
        s.aid = polyhedron_support(s.a, s.da, verts, cnt);
        s.bid = line_support(s.b, s.db, ca, cb);
    }
    /* check distance between closest points */
    struct gjk_result res;
    gjk_analyze(&res, &gsx);
    gjk_quad(&res, 0, cr);
    return res.hit;
}

xample_transform.c

#include "gjk.h"

static void
transform(float *r3, const float *v3, const float *r33, const float *t3)
{
    r3[0] = v3[0] * r33[0*3+0] + v3[1] * r33[1*3+0] + v3[2] * r33[2*3+0] + t3[0];
    r3[1] = v3[0] * r33[0*3+1] + v3[1] * r33[1*3+1] + v3[2] * r33[2*3+1] + t3[1];
    r3[2] = v3[0] * r33[0*3+2] + v3[1] * r33[1*3+2] + v3[2] * r33[2*3+2] + t3[2];
}
static void
transformI(float *r3, const float *v3, const float *r33, const float *t3)
{
    const float p[3] = {v3[0] - t3[0], v3[1] - t3[1], v3[2] - t3[2] };
    r3[0] = p[0] * r33[0*3+0] + p[1] * r33[0*3+1] + p[2] * r33[0*3+2];
    r3[1] = p[0] * r33[1*3+0] + p[1] * r33[1*3+1] + p[2] * r33[1*3+2];
    r3[2] = p[0] * r33[2*3+0] + p[1] * r33[2*3+1] + p[2] * r33[2*3+2];
}
static int
polyhedron_support(float *support, const float *d,
    const float *verts, int cnt)
{
    int imax = 0;
    float dmax = f3dot(verts, d);
    for (int i = 1; i < cnt; ++i) {
        /* find vertex with max dot product in direction d */
        float dot = f3dot(&verts[i*3], d);
        if (dot < dmax) continue;
        imax = i, dmax = dot;
    } f3cpy(support, &verts[imax*3]);
    return imax;
}
static int
polyhedron_intersect_polyhedron(
    const float *averts, int acnt, const float *apos, const float *arot,
    const float *bverts, int bcnt, const float *bpos, const float *brot)
{
    /* initial guess */
    struct gjk_support s = {0};
    transform(s.a, averts, arot, apos);
    transform(s.b, bverts, brot, bpos);

    /* run gjk algorithm */
    struct gjk_simplex gsx = {0};
    while (gjk(&gsx, &s)) {
        /* transform direction */
        float da[3]; transformI(da, s.da, arot, apos);
        float db[3]; transformI(db, s.db, brot, bpos);
        
        /* run support function on tranformed directions  */
        float sa[3]; s.aid = polyhedron_support(sa, da, averts, acnt);
        float sb[3]; s.bid = polyhedron_support(sb, db, bverts, bcnt);
        
        /* calculate distance vector on transformed points */
        transform(s.a, sa, arot, apos);
        transform(s.b, sb, brot, bpos);
    }
    return gsx.hit;
}

xample_xdebug.c

#include "gjk.h"

static int
polyhedron_support(float *support, const float *d,
    const float *verts, int cnt)
{
    int imax = 0;
    float dmax = f3dot(verts, d);
    for (int i = 1; i < cnt; ++i) {
        /* find vertex with max dot product in direction d */
        float dot = f3dot(&verts[i*3], d);
        if (dot < dmax) continue;
        imax = i, dmax = dot;
    } f3cpy(support, &verts[imax*3]);
    return imax;
}
static int
polyhedron_intersect_sphere_debug(
    struct gjk_simplex *gsx,
    const float *verts, int cnt,
    const float *sc, float sr)
{
    /* initial guess */
    int n = 0;
    struct gjk_support s = {0};
    f3cpy(s.a, verts);
    f3cpy(s.b, sc);

    /* run gjk algorithm */
    while (gjk(&gsx[n++], &s)) {
        s.aid = polyhedron_support(s.a, s.da, verts, cnt);
        gsx[n] = gsx[n-1];
    }
    return n;
}
static int
debug_draw_polyhedron_intersect_sphere(int key_frame,
    const float *verts, int cnt,
    const float *sc, float sr)
{
    struct gjk_simplex gsx[GJK_MAX_ITERATIONS] = {{0}};
    int n = polyhedron_intersect_sphere_debug(gsx, verts, cnt, sc, sr);
    key_frame = clamp(0, key_frame, n-1);
    
    struct gjk_result res;
    gjk_analyze(&gsx[key_frame]);
    gjk_quad(&res, 0, sr);
    
    glBox(res.p0, 0.05f, 0.05f, 0.05f);
    glBox(res.p1, 0.05f, 0.05f, 0.05f);
    glLine(res.p0, res.p1);
    return key_frame;
}   

xample_xmov.c

#include "gjk.h"

static void
transform(float *r3, const float *v3, const float *r33, const float *t3)
{
    r3[0] = v3[0] * r33[0*3+0] + v3[1] * r33[1*3+0] + v3[2] * r33[2*3+0] + t3[0];
    r3[1] = v3[0] * r33[0*3+1] + v3[1] * r33[1*3+1] + v3[2] * r33[2*3+1] + t3[1];
    r3[2] = v3[0] * r33[0*3+2] + v3[1] * r33[1*3+2] + v3[2] * r33[2*3+2] + t3[2];
}
static void
transformI(float *r3, const float *v3, const float *r33, const float *t3)
{
    const float p[3] = {v3[0] - t3[0], v3[1] - t3[1], v3[2] - t3[2] };
    r3[0] = p[0] * r33[0*3+0] + p[1] * r33[0*3+1] + p[2] * r33[0*3+2];
    r3[1] = p[0] * r33[1*3+0] + p[1] * r33[1*3+1] + p[2] * r33[1*3+2];
    r3[2] = p[0] * r33[2*3+0] + p[1] * r33[2*3+1] + p[2] * r33[2*3+2];
}
static int
polyhedron_support(float *support, const float *d,
    const float *verts, int cnt)
{
    int imax = 0;
    float dmax = f3dot(verts, d);
    for (int i = 1; i < cnt; ++i) {
        /* find vertex with max dot product in direction d */
        float dot = f3dot(&verts[i*3], d);
        if (dot < dmax) continue;
        imax = i, dmax = dot;
    } f3cpy(support, &verts[imax*3]);
    return imax;
}
static int
polyhedron_intersect_polyhedron(
    const float *averts, int acnt, const float *apos, const float *arot, const float *ta,
    const float *bverts, int bcnt, const float *bpos, const float *brot, const float *tb)
{
    /* initial guess */
    struct gjk_support s = {0};
    transform(s.a, averts, arot, apos);
    transform(s.b, bverts, brot, bpos);

    /* run gjk algorithm */
    struct gjk_simplex gsx = {0};
    while (gjk(&gsx, &s)) {
        /* transform direction */
        float da[3]; transformI(da, s.da, arot, apos);
        float db[3]; transformI(db, s.db, brot, bpos);
        
        /* run support function on tranformed into model space directions  */
        float sa[3]; s.aid = polyhedron_support(sa, da, averts, acnt);
        float sb[3]; s.bid = polyhedron_support(sb, db, bverts, bcnt);
        
        /* transform points to world space */
        transform(s.a, sa, arot, apos);
        transform(s.b, sb, brot, bpos);  
      
        /* calculate distance vector */
        if (f3dot(s.da, ta) > 0) f3add(s.a, s.a, ta);
        if (f3dot(s.db, tb) > 0) f3add(s.b, s.b, tb);
    }
    return gsx.hit;
}

I have translated this to Java and the intersection test seems to work well.
The problem I have is when finding the closest distance between two cubes (like AABB's). Sometimes the answer is correct and sometimes it is completely wrong. I have plotted the simplex:

• when the simplex is a triangle the closest distance result is correct.
• simply moving one cube away from the other by a short distance turns triangle simplex into a line simplex (as if it were one edge of a triangle) and closest distance calculation is then wrong.

Obviously I might have translated something wrong! But do you know if this situation is handled correctly in you program?
Thanks
Jim

I found this is something with translation. I am not sure if that's what caused problems you have.

@yzd0014 please could you post code (or diff) or point me to a repository. Thanks. Jim

transform(float r3, const float v3, const float r33, const float t3)
{
r3[0] = v3[0] * r33[03+0] + v3[0] * r33[03+1] + v3[0] * r33[03+2] + t3[0];
r3[1] = v3[1] * r33[13+0] + v3[1] * r33[13+1] + v3[1] * r33[13+2] + t3[1];
r3[2] = v3[2] * r33[23+0] + v3[2] * r33[23+1] + v3[2] * r33[2*3+2] + t3[2];
}

In my opinion, every row should use v3[0], v3[1], v3[2] instead the same one.

does not seem to work reliably when testing aabb intersections:
The collision of the aabbs [(0, 0, 0), (1, 1, 1)], [(0.5, 0.7, 0.5), (2, 2, 2)] will not be detected properly, for example.
This happens when the origin lies on a line or triangle simplex and the resulting search direction is (0, 0, 0).
Can be worked around by treating very small distances as collisions.

https://github.com/yzd0014/GameEngineV2/blob/master/Engine/Physics/sRigidBodyState.cpp

I made it work eventually, see line 250 for GJK algorithm.

@yzd0014 thanks for posting, hopefully I can try your code in my Java version soon. Thanks.

You can let me know if you have any problems or questions.

Thanks @yzd0014 and @jimbok8 I will test the two problematic aabbs as well and try out your fix yzd0014.

Hello,
I would like to use your GJK implementation and would like to know if you have fixed the issues reported above.

The collision of the aabbs [(0, 0, 0), (1, 1, 1)], [(0.5, 0.7, 0.5), (2, 2, 2)] will not be detected properly,

at least for me it detects these two as a collision (red means collided)

Am I doing something wrong or is there another example?

i used this code to test it:

#include "gjk.h"

#define fop(r,e,a,p,b,i,s) (r) e ((a) p (b)) i (s)
#define f3op(r,e,a,p,b,i,s) do {\
    fop((r)[0],e,(a)[0],p,(b)[0],i,s),\
    fop((r)[1],e,(a)[1],p,(b)[1],i,s),\
    fop((r)[2],e,(a)[2],p,(b)[2],i,s);}while(0)
#define f3cpy(d,s) (d)[0]=(s)[0],(d)[1]=(s)[1],(d)[2]=(s)[2]
#define f3dot(a,b) ((a)[0]*(b)[0]+(a)[1]*(b)[1]+(a)[2]*(b)[2])
#define f3sub(d,a,b) f3op(d,=,a,-,b,+,0)

static int
polyhedron_support(float *support, const float *d,
    const float *verts, int cnt)
{
    int imax = 0;
    float dmax = f3dot(verts, d);
    for (int i = 1; i < cnt; ++i) {
        /* find vertex with max dot product in direction d */
        float dot = f3dot(&verts[i*3], d);
        if (dot < dmax) continue;
        imax = i, dmax = dot;
    } f3cpy(support, &verts[imax*3]);
    return imax;
}


static int
polyhedron_intersect_polyhedron(
    const float *averts, int acnt,
    const float *bverts, int bcnt)
{
    /* initial guess */
    struct gjk_support s = {0};
    f3sub(s.d, s.b, s.a);

    /* run gjk algorithm */
    struct gjk_simplex gsx = {0};
    while (gjk(&gsx, &s)) {
        
        /* run support function on tranformed into model space directions  */
        s.aid = polyhedron_support(s.a, s.da, averts, acnt);
        s.bid = polyhedron_support(s.b, s.db, bverts, bcnt);
              
        f3sub(s.d, s.b, s.a);
    }
    return gsx.hit;
}


#define f3cpy(d,x,y,z) ((d)[0]=(x),(d)[1]=(y),(d)[2]=(z))
void 
make_box(float* box, float *min, float *max)
{
        f3cpy(&box[0], min[0], min[1], min[2]);
        f3cpy(&box[1 * 3], min[0], min[1], max[2]);
        f3cpy(&box[2 * 3], min[0], max[1], min[2]);
        f3cpy(&box[3 * 3], min[0], max[1], max[2]);
        f3cpy(&box[4 * 3], max[0], min[1], min[2]);
        f3cpy(&box[5 * 3], max[0], min[1], max[2]);
        f3cpy(&box[6 * 3], max[0], max[1], min[2]);
        f3cpy(&box[7 * 3], max[0], max[1], max[2]);
}
 
#include <stdio.h>

int
main(void)
{
        float box1[24], box2[24];
        float min1[3] = {0.0f, 0.0f, 0.0f};
        float max1[3] = {1.0f, 1.0f, 1.0f};
        float min2[3] = {.5f, .7f, .5f};
        float max2[3] = {2.0f, 2.0f, 2.0f};
        make_box(box1, min1, max1);
        make_box(box2, min2, max2);

        int result = polyhedron_intersect_polyhedron(box1, 24, box2, 24);
        printf("%d\n", result);

}

it prints 0 for me

the problem seems to be in the gjk function: a hit is only detected if we can enclose the origin in a 4-simplex. however, the origin can lie on the 3-simplex or 2-simplex. this edge case is (as far as i can see) not accounted for.

there were some bugs in your code. Don't know if these were just copy and paste bugs but it should look closer to:

#define cpy3(d,v) ((d)[0]=(v)[0],(d)[1]=(v)[1],(d)[2]=(z)[2])

static int
polyhedron_intersect_polyhedron(
    const float *averts, int acnt,
    const float *bverts, int bcnt)
{
    struct gjk_support s = {0};
    cpy3(s.a, averts);
    cpy3(s.b, bverts);
    f3sub(s.d, s.b, s.a);

    /* run gjk algorithm */
    struct gjk_simplex gsx = {0};
    while (gjk(&gsx, &s)) {
        s.aid = polyhedron_support(s.a, s.da, averts, acnt);
        s.bid = polyhedron_support(s.b, s.db, bverts, bcnt);
        f3sub(s.d, s.b, s.a);
    }
    gjk_analyze(&res, &gsx); // not required but helpful
    return gsx.hit;
}

#include <stdio.h>

int main(void)
{
        float box1[24], box2[24];
        float min1[3] = {0.0f, 0.0f, 0.0f};
        float max1[3] = {1.0f, 1.0f, 1.0f};
        float min2[3] = {.5f, .7f, .5f};
        float max2[3] = {2.0f, 2.0f, 2.0f};
        make_box(box1, min1, max1);
        make_box(box2, min2, max2);

        int result = polyhedron_intersect_polyhedron(box1, 8, box2, 8); // 8 as in number of vertexes not floats
        printf("%d\n", result);

}

However I tested your example in my code and it worked and showed a collision. However I did not have so much time yesterday so I will test again a little bit more hopefully today.

you are right, but after fixing these mistakes, i still get no collision (compiled with gcc version 9.3.0, no optimizations enabled).

Thanks @vogelj for your work and patience. I was able to reproduce and hopefully fix it. Took a bit of time to actually reproduce it. Looks like my compiler generated for 0.7f the number 0.69999997 and another compiler 0.700004 or something similar. This small numeric difference destroyed everything.

I added another check in the tetrahedron voronoi region check function to prevent floating point errors. I tested with all my test and so far everything seems to work again.

Why the /* VII.) New search direction / section do all the work calculating a direction to search, if the last iteration already calculated the origin projected in the simplex, so if you take the projected point calculated on / VI.) Ensure closing in on origin to prevent multi-step cycling */, and use it as a direction by subtracting the origin from the projected point, you will get the new search direction without aditional steps.
Maybe even solving the problem where a degenerate triangle or line doesn't have a proper normal, the projection will still lie in the simplex, and you will get a correct search normal.

Let me know if there is any reason on not doing this way and going in all the trouble calculating normals and everything.