GJK - The Code - Gilbert, Johnson and Keerthi

Computing the Distance between Objects

One of the basic problems for planning and simulation, namely the fast calculation of the distance between two objects.

Implemented an enhanced version of the distance routine of Gilbert, Johnson and Keerthi (GJK), which allows the tracking of the distance between a pair of convex polyhedra in time that is expected to be bounded by a constant. Experimental results confirm this result, at least for moderate relative velocities, and suggest that the enhanced algorithm is roughly the same speed as the Lin-Canny algorithm, as used in I-COLLIDE, and Brian Mirtich's V-Clip.

The Code

The code for version 2.4 of GJK is now available. It consists of the core routines (gjk.c, with header file gjk.h), and a test-bed wrapper (gjkdemo.c). The enhanced algorithm requires a list of all the edges in each convex polyhedra, and without it the performance of the routine is the same as the original GJK.

The test bed program generates pairs of randomly placed hulls of a given size (size greater or equal to three), and calls the enhanced GJK routine to find their distance apart. It then checks the answer in an independent piece of code, and will report on any errors found.

By default the routine generates a semi-regular shape that I call a polyball, which it can do without using any routines for generating convex hulls. The program gjkdemo can only generate this form of polyhedra. However if the public domain package called Qhull is used, together with the glue code in sac.c, then the program created from gjkdemo.ccan also test GJK using hulls of randomly generated point sets (-Q option). With the makefile given this is created as a program called gjkqhull, with the same options as gjkdemo but also supporting the -Q option. Also, the testing of zero-distance answers is only done if Qhull is linked.

Source:

http://www.cs.ox.ac.uk/stephen.cameron/distances/

GJK C Program Example-1

GJK C Program Example-1

Computing the Distance between Objects

#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include <stdbool.h>

#include "vector3.h"

typedef struct TSimplex {
    TVec3 points[4];
    int size;
} TSimplex;

typedef enum EShapeType {
    CS_SPHERE,
    CS_CONVEX,
} EShapeType;

typedef struct TConvexShape {
    TVec3 * points;
    int count;
    int type;
    float radius; // for sphere
} TConvexShape;

// ===========================
// HELPERS
// ===========================
bool Helper_SameDirection( TVec3 a, TVec3 b ) {
    return Vec3_Dot( a, b ) > 0;
}

// ===========================
// SIMPLEX ROUTINE
// ===========================
void Simplex_RemovePoint( TSimplex * s, int num ) {
    if( s->size > 0 ) {
        if( num == 0 ) {
            s->points[0] = s->points[1];
            s->points[1] = s->points[2];
            s->points[2] = s->points[3];
        }
        if( num == 1 ) {
            s->points[1] = s->points[2];
            s->points[2] = s->points[3];
        }
        if( num == 2 ) {
            s->points[2] = s->points[3];
        }
        s->size--;
    }
}

void Simplex_AddPoint( TSimplex * s, TVec3 p ) {
    if( s->size < 3 ) {
        s->points[ s->size ] = p;
        s->size++;
    }
}

// ===========================
// CONVEX SHAPE ROUTINE
// ===========================
void ConvexShape_CreateTriangle( TConvexShape * shape, TVec3 a, TVec3 b, TVec3 c ) {
    shape->count = 3;
    shape->points = malloc( shape->count * sizeof( TVec3 ));
    shape->points[0] = a;
    shape->points[1] = b;
    shape->points[2] = c;
    shape->type = CS_CONVEX;
    shape->radius = 0;
}

void ConvexShape_CreateTetrahedron( TConvexShape * shape, TVec3 a, TVec3 b, TVec3 c, TVec3 d ) {
    shape->count = 4;
    shape->points = malloc( shape->count * sizeof( TVec3 ));
    shape->points[0] = a;
    shape->points[1] = b;
    shape->points[2] = c;
    shape->points[3] = d;
    shape->type = CS_CONVEX;
    shape->radius = 0;
}

void ConvexShape_CreateSphere( TConvexShape * shape, TVec3 position, float radius ) {
    shape->count = 1;
    shape->points = malloc( shape->count * sizeof( TVec3 ));
    shape->points[ 0 ] = position;
    shape->type = CS_SPHERE;
    shape->radius = radius;
}


TVec3 ConvexShape_GetFarthestPointInDirection( TConvexShape * shape, TVec3 dir ) {
    if( shape->type == CS_CONVEX ) {
        TVec3 farthest;
        float lastDot = -FLT_MAX;
        for( int i = 0; i < shape->count; i++ ) {
            float dot = Vec3_Dot( dir, shape->points[i] );
            if( dot > lastDot ) {
                farthest = shape->points[i];
                lastDot = dot;
            }
        }
        return farthest;
    } else {
        if( fabs( dir.x ) < 0.000001 &&
            fabs( dir.y ) < 0.000001 &&
            fabs( dir.z ) < 0.000001 ) {
            printf( "Warn! Zero dir passed!\n" );
        }
        TVec3 dn = Vec3_Normalize( dir );
       
        return Vec3_Add( shape->points[0], Vec3_Scale( dn, shape->radius ));
    }
}

