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

• 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, const struct gjk_support *sup, float *dv)
{
    assert(s);
    assert(dv);
    assert(sup);
    if (!s || !sup || !dv) 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);
    f3cpy(vert->p, dv);
    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;
    case 4: {
        /* ----- Tetrahedron ----- */
        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]);
        float d[3]; f3mul(d, s->v[3].p, denom * s->bc[3]);

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

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

    /* VII.) New search direction */
    switch (s->cnt) {
    default: assert(0); break;
    case 1: {
        /* --------- Point -------- */
        f3mul(dv, 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(dv, 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(dv, n);
        else f3mul(dv, n, -1);
    }}
    if (f3dot(dv,dv) < GJK_EPSILON * GJK_EPSILON)
        return 0;
    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;
    float a[3];
    float b[3];
};
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, const struct gjk_support *sup, float *dv);
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 */
    float d[3] = {0};
    struct gjk_support s = {0};
    f3cpy(s.a, verts);
    f3cpy(s.b, ca);
    f3sub(d, s.b, s.a);

    /* run gjk algorithm */
    struct gjk_simplex gsx = {0};
    while (gjk(&gsx, &s, d)) {
        float n[3]; f3mul(n, d, -1);
        s.aid = polyhedron_support(s.a, n, verts, cnt);
        s.bid = line_support(s.b, d, ca, cb);
        f3sub(d, s.b, s.a);
    }
    /* 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[0] * r33[0*3+1] + v3[0] * r33[0*3+2] + t3[0];
    r3[1] = v3[1] * r33[1*3+0] + v3[1] * r33[1*3+1] + v3[1] * r33[1*3+2] + t3[1];
    r3[2] = v3[2] * r33[2*3+0] + v3[2] * r33[2*3+1] + v3[2] * r33[2*3+2] + t3[2];
}
static void
transformI(float *r, const float *v, const float *r33, const float *t3)
{
    const float v[3] = {v3[0] - t3[0], v3[1] - t3[1], v3[2] - t3[2] };
    r3[0] = v[0] * r33[0*3+0] + v[0] * r33[1*3+0] + v[0] * r33[2*3+0];
    r3[1] = v[1] * r33[0*3+1] + v[1] * r33[1*3+1] + v[1] * r33[2*3+1];
    r3[2] = v[2] * r33[0*3+2] + v[2] * r33[1*3+2] + v[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 */
    float d[3] = {0};
    struct gjk_support s = {0};
    transform(s.a, averts, arot, apos);
    transform(s.b, bverts, brot, bpos);
    f3sub(d, s.b, s.a);

    /* run gjk algorithm */
    struct gjk_simplex gsx = {0};
    while (gjk(&gsx, &s, d)) {
        /* transform direction */
        float n[3]; f3mul(n, d, -1);
        float da[3]; transformI(da, n, arot, apos);
        float db[3]; transformI(db, d, brot, bpos);
        
        /* run support function on tranformed directions  */
        s.aid = polyhedron_support(s.a, da, averts, acnt);
        s.bid = polyhedron_support(s.b, db, bverts, bcnt);
        
        /* calculate distance vector on transformed points */
        transform(s.a, s.a, arot, apos);
        transform(s.b, s.b, brot, bpos);       
        f3sub(d, s.b, s.a);
    }
    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;
    float d[3] = {0};
    struct gjk_support s = {0};
    
    f3cpy(s.a, verts);
    f3cpy(s.b, sc);
    f3sub(d, s.b, s.a);

    /* run gjk algorithm */
    while (gjk(&gsx[n++], &s, d)) {
        float nd[3]; f3mul(nd, d, -1);
        s.aid = polyhedron_support(s.a, nd, verts, cnt);
        f3sub(d, s.b, s.a);
        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[0] * r33[0*3+1] + v3[0] * r33[0*3+2] + t3[0];
    r3[1] = v3[1] * r33[1*3+0] + v3[1] * r33[1*3+1] + v3[1] * r33[1*3+2] + t3[1];
    r3[2] = v3[2] * r33[2*3+0] + v3[2] * r33[2*3+1] + v3[2] * r33[2*3+2] + t3[2];
}
static void
transformI(float *r, const float *v, const float *r33, const float *t3)
{
    const float v[3] = {v3[0] - t3[0], v3[1] - t3[1], v3[2] - t3[2] };
    r3[0] = v[0] * r33[0*3+0] + v[0] * r33[1*3+0] + v[0] * r33[2*3+0];
    r3[1] = v[1] * r33[0*3+1] + v[1] * r33[1*3+1] + v[1] * r33[2*3+1];
    r3[2] = v[2] * r33[0*3+2] + v[2] * r33[1*3+2] + v[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 */
    float d[3] = {0};
    struct gjk_support s = {0};
    transform(s.a, averts, arot, apos);
    transform(s.b, bverts, brot, bpos);
    f3sub(d, s.b, s.a);

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