GJK

#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;
}