// ===========================
// GJK ALGORITHM ROUTINE
// ===========================
TVec3 GJK_GetSupport( TConvexShape * shape1, TConvexShape * shape2, TVec3 dir ) {
    return Vec3_Sub( ConvexShape_GetFarthestPointInDirection( shape1, dir ), ConvexShape_GetFarthestPointInDirection( shape2, Vec3_Negate( dir )));
}

bool GJK_ProcessLine( TSimplex * simplex, TVec3 * outDirection ) {
    TVec3 a = simplex->points[1];
    TVec3 b = simplex->points[0];
    TVec3 ab = Vec3_Sub( b, a );
    TVec3 aO = Vec3_Negate( a );
   
    if( Helper_SameDirection( ab, aO )) {
        *outDirection = Vec3_Cross( Vec3_Cross( ab, aO ), ab );
    } else {
        Simplex_RemovePoint( simplex, 0 );
        *outDirection = aO;
    }
    return false;
}

bool GJK_ProcessTriangle( TSimplex * simplex, TVec3 * outDirection ) {
    TVec3 a = simplex->points[2];
    TVec3 b = simplex->points[1];
    TVec3 c = simplex->points[0];
    TVec3 aO = Vec3_Negate( a );
    TVec3 ab = Vec3_Sub( b, a );
    TVec3 ac = Vec3_Sub( c, a );
    TVec3 abPerp = Vec3_Cross( Vec3_Cross( ac, ab ), ab );
    TVec3 acPerp = Vec3_Cross( Vec3_Cross( ab, ac ), ac );
    if( Helper_SameDirection( abPerp, aO )) {
        Simplex_RemovePoint( simplex, 0 );
        *outDirection = abPerp;
    } else {
        if( Helper_SameDirection( acPerp, aO )) {
            Simplex_RemovePoint( simplex, 1 );
            *outDirection = acPerp;
        } else {
            return true;
        }
    }
    return false;
}

bool GJK_ProcessTetrahedron( TSimplex * simplex, TVec3 * outDirection ) {
    TVec3 a = simplex->points[3];
    TVec3 b = simplex->points[2];
    TVec3 c = simplex->points[1];
    TVec3 d = simplex->points[0];
   
    TVec3 ac = Vec3_Sub( c, a );
    TVec3 ab = Vec3_Sub( b, a );
    TVec3 ad = Vec3_Sub( d, a );
   
    TVec3 acd = Vec3_Cross( ad, ac );
    TVec3 abd = Vec3_Cross( ab, ad );
    TVec3 abc = Vec3_Cross( ac, ab );
   
    TVec3 aO = Vec3_Negate( a );
   
    if( Helper_SameDirection( abc, aO )) {
        if( Helper_SameDirection( Vec3_Cross( abc, ac ), aO )) {
            Simplex_RemovePoint( simplex, 2 );
            Simplex_RemovePoint( simplex, 0 );
            *outDirection = Vec3_Cross( Vec3_Cross( ac, aO ), ac );
        } else if( Helper_SameDirection( Vec3_Cross( ab, abc ), aO )) {
            Simplex_RemovePoint( simplex, 1 );
            Simplex_RemovePoint( simplex, 0 );
            *outDirection = Vec3_Cross( Vec3_Cross( ab, aO ), ab );
        } else {
            Simplex_RemovePoint( simplex, 0 );
            *outDirection = abc;
        }
    } else if( Helper_SameDirection( acd, aO )) {
        if( Helper_SameDirection( Vec3_Cross( acd, ad ), aO )) {
            Simplex_RemovePoint( simplex, 2 );
            Simplex_RemovePoint( simplex, 1 );
            *outDirection = Vec3_Cross( Vec3_Cross( ad, aO ), ad );
        } else if ( Helper_SameDirection( Vec3_Cross( ac, acd ), aO )) {
            Simplex_RemovePoint( simplex, 2 );
            Simplex_RemovePoint( simplex, 0 );
            *outDirection = Vec3_Cross( Vec3_Cross( ac, aO ), ac );
        } else {
           Simplex_RemovePoint( simplex, 2 );
            *outDirection = acd;
        }
    } else if( Helper_SameDirection( abd, aO )) {
        if( Helper_SameDirection( Vec3_Cross( abd, ab ), aO )) {
            Simplex_RemovePoint( simplex, 1 );
            Simplex_RemovePoint( simplex, 0 );
            *outDirection = Vec3_Cross( Vec3_Cross( ab, aO ), ab );
        } else if( Helper_SameDirection( Vec3_Cross( ad, abd ), aO )) {
            Simplex_RemovePoint( simplex, 2 );
            Simplex_RemovePoint( simplex, 1 );
            *outDirection = Vec3_Cross( Vec3_Cross( ad, aO ), ad );
        } else {
            Simplex_RemovePoint( simplex, 1 );
            *outDirection = abd;
        }
    } else {
        return true;
    }
   
    return false;
}

bool GJK_ProcessSimplex( TSimplex * simplex, TVec3 * outDirection ) {
    if( simplex->size == 2 ) {
        return GJK_ProcessLine( simplex, outDirection );
    } else if ( simplex->size == 3 ) {
        return GJK_ProcessTriangle( simplex, outDirection );
    } else {
        return GJK_ProcessTetrahedron( simplex, outDirection );
    }
}

bool GJK_IsIntersects( TConvexShape * shape1, TConvexShape * shape2 ) {
    TVec3 dir = Vec3_Set( 0, 1, 0 );
   
    TSimplex simplex = { 0 };
    Simplex_AddPoint( &simplex, GJK_GetSupport( shape1, shape2, dir ));
   
    dir = Vec3_Negate( dir );
   
    int convergenceLimit = 45;
    for( int i = 0; i < convergenceLimit; i++ ) {
        TVec3 lastSupport = GJK_GetSupport( shape1, shape2, dir );              
        if( Helper_SameDirection( dir, lastSupport )) {
            Simplex_AddPoint( &simplex, lastSupport );          
            if( GJK_ProcessSimplex( &simplex, &dir )) {
                printf( "Intersection! %d iteration(s)!\n", i );
                return true;
            }          
        } else {
            printf( "No intersection! %d iteration(s)!\n", i );          
            return false;
        }
    }
   
    printf( "No intersection! Convergence limit has reached!\n" );
    return false;
}

int main(int argc, char **argv) {
    {
        printf( "Triangle-Triangle Intersection Test - " );
        TConvexShape shape1, shape2;
        ConvexShape_CreateTriangle( &shape1, Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ));
        ConvexShape_CreateTriangle( &shape2, Vec3_Set( 0, 0.5, 0 ), Vec3_Set( 0, 1.5, 1 ), Vec3_Set( 1, 0.5, 1 ));
        GJK_IsIntersects( &shape1, &shape2 );
    }
    {
        printf( "Triangle-Tetrahedron Intersection Test - " );
        TConvexShape shape1, shape2;
        ConvexShape_CreateTriangle( &shape1, Vec3_Set( 0, 0, 0.5 ), Vec3_Set( 0, 1, 0.5 ), Vec3_Set( 1, 0, 0.5 ));
        ConvexShape_CreateTetrahedron( &shape2, Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ), Vec3_Set( 0, 0, 1 ));
        GJK_IsIntersects( &shape1, &shape2 );
    }
    {
        {
            printf( "Sphere-Tetrahedron Intersection Test Without Small Offset - " );
            TConvexShape shape1, shape2;
            ConvexShape_CreateSphere( &shape1, Vec3_Set( 0,0,0 ), 1 );
            ConvexShape_CreateTetrahedron( &shape2, Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ), Vec3_Set( 0, 0, 1 ));
            GJK_IsIntersects( &shape1, &shape2 );
        }
        {
            printf( "Sphere-Tetrahedron Intersection Test With Small Offset - " );
            TConvexShape shape1, shape2;
            ConvexShape_CreateSphere( &shape1, Vec3_Set( 0.0001, 0, 0 ), 1 );
            ConvexShape_CreateTetrahedron( &shape2, Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ), Vec3_Set( 0, 0, 1 ));
            GJK_IsIntersects( &shape1, &shape2 );
        }      
    }
    {
        printf( "Tetrahedron-Tetrahedron Intersection Test - " );
        TConvexShape shape1, shape2;
        ConvexShape_CreateTetrahedron( &shape1, Vec3_Set( 0, 0, 0.5 ), Vec3_Set( 0, 1, 0.5 ), Vec3_Set( 1, 0, 0.5 ), Vec3_Set( 0, 0, 1.5 ));
        ConvexShape_CreateTetrahedron( &shape2, Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ), Vec3_Set( 0, 0, 1 ));
        GJK_IsIntersects( &shape1, &shape2 );
    }
    {
        printf( "Sphere-Sphere Intersection Test - " );
        TConvexShape shape1, shape2;
        ConvexShape_CreateSphere( &shape1, Vec3_Set( 1,0,0 ), 1 );
        ConvexShape_CreateSphere( &shape2, Vec3_Set( 0,0,0 ), 1 );
        GJK_IsIntersects( &shape1, &shape2 );
    }  
return 0;
}