GJK EPA 4

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

#include "vector3.h"

typedef struct TSupport {
    TVec3 supportA;
    TVec3 minkowskiDifPoint;
} TSupport;

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

typedef struct TEPAFace {
    int a, b, c;
} TEPAFace;

typedef struct TSphereShape {
    float radius;
} TSphereShape;

typedef struct TConvexShape {
    TVec3 * points;
    int count;
} TConvexShape;

typedef struct TTriangleShape {
    TVec3 a, b, c;
} TTriangleShape;

typedef struct TTriangleMeshShape {
    TVec3 * vertices;
    int vertexCount;
    TEPAFace * faces;
    int faceCount;
} TTriangleMeshShape;

typedef struct TBoxShape {
    float size;
    TVec3 vertices[8];
} TBoxShape;

typedef struct TShape {
    TConvexShape * convex;
    TSphereShape * sphere;
    TTriangleMeshShape * triMesh;
    TTriangleShape * triangle;
    TBoxShape * box;
    TVec3 position;
} TShape;

typedef struct TEPATriangle {
    TSupport a, b, c;
    TVec3 normal;
    float dist;
    int numInPolytope;
} TEPATriangle;

typedef struct TEPAContact {
    TVec3 normal;
    float penetrationDepth;
} TEPAContact;

typedef struct TEPAPolytope {
    TSupport * vertices;
    int vertexCount;
    TEPAFace * faces;
    int faceCount;
} TEPAPolytope;

typedef struct TEdge {
    int a;
    int b;
    bool free;
} TEdge;

typedef struct TEdgeList {
    TEdge * edges;
    int count;
    int freeCount;
} TEdgeList;

typedef struct TBody {
    TVec3 position;
    TShape * shape;
} TBody;

#ifndef M_PI
#   define M_PI 3.14159
#endif


TEPAPolytope * currentPolytope;
TShape * currentShape1;
TShape * currentShape2;
TShape * currentShape3;

TBody * body1;
TBody * body2;
TBody * body3;
// ===========================
// 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, TSupport p ) {
    if( s->size < 4 ) {
        s->points[ s->size ] = p;
        s->size++;
    } else {
        printf( "SIMPLEX WARNING! Not enough space!\n" );
    }
}

// ===========================
// CONVEX SHAPE ROUTINE
// ===========================
TShape * ConvexShape_CreateTriangle( TVec3 a, TVec3 b, TVec3 c ) {
    TShape * shape = calloc( 1, sizeof( TShape ));
    shape->convex = calloc( 1, sizeof( TConvexShape ));
    shape->convex->count = 3;
    shape->convex->points = malloc( shape->convex->count * sizeof( TVec3 ));
    shape->convex->points[0] = a;
    shape->convex->points[1] = b;
    shape->convex->points[2] = c;
    return shape;
}

TShape * ConvexShape_CreateTetrahedron( TVec3 a, TVec3 b, TVec3 c, TVec3 d ) {
    TShape * shape = calloc( 1, sizeof( TShape ));
    shape->convex = calloc( 1, sizeof( TConvexShape ));
    shape->convex->count = 4;
    shape->convex->points = malloc( shape->convex->count * sizeof( TVec3 ));
    shape->convex->points[0] = a;
    shape->convex->points[1] = b;
    shape->convex->points[2] = c;
    shape->convex->points[3] = d;
    shape->position = Vec3_Set( 0,0,0 );
    return shape;
}

TShape * ConvexShape_CreateSphere( TVec3 position, float radius ) {
    TShape * shape = calloc( 1, sizeof( TShape ));
    shape->sphere = calloc( 1, sizeof( TSphereShape ));
    shape->position = position;
    shape->sphere->radius = radius;
    return shape;
}

void ConvexShape_Delete( TShape * shape ) {
    if( shape->convex ) {
        free( shape->convex );
    }
    if( shape->sphere ) {
        free( shape->sphere );
    }
    free( shape );
}

TVec3 ConvexShape_GetFarthestPointInDirection( TShape * shape, TVec3 dir ) {
    const float eps = 0.000001;
    if( fabs( dir.x ) < eps && fabs( dir.y ) < eps && fabs( dir.z ) < eps ) {
        printf( "GJK Warning: Zero direction passed!\n" );
    }
    dir = Vec3_Normalize( dir );
    if( shape->convex ) {
        TVec3 farthest;
        float lastDot = -FLT_MAX;
        for( int i = 0; i < shape->convex->count; i++ ) {
            float dot = Vec3_Dot( dir, shape->convex->points[i] );
            if( dot > lastDot ) {
                farthest = shape->convex->points[i];
                lastDot = dot;
            }
        }
        farthest = Vec3_Add( farthest, shape->position );
        return farthest;
    }
    if( shape->sphere ) {
        return Vec3_Add( shape->position, Vec3_Scale( dir, shape->sphere->radius ));
    }
    if( shape->box ) {
        TVec3 farthest;
        float lastDot = -FLT_MAX;
        for( int i = 0; i < 8; i++ ) {
            float dot = Vec3_Dot( dir, shape->box->vertices[i] );
            if( dot > lastDot ) {
                farthest = shape->box->vertices[i];
                lastDot = dot;
            }
        }
        farthest = Vec3_Add( farthest, shape->position );
        return farthest;
    }
    if( shape->triangle ) {
        int n;
        float lastDot = -FLT_MAX;
        float dots[3] = { Vec3_Dot( dir, shape->triangle->a ), Vec3_Dot( dir, shape->triangle->b ), Vec3_Dot( dir, shape->triangle->c ) };
        for( int i = 0; i < 3; i++ ) {
            if( dots[i] > lastDot ) {
                n = i;
                lastDot = dots[i];
            }
        }
        TVec3 farthest;
        if( n == 0 ) {
            farthest = shape->triangle->a;
        }
        if( n == 1 ) {
            farthest = shape->triangle->b;
        }
        if( n == 2 ) {
            farthest = shape->triangle->c;
        }
        farthest = Vec3_Add( farthest, shape->position );
        return farthest;
    }
    return Vec3_Zero();
}

TShape * BoxShape_Create( float size ) {
    float hs = size / 2;
    TShape * shape = calloc( 1, sizeof( TShape ));
    shape->box = calloc( 1, sizeof( TBoxShape ));
    shape->box->vertices[0] = Vec3_Set( -hs, -hs, -hs );
    shape->box->vertices[1] = Vec3_Set( -hs, -hs,  hs );
    shape->box->vertices[2] = Vec3_Set(  hs, -hs, -hs );
    shape->box->vertices[3] = Vec3_Set(  hs, -hs,  hs );

    shape->box->vertices[4] = Vec3_Set( -hs,  hs, -hs );
    shape->box->vertices[5] = Vec3_Set( -hs,  hs,  hs );
    shape->box->vertices[6] = Vec3_Set(  hs,  hs, -hs );
    shape->box->vertices[7] = Vec3_Set(  hs,  hs,  hs );
    shape->box->size = size;
    return shape;
}

// ===========================
// GJK ALGORITHM ROUTINE
// ===========================
TSupport GJK_GetSupport( TShape * shape1, TShape * shape2, TVec3 dir ) {
    TSupport sup;
    sup.supportA = ConvexShape_GetFarthestPointInDirection( shape1, dir );
    sup.minkowskiDifPoint = Vec3_Sub( sup.supportA, ConvexShape_GetFarthestPointInDirection( shape2, Vec3_Negate( dir )));
    return sup;
}

bool GJK_ProcessLine( TSimplex * simplex, TVec3 * outDirection ) {
    TVec3 a = simplex->points[1].minkowskiDifPoint;
    TVec3 b = simplex->points[0].minkowskiDifPoint;
    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].minkowskiDifPoint;
    TVec3 b = simplex->points[1].minkowskiDifPoint;
    TVec3 c = simplex->points[0].minkowskiDifPoint;
    TVec3 aO = Vec3_Negate( a );
    TVec3 ab = Vec3_Sub( b, a );
    TVec3 ac = Vec3_Sub( c, a );
    TVec3 abc = Vec3_Cross( ab, ac );
    TVec3 acNormal = Vec3_Cross( abc, ac );
    TVec3 abNormal = Vec3_Cross( ab, abc );

    if( Helper_SameDirection( acNormal, aO )) {
        if( Helper_SameDirection( ac, aO )) {
            Simplex_RemovePoint( simplex, 1 );
            *outDirection = Vec3_Cross( Vec3_Cross(ac, aO), ac );
        } else {
            if( Helper_SameDirection( ab, aO )) {
                Simplex_RemovePoint( simplex, 0 );
                *outDirection = Vec3_Cross( Vec3_Cross(ab, aO), ab);
            } else {
                Simplex_RemovePoint( simplex, 1 );
                Simplex_RemovePoint( simplex, 0 );
                *outDirection = aO;
            }
        }
    } else {
        if( Helper_SameDirection( abNormal, aO )) {
            if( Helper_SameDirection( ab, aO )) {
                Simplex_RemovePoint( simplex, 0 );
                *outDirection = Vec3_Cross( Vec3_Cross(ab, aO), ab);
            } else {
                Simplex_RemovePoint( simplex, 1 );
                Simplex_RemovePoint( simplex, 0 );
                *outDirection = aO;
            }
        } else {
            if( Helper_SameDirection( abc, aO )) {
                *outDirection = Vec3_Cross(Vec3_Cross(abc, aO), abc);
            } else {
                *outDirection = Vec3_Cross(Vec3_Cross( Vec3_Negate( abc ), aO), Vec3_Negate( abc ) );
            }
        }
    }

    return false;
}

bool GJK_ProcessTetrahedron( TSimplex * simplex, TVec3 * outDirection ) {
    TVec3 a = simplex->points[3].minkowskiDifPoint;
    TVec3 b = simplex->points[2].minkowskiDifPoint;
    TVec3 c = simplex->points[1].minkowskiDifPoint;
    TVec3 d = simplex->points[0].minkowskiDifPoint;

    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 if( simplex->size == 4 ) {
        return GJK_ProcessTetrahedron( simplex, outDirection );
    }
    return false;
}

void GJK_FixSimplex( TSimplex * simplex, TShape * shape1, TShape * shape2 ) {
    float epsilon = 0.0001f;
    static const TVec3 directions[6] = {
        { .x =  1.0f,  .y =  0.0f,  .z =  0.0f },
        { .x = -1.0f,  .y =  0.0f,  .z =  0.0f },
        { .x =  0.0f,  .y =  1.0f,  .z =  0.0f },
        { .x =  0.0f,  .y = -1.0f,  .z =  0.0f },
        { .x =  0.0f,  .y =  0.0f,  .z =  1.0f },
        { .x =  0.0f,  .y =  0.0f,  .z = -1.0f }};
    switch( simplex->size ) {
        // case 'fall-through' is need to continuously add new points to the simplex

        // our simplex is a point
        case 1: {
            TSupport additionalSupport;
            for( int i = 0; i < 6; i++ ) {
                additionalSupport = GJK_GetSupport( shape1, shape2, directions[i] );
                if( Vec3_SqrDistance( additionalSupport.minkowskiDifPoint, simplex->points[0].minkowskiDifPoint ) > epsilon ) {
                    Simplex_AddPoint( simplex, additionalSupport );
                    break;
                }
            }
        }

        // our simplex is a line
        case 2: {

        }

        // our simplex is a triangle
        case 3: {

        }
    }
}

bool GJK_IsIntersects( TShape * shape1, TShape * shape2, TSimplex * finalSimplex ) {
    TVec3 dir = Vec3_Set( 1, 1, 1 );

    TSimplex simplex = { 0 };
    Simplex_AddPoint( &simplex, GJK_GetSupport( shape1, shape2, dir ) );

    dir = Vec3_Negate( dir );
    TSimplex lastFullSimplex;
    int convergenceLimit = 1000;
    for( int i = 0; i < convergenceLimit; i++ ) {
        if( simplex.size == 4 ) {
            lastFullSimplex = simplex;
        }
        TSupport lastSupport = GJK_GetSupport( shape1, shape2, dir );
        if( Helper_SameDirection( dir, lastSupport.minkowskiDifPoint )) {
            Simplex_AddPoint( &simplex, lastSupport );
            if( GJK_ProcessSimplex( &simplex, &dir )) {

                printf( "GJK: Intersection! %d iteration(s)!\n", i );
                if( finalSimplex ) {
                    *finalSimplex = simplex;
                }
                return true;
            }
        } else {
            printf( "GJK: No intersection! %d iteration(s)!\n", i );
            return false;
        }
    }
    //printf( "GJK: No intersection! Convergence limit has reached!\n" );
    if( finalSimplex ) {
        *finalSimplex = lastFullSimplex;
    }
    return true;
}

// ===========================
// EPA ALGORITHM ROUTINE
// ===========================
void Polytope_SetFromSimplex( TEPAPolytope * polytope, TSimplex * simplex ) {
    polytope->vertexCount = 4;
    polytope->vertices = calloc( polytope->vertexCount, sizeof( TSupport ));
    for( int i = 0; i < polytope->vertexCount; i++ ) {
        polytope->vertices[i] = simplex->points[i];
    }
    polytope->faceCount = 4;
    polytope->faces = calloc( polytope->faceCount, sizeof( TEPAFace ));
    polytope->faces[0] = (TEPAFace) { 0, 1, 2 };
    polytope->faces[1] = (TEPAFace) { 0, 3, 1 };
    polytope->faces[2] = (TEPAFace) { 0, 2, 3 };
    polytope->faces[3] = (TEPAFace) { 2, 1, 3 };
}

void Polytope_Free( TEPAPolytope * polytope ) {
    free( polytope->faces );
    free( polytope->vertices );
    polytope->faceCount = -1;
    polytope->vertexCount = -1;
}

void EdgeList_Create( TEdgeList * edgeList, TEPAPolytope * polytope ) {
    edgeList->count = polytope->faceCount * 3;
    edgeList->edges = calloc( edgeList->count, sizeof( TEdge ));
}

void EdgeList_Free( TEdgeList * edgeList ) {
    free( edgeList->edges );
    edgeList->count = 0;
}

void EdgeList_Fill( TEdgeList * edgeList, TEPAPolytope * polytope ) {
    for( int i = 0, j = 0; i < polytope->faceCount; i++, j += 3 ) {
        edgeList->edges[j+0] = (TEdge) { .a = polytope->faces[i].a, .b = polytope->faces[i].b, .free = true };
        edgeList->edges[j+1] = (TEdge) { .a = polytope->faces[i].b, .b = polytope->faces[i].c, .free = true };
        edgeList->edges[j+2] = (TEdge) { .a = polytope->faces[i].c, .b = polytope->faces[i].a, .free = true };
       // printf( "%d %d\n%d %d\n%d %d\n", edgeList->edges[j+0].a, edgeList->edges[j+0].b, edgeList->edges[j+1].a, edgeList->edges[j+1].b, edgeList->edges[j+2].a, edgeList->edges[j+2].b );
    }
}

void EdgeList_MarkHoles( TEdgeList * edgeList ) {
    for( int i = 0; i < edgeList->count; i++ ) {
        for( int j = 0; j < edgeList->count; j++ ) {
            if( edgeList->edges[i].free && edgeList->edges[j].free ) {
                if( edgeList->edges[j].a == edgeList->edges[i].b && edgeList->edges[j].b == edgeList->edges[i].a ) {
                    edgeList->edges[i].free = false;
                    edgeList->edges[j].free = false;
                }
            }
        }
    }
    edgeList->freeCount = 0;
    for( int i = 0; i < edgeList->count; i++ ) {
        if( edgeList->edges[i].free ) {
            edgeList->freeCount++;
        }
    }
    //printf( "Free count: %d\n", edgeList->freeCount );
}

int Polytope_ReserveFaces( TEPAPolytope * polytope, int faceCount ) {
    int last = polytope->faceCount;
    polytope->faceCount += faceCount;
    polytope->faces = realloc( polytope->faces, polytope->faceCount * sizeof( TEPAFace ));
    return last;
}

void Polytope_PatchHoles( TEPAPolytope * polytope, TEdgeList * edgeList, int newPointIndex ) {
    int lastFree = Polytope_ReserveFaces( polytope, edgeList->freeCount );
    for( int i = 0; i < edgeList->count; i++ ) {
        if( edgeList->edges[i].free ) {
            polytope->faces[lastFree] = (TEPAFace) { .a = newPointIndex, .b = edgeList->edges[i].b, .c = edgeList->edges[i].a };
            lastFree++;
        }
    }
}

void Polytope_RemoveFace( TEPAPolytope * polytope, int n ) {
    if( n == 0 ) {
        polytope->faceCount--;
        memmove( polytope->faces, polytope->faces + 1, sizeof( TEPAFace ) * polytope->faceCount );
    } else if( n == polytope->faceCount - 1 ) {
        // keep last face in array but reduce face count
        polytope->faceCount--;
    } else {
        memmove( polytope->faces + n, polytope->faces + n + 1, sizeof( TEPAFace ) * ( polytope->faceCount - n ));
        polytope->faceCount--;
    }
}

int Polytope_AddVertex( TEPAPolytope * polytope, TSupport newSupport ) {
    int last = polytope->vertexCount;
    polytope->vertexCount++;
    polytope->vertices = realloc( polytope->vertices, polytope->vertexCount * sizeof( TSupport ));
    polytope->vertices[last] = newSupport;
    return last;
}

TVec3 Math_ProjectOriginOntoPlane( TVec3 planePoint, TVec3 planeNormal ) {
    float t = -Vec3_Dot( planePoint, planeNormal );
    return Vec3_Negate( Vec3_Scale( planeNormal, t ));
}

float Math_DistanceToOrigin( TVec3 normal, TVec3 point ) {
    return Vec3_Dot( normal, point );
}

void Math_GetBarycentricCoords( TVec3 p, TVec3 a, TVec3 b, TVec3 c, float *u,float *v,float *w ) {
     TVec3 v0 = Vec3_Sub( b, a );
     TVec3 v1 = Vec3_Sub( c, a );
     TVec3 v2 = Vec3_Sub( p, a );
     float d00 = Vec3_Dot( v0, v0 );
     float d01 = Vec3_Dot( v0, v1 );
     float d11 = Vec3_Dot( v1, v1 );
     float d20 = Vec3_Dot( v2, v0 );
     float d21 = Vec3_Dot( v2, v1 );
     float denom = d00 * d11 - d01 * d01;
     *v = (d11 * d20 - d01 * d21) / denom;
     *w = (d00 * d21 - d01 * d20) / denom;
     *u = 1.0f - *v - *w;
}

bool Polytope_IsFaceSeenFromPoint( TEPAPolytope * polytope, int a, int b, int c, TVec3 point ) {
    TVec3 va = polytope->vertices[ a ].minkowskiDifPoint;
    TVec3 vb = polytope->vertices[ b ].minkowskiDifPoint;
    TVec3 vc = polytope->vertices[ c ].minkowskiDifPoint;
    TVec3 normal = Vec3_Cross( Vec3_Sub( vb, va ), Vec3_Sub( vc, va ));
    return Vec3_Dot( normal, Vec3_Sub( point, va )) > 0;
}

int Polytope_GetFirstFaceSeenFromPoint( TEPAPolytope * polytope, TVec3 point ) {
    for( int i = 0; i < polytope->faceCount; i++ ) {
        if( Polytope_IsFaceSeenFromPoint( polytope, polytope->faces[i].a, polytope->faces[i].b, polytope->faces[i].c, point )) {
            return i;
        }
    }
    return -1;
}

bool Polytope_IsFaceDegenerated( TEPAPolytope * polytope, int n ) {
    TVec3 a = polytope->vertices[ polytope->faces[ n ].a ].minkowskiDifPoint;
    TVec3 b = polytope->vertices[ polytope->faces[ n ].b ].minkowskiDifPoint;
    TVec3 c = polytope->vertices[ polytope->faces[ n ].c ].minkowskiDifPoint;

    TVec3 normal = Vec3_Cross( Vec3_Sub( b, a ), Vec3_Sub( c, a ) );

    // degenerated triangle, ignore
    return Vec3_SqrLength( normal ) < 0.00001;
}

void Polytope_RemoveDegeneratedFaces( TEPAPolytope * polytope ) {
    while( true ) {
        int n = -1;
        for( int i = 0; i < polytope->faceCount; i++ ) {
            if( Polytope_IsFaceDegenerated( polytope, i )) {
                Polytope_RemoveFace( polytope, i );
                n = i;
                break;
            }
        }
        if( n < 0 ) {
            break;
        }
    }
}

TEPATriangle Polytope_GetClosestTriangleToOrigin( TEPAPolytope * polytope ) {
    int closest;
    float closestDistance = FLT_MAX;
    TVec3 closestNormal;
    for( int i = 0; i < polytope->faceCount; i++ ) {
        TVec3 a = polytope->vertices[ polytope->faces[ i ].a ].minkowskiDifPoint;
        TVec3 b = polytope->vertices[ polytope->faces[ i ].b ].minkowskiDifPoint;
        TVec3 c = polytope->vertices[ polytope->faces[ i ].c ].minkowskiDifPoint;

        TVec3 normal = Vec3_Cross( Vec3_Sub( b, a ), Vec3_Sub( c, a ) );

        // degenerated triangle, ignore
        if( Vec3_SqrLength( normal ) < 0.00001 ) {
            continue;
        }
        float d = Math_DistanceToOrigin( normal, a );
        if( d < closestDistance ) {
            closestDistance = d;
            closest = i;
            closestNormal = normal;
        }
    }
    return (TEPATriangle) { .a = polytope->vertices[ polytope->faces[ closest ].a ], .b = polytope->vertices[ polytope->faces[ closest ].b ],
                            .c = polytope->vertices[ polytope->faces[ closest ].c ], .normal = closestNormal, .dist = closestDistance, .numInPolytope = closest };
}

bool EPA_ComputeContacts( TEPAPolytope * polytope, TShape * shape1, TShape * shape2, TEPAContact * outContact ) {
    const int convergenceLimit = 100;
    for( int i = 0; i < convergenceLimit; i++ ) {
        Polytope_RemoveDegeneratedFaces( polytope );
        TEPATriangle closestTriangle = Polytope_GetClosestTriangleToOrigin( polytope );
        TSupport p = GJK_GetSupport( shape1, shape2, closestTriangle.normal );
        float d = Vec3_Dot( p.minkowskiDifPoint, closestTriangle.normal );
        if( d - closestTriangle.dist < 0.0001 ) {
            if( d < 0.00001 ) {
                return false;
            }
            TVec3 proj = Math_ProjectOriginOntoPlane( closestTriangle.a.minkowskiDifPoint, closestTriangle.normal );
            float u, v, w;
            Math_GetBarycentricCoords( proj, closestTriangle.a.minkowskiDifPoint, closestTriangle.b.minkowskiDifPoint, closestTriangle.c.minkowskiDifPoint, &u, &v, &w );
            TVec3 a = Vec3_Scale( closestTriangle.a.supportA, u );
            TVec3 b = Vec3_Scale( closestTriangle.b.supportA, v );
            TVec3 c = Vec3_Scale( closestTriangle.c.supportA, w );
            TVec3 collPoint = Vec3_Add( Vec3_Add( a, b ), c );
            closestTriangle.normal = Vec3_Normalize( Vec3_Negate( closestTriangle.normal ));
            outContact->normal = closestTriangle.normal;
            outContact->penetrationDepth = d;
            printf( "EPA: Done in %d iteration(s)!\nClosest: %.3f, %.3f, %.3f\nNormal: %.3f, %.3f, %.3f\nPenetration depth: %f\n", i, collPoint.x, collPoint.y, collPoint.z, closestTriangle.normal.x, closestTriangle.normal.y, closestTriangle.normal.z, d );
            return true;
        } else {
            TEdgeList edgeList;
            while( true ) {
                int seenFace = Polytope_GetFirstFaceSeenFromPoint( polytope, p.minkowskiDifPoint );
                if( seenFace < 0 ) {
                    break;
                } else {
                    Polytope_RemoveFace( polytope, seenFace );
                }
            }
            EdgeList_Create( &edgeList, polytope );
            EdgeList_Fill( &edgeList, polytope );
            EdgeList_MarkHoles( &edgeList );
            Polytope_PatchHoles( polytope, &edgeList, Polytope_AddVertex( polytope, p ) );
            EdgeList_Free( &edgeList );
        }
    }
    printf( "EPA: Convergence limit has reached!\n" );
    return false;
}

// ===========================
// TRIANGLE MESH SHAPE ROUTINE
// ===========================
TShape * TriangleMesh_CreatePlane( float width, float depth ) {
    TShape * shape = calloc( 1, sizeof( TShape ));
    TTriangleMeshShape * triMesh = calloc( 1, sizeof( TTriangleMeshShape ));
    triMesh->vertexCount = 4;
    triMesh->vertices = calloc( triMesh->vertexCount, sizeof( TVec3 ));
    float hw = width / 2, hd = depth / 2;
    triMesh->vertices[0] = Vec3_Set( -hw, 0.0, -hd );
    triMesh->vertices[1] = Vec3_Set( -hw, 0.0,  hd );
    triMesh->vertices[2] = Vec3_Set(  hw, 0.0,  hd );
    triMesh->vertices[3] = Vec3_Set(  hw, 0.0, -hd );
    triMesh->faceCount = 2;
    triMesh->faces = calloc( triMesh->faceCount, sizeof( TEPAFace ));
    triMesh->faces[0] = (TEPAFace) { 0, 1, 2 };
    triMesh->faces[1] = (TEPAFace) { 0, 2, 3 };
    shape->triMesh = triMesh;
    return shape;
}

// ===========================
// BODY ROUTINE
// ===========================
TBody * Body_Create( TShape * shape ) {
    TBody * body = calloc( 1, sizeof( TBody ));
    body->position = shape->position;
    body->shape = shape;
    return body;
}

void Body_ProcessCollision( TBody * body1, TShape * shape ) {
    TSimplex finalSimplex;
    TEPAContact contact;
    while( GJK_IsIntersects( body1->shape, shape, &finalSimplex )) {
        if( finalSimplex.size == 4 ) {
            if( currentPolytope ) {
                Polytope_Free( currentPolytope );
                free( currentPolytope );
            }
            currentPolytope = calloc( 1, sizeof( TEPAPolytope ));
            Polytope_SetFromSimplex( currentPolytope, &finalSimplex );
            if( EPA_ComputeContacts( currentPolytope, body1->shape, shape, &contact )) {
                body1->position = Vec3_Add( body1->position, Vec3_Scale( contact.normal, contact.penetrationDepth / 2 ));
            } else {
                printf( "EPA: Unable to compute contacts!\n" );
                break;
            }
            body1->shape->position = body1->position;
        } else {
            printf( "Wrong simplex size! Must be tetrahedron, got %d\n", finalSimplex.size );
            break;
        }
    }
}

void Body_SolveCollision( TBody * body1, TBody * body2 ) {
    if( body1->shape->triMesh ) {
        TBody * temp = body1;
        body1 = body2;
        body2 = temp;
    }

    currentShape1 = body1->shape;
    currentShape2 = body2->shape;

    body1->shape->position = body1->position;
    body2->shape->position = body2->position;

    if( body2->shape->triMesh ) {
        TTriangleMeshShape * triMesh = body2->shape->triMesh;
        for( int i = 0; i < triMesh->faceCount; i++ ) {
            TTriangleShape triangle = { .a = triMesh->vertices[ triMesh->faces[i].a ], .b = triMesh->vertices[ triMesh->faces[i].b ], .c = triMesh->vertices[ triMesh->faces[i].c ] };
            TShape shape = { .position = body2->position, .triangle = &triangle };
            Body_ProcessCollision( body1, &shape );
        }
    } else {
        Body_ProcessCollision( body1, body2->shape );
    }
}

// ===========================
// RENDERING ROUTINE
// ===========================
#include <windows.h>
#include "glut.h"
#include "gl/gl.h"

#define WINDOW_SIZE 800

void Renderer_DrawShape( TShape * shape ) {
   // glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );

    if( shape->convex ) {
        if( shape->convex->count == 3 ) { // triangle
            glPushMatrix();

            glTranslatef( shape->position.x, shape->position.y, shape->position.z );

            TVec3 a = shape->convex->points[0];
            TVec3 b = shape->convex->points[1];
            TVec3 c = shape->convex->points[2];

            glBegin(GL_TRIANGLES);
            glColor3ub( 0, 0, 255 );
            glVertex3f( a.x, a.y, a.z );
            glVertex3f( b.x, b.y, b.z );
            glVertex3f( c.x, c.y, c.z );

            glEnd();

            glPopMatrix();
        }

        if( shape->convex->count == 4 ) { // tetrahedron
            glPushMatrix();

            glTranslatef( shape->position.x, shape->position.y, shape->position.z );

            TVec3 a = shape->convex->points[0];
            TVec3 b = shape->convex->points[1];
            TVec3 c = shape->convex->points[2];
            TVec3 d = shape->convex->points[3];

            glBegin(GL_TRIANGLES);
            glColor3ub( 255, 0, 0 );
            glVertex3f( a.x, a.y, a.z );
            glVertex3f( b.x, b.y, b.z );
            glVertex3f( c.x, c.y, c.z );

            glColor3ub( 0, 255, 0 );
            glVertex3f( a.x, a.y, a.z );
            glVertex3f( d.x, d.y, d.z );
            glVertex3f( b.x, b.y, b.z );

            glColor3ub( 0, 0, 255 );
            glVertex3f( a.x, a.y, a.z );
            glVertex3f( c.x, c.y, c.z );
            glVertex3f( d.x, d.y, d.z );

            glColor3ub( 255, 255, 0 );
            glVertex3f( c.x, c.y, c.z );
            glVertex3f( b.x, b.y, b.z );
            glVertex3f( d.x, d.y, d.z );

            glEnd();

            glPopMatrix();
        }
    } else if( shape->sphere ) {

        glPushMatrix();
        glTranslatef( shape->position.x, shape->position.y, shape->position.z );
        glutSolidSphere( shape->sphere->radius, 32, 32 );
        glPopMatrix();

    } else if( shape->box ) {
        /*
            shape->box->vertices[0] = Vec3_Set( -hs, -hs, -hs );
    shape->box->vertices[1] = Vec3_Set( -hs, -hs,  hs );
    shape->box->vertices[2] = Vec3_Set(  hs, -hs, -hs );
    shape->box->vertices[3] = Vec3_Set(  hs, -hs,  hs );

    shape->box->vertices[4] = Vec3_Set( -hs,  hs, -hs );
    shape->box->vertices[5] = Vec3_Set( -hs,  hs,  hs );
    shape->box->vertices[6] = Vec3_Set(  hs,  hs, -hs );
    shape->box->vertices[7] = Vec3_Set(  hs,  hs,  hs );*/


        glPushMatrix();
        glScalef( shape->box->size, shape->box->size, shape->box->size );
        glTranslatef( shape->position.x, shape->position.y, shape->position.z );
        glutSolidCube( 1 );
        glPopMatrix();
        /*
        glBegin( GL_QUADS );

        glVertex3f( shape->box->vertices[0].x + shape->position.x, shape->box->vertices[0].y + shape->position.y, shape->box->vertices[0].z + shape->position.z );
        glVertex3f( shape->box->vertices[1].x + shape->position.x, shape->box->vertices[1].y + shape->position.y, shape->box->vertices[1].z + shape->position.z );
        glVertex3f( shape->box->vertices[2].x + shape->position.x, shape->box->vertices[2].y + shape->position.y, shape->box->vertices[2].z + shape->position.z );
        glVertex3f( shape->box->vertices[3].x + shape->position.x, shape->box->vertices[3].y + shape->position.y, shape->box->vertices[3].z + shape->position.z );

        glVertex3f( shape->box->vertices[4].x + shape->position.x, shape->box->vertices[4].y + shape->position.y, shape->box->vertices[4].z + shape->position.z );
        glVertex3f( shape->box->vertices[5].x + shape->position.x, shape->box->vertices[5].y + shape->position.y, shape->box->vertices[5].z + shape->position.z );
        glVertex3f( shape->box->vertices[6].x + shape->position.x, shape->box->vertices[6].y + shape->position.y, shape->box->vertices[6].z + shape->position.z );
        glVertex3f( shape->box->vertices[7].x + shape->position.x, shape->box->vertices[7].y + shape->position.y, shape->box->vertices[7].z + shape->position.z );

        glEnd();*/

    } else if( shape->triMesh ) {
        glPushMatrix();
        glTranslatef( shape->position.x, shape->position.y, shape->position.z );

        glColor3ub( 125, 200, 80 );

        glBegin(GL_TRIANGLES);
        for( int i = 0; i < shape->triMesh->faceCount; i++ ) {

            TVec3 a = shape->triMesh->vertices[ shape->triMesh->faces[ i ].a ];
            TVec3 b = shape->triMesh->vertices[ shape->triMesh->faces[ i ].b ];
            TVec3 c = shape->triMesh->vertices[ shape->triMesh->faces[ i ].c ];

            glVertex3f( a.x, a.y, a.z );
            glVertex3f( b.x, b.y, b.z );
            glVertex3f( c.x, c.y, c.z );
        }
        glEnd();

        glPopMatrix();
    }
}

void Renderer_Display() {
    static float a = 0;
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    gluLookAt( 0, 0.4, 2.2, 0, 0, 0, 0, 1, 0 );
    glRotatef( a, 0, 1, 0 );
   // a+=0.725;

   /*
    if( currentPolytope ) {
        //glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );

        for( int i = 0; i < currentPolytope->faceCount; i++ ) {
            int ia = currentPolytope->faces[i].a;
            int ib = currentPolytope->faces[i].b;
            int ic = currentPolytope->faces[i].c;

            TVec3 a = currentPolytope->vertices[ ia ].minkowskiDifPoint;
            TVec3 b = currentPolytope->vertices[ ib ].minkowskiDifPoint;
            TVec3 c = currentPolytope->vertices[ ic ].minkowskiDifPoint;

            TVec3 normal = Vec3_Normalize( Vec3_Cross( Vec3_Sub( b,a ), Vec3_Sub( c,a )));

            glBegin(GL_TRIANGLES);
            //glColor3ub( i * 8 + 50, i * 8 + 50, i * 8 + 50 );
            glVertex3f( a.x, a.y, a.z );
            glVertex3f( b.x, b.y, b.z );
            glVertex3f( c.x, c.y, c.z );
            glEnd();

            glBegin(GL_LINES);
            glColor3ub( 255, 0, 255 );
            glVertex3f( a.x * 0.333f + b.x * 0.333f + c.x * 0.333f, a.y * 0.333f + b.y * 0.333f + c.y * 0.333f, a.z * 0.333f + b.z * 0.333f + c.z * 0.333f );
            glVertex3f( a.x * 0.333f + b.x * 0.333f + c.x * 0.333f + normal.x, a.y * 0.333f + b.y * 0.333f + c.y * 0.333f + normal.y, a.z * 0.333f + b.z * 0.333f + c.z * 0.333f + normal.z );
            glEnd();
        }
    }*/
    glColor3ub( 255, 0, 0 );
    Renderer_DrawShape( currentShape1 );
    glColor3ub( 255, 255, 0 );
    Renderer_DrawShape( currentShape2 );
    glColor3ub( 0, 255, 255 );
    Renderer_DrawShape( currentShape3 );
    glutSwapBuffers();
    glutPostRedisplay();
}

void Renderer_Init() {
    glClearColor(0.000, 0.110, 0.392, 0.0);

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective( 80, 1, 0.01, 1024 );



    glClearDepth( 1.0f );
    glEnable( GL_DEPTH_TEST );
    glDepthFunc( GL_LEQUAL );
    glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST );
    glEnable( GL_TEXTURE_2D );
    glShadeModel( GL_SMOOTH );

    glEnable( GL_CULL_FACE );

    glDisable( GL_STENCIL_TEST );
    glCullFace( GL_BACK );
    glEnable( GL_ALPHA_TEST );
    glAlphaFunc( GL_GREATER, 0.025f );


    //currentShape1 = ConvexShape_CreateTetrahedron( Vec3_Set( -1, .5, 0 ), Vec3_Set( -1, 1.5, 0 ), Vec3_Set( 0.1, 0.5, 0 ), Vec3_Set( -1, 0.5, 1 ));
    //currentShape1 = ConvexShape_CreateTetrahedron( Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ), Vec3_Set( 0, 0, 1 ));

    //currentShape2 = ConvexShape_CreateSphere( Vec3_Set( 0.5,0,0.0), 0.8 );
    //currentShape2 = ConvexShape_CreateTriangle( Vec3_Set( 0, 0, 0.5 ), Vec3_Set( 0.5, 1, 0.5 ), Vec3_Set( 0.5, 0, 0 ));
   // currentShape2 = ConvexShape_CreateTriangle( Vec3_Set( 0, 0.1, 0 ), Vec3_Set( 0, 0.1, 1 ), Vec3_Set( 1, 0.1, 0 ));

    currentShape1 = BoxShape_Create( 1 );
    currentShape2 = TriangleMesh_CreatePlane( 2, 2 );
    currentShape3 = BoxShape_Create( 1 );//ConvexShape_CreateSphere( Vec3_Set( 0,0,0 ), 0.2 );

    body1 = Body_Create( currentShape1 );
    body1->position = Vec3_Set( 0, 0.5, 0 );

    body2 = Body_Create( currentShape2 );

    body3 = Body_Create( currentShape3 );
    body3->position = Vec3_Set( 0, 1, 0 );

    Body_SolveCollision( body1, body3 );
    //Body_SolveCollision( body1, body2 );
    //Body_SolveCollision( body3, body2 );
}

void KeyFunc( unsigned char key, int x, int y ) {
    bool move = false;
    if( key == 'A' || key == 'a' ) {
        body1->position = Vec3_Add( body1->position, Vec3_Set( -0.01, 0.0, 0.0 ));
        move = true;
    }
    if( key == 'D' || key == 'd' ) {
        body1->position = Vec3_Add( body1->position, Vec3_Set(  0.01, 0.0, 0.0 ));
        move = true;
    }
    if( key == 'W' || key == 'w' ) {
        body1->position = Vec3_Add( body1->position, Vec3_Set( 0.0, 0.0, -0.01 ));
        move = true;
    }
    if( key == 'S' || key == 's' ) {
        body1->position = Vec3_Add( body1->position, Vec3_Set( 0, 0.0, 0.01 ));
        move = true;
    }
    if( key == 'X' || key == 'x' ) {
        body1->position = Vec3_Add( body1->position, Vec3_Set( 0.0, 0.01, 0.0 ));
        move = true;
    }
    if( key == 'Z' || key == 'z' ) {
        body1->position = Vec3_Add( body1->position, Vec3_Set( 0, -0.01, 0.0 ));
        move = true;
    }
    if( move ) {
        Body_SolveCollision( body1, body3 );
        //Body_SolveCollision( body1, body2 );
       // Body_SolveCollision( body3, body2 );
    }
}

// ===========================
// TESTS
// ===========================
int main(int argc, char **argv) {
    glutInit(&argc, argv);
    glutInitDisplayMode( GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA );
    glutInitWindowSize( 800, 800 );
    glutInitWindowPosition(0, 0);
    glutCreateWindow("Test");
    glutDisplayFunc(Renderer_Display);
    glutKeyboardFunc( KeyFunc );

    Renderer_Init();

    glutMainLoop();
}