GJK C Program Example-1
#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;
}