API tools faq
paste
mrDIMAS

GJK EPA 3 (good)

mrDIMAS
Oct 12th, 2015
274
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 28.24 KB | None | 0 0
raw download clone embed print report
  1. #include <stdio.h>
  2. #include <stdlib.h>
  3. #include <float.h>
  4. #include <stdbool.h>
  5. #include <string.h>
  6.  
  7. #include "vector3.h"
  8.  
  9. typedef struct TSupport {
  10.     TVec3 supportA;
  11.     TVec3 minkowskiDifPoint;
  12. } TSupport;
  13.  
  14. typedef struct TSimplex {
  15.     TSupport points[4];
  16.     int size;
  17. } TSimplex;
  18.  
  19. typedef struct TSphereShape {
  20.     float radius;
  21.     TVec3 position;
  22. } TSphereShape;
  23.  
  24. typedef struct TConvexShape {
  25.     TVec3 * points;
  26.     int count;
  27.     TVec3 position;
  28. } TConvexShape;
  29.  
  30. typedef struct TShape {
  31.     TConvexShape * convex;
  32.     TSphereShape * sphere;
  33. } TShape;
  34.  
  35. typedef struct TEPATriangle {
  36.     TSupport a, b, c;
  37.     TVec3 normal;
  38.     float dist;
  39.     int numInPolytope;
  40. } TEPATriangle;
  41.  
  42. typedef struct TEPAContact {
  43.     TVec3 normal;
  44.     float penetrationDepth;
  45. } TEPAContact;
  46.  
  47. typedef struct TEPAFace {
  48.     int a, b, c;
  49. } TEPAFace;
  50.  
  51. typedef struct TEPAPolytope {
  52.     TSupport * vertices;
  53.     int vertexCount;
  54.     TEPAFace * faces;
  55.     int faceCount;
  56. } TEPAPolytope;
  57.  
  58. typedef struct TEdge {
  59.     int a;
  60.     int b;
  61.     bool free;
  62. } TEdge;
  63.  
  64. typedef struct TEdgeList {
  65.     TEdge * edges;
  66.     int count;
  67.     int freeCount;
  68. } TEdgeList;
  69.  
  70. #ifndef M_PI
  71. #define M_PI 3.14159
  72. #endif
  73.  
  74. void DrawPolytope( TEPAPolytope * polytope );
  75.  
  76. TEPAPolytope * currentPolytope;
  77. TShape * currentShape1;
  78. TShape * currentShape2;
  79. // ===========================
  80. // HELPERS
  81. // ===========================
  82. bool Helper_SameDirection( TVec3 a, TVec3 b ) {
  83.     return Vec3_Dot( a, b ) > 0;
  84. }
  85.  
  86. // ===========================
  87. // SIMPLEX ROUTINE
  88. // ===========================
  89. void Simplex_RemovePoint( TSimplex * s, int num ) {
  90.     if( s->size > 0 ) {
  91.         if( num == 0 ) {
  92.             s->points[0] = s->points[1];
  93.             s->points[1] = s->points[2];
  94.             s->points[2] = s->points[3];
  95.         }
  96.         if( num == 1 ) {
  97.             s->points[1] = s->points[2];
  98.             s->points[2] = s->points[3];
  99.         }
  100.         if( num == 2 ) {
  101.             s->points[2] = s->points[3];
  102.         }
  103.         s->size--;
  104.     }
  105. }
  106.  
  107. void Simplex_AddPoint( TSimplex * s, TSupport p ) {
  108.     if( s->size < 4 ) {
  109.         s->points[ s->size ] = p;
  110.         s->size++;
  111.     }
  112. }
  113.  
  114. // ===========================
  115. // CONVEX SHAPE ROUTINE
  116. // ===========================
  117. TShape * ConvexShape_CreateTriangle( TVec3 a, TVec3 b, TVec3 c ) {
  118.     TShape * shape = calloc( 1, sizeof( TShape ));
  119.     shape->convex = calloc( 1, sizeof( TConvexShape ));
  120.     shape->convex->count = 3;
  121.     shape->convex->points = malloc( shape->convex->count * sizeof( TVec3 ));
  122.     shape->convex->points[0] = a;
  123.     shape->convex->points[1] = b;
  124.     shape->convex->points[2] = c;
  125.     return shape;
  126. }
  127.  
  128. TShape * ConvexShape_CreateTetrahedron( TVec3 a, TVec3 b, TVec3 c, TVec3 d ) {
  129.     TShape * shape = calloc( 1, sizeof( TShape ));
  130.     shape->convex = calloc( 1, sizeof( TConvexShape ));
  131.     shape->convex->count = 4;
  132.     shape->convex->points = malloc( shape->convex->count * sizeof( TVec3 ));
  133.     shape->convex->points[0] = a;
  134.     shape->convex->points[1] = b;
  135.     shape->convex->points[2] = c;
  136.     shape->convex->points[3] = d;
  137.     shape->convex->position = Vec3_Set( 0,0,0 );
  138.     return shape;
  139. }
  140.  
  141. TShape * ConvexShape_CreateSphere( TVec3 position, float radius ) {
  142.     TShape * shape = calloc( 1, sizeof( TShape ));
  143.     shape->sphere = calloc( 1, sizeof( TSphereShape ));    
  144.     shape->sphere->position = position;
  145.     shape->sphere->radius = radius;
  146.     return shape;
  147. }
  148.  
  149. void ConvexShape_Delete( TShape * shape ) {
  150.     if( shape->convex ) {
  151.         free( shape->convex );
  152.     }
  153.     if( shape->sphere ) {
  154.         free( shape->sphere );
  155.     }
  156.     free( shape );
  157. }
  158.  
  159. TVec3 ConvexShape_GetFarthestPointInDirection( TShape * shape, TVec3 dir ) {
  160.     const float eps = 0.000001;
  161.     if( fabs( dir.x ) < eps && fabs( dir.y ) < eps && fabs( dir.z ) < eps ) {
  162.         printf( "GJK Warning: Zero direction passed!\n" );
  163.     }
  164.        
  165.     if( shape->convex ) {
  166.         TVec3 farthest;
  167.         float lastDot = -FLT_MAX;
  168.         for( int i = 0; i < shape->convex->count; i++ ) {
  169.             float dot = Vec3_Dot( dir, shape->convex->points[i] );
  170.             if( dot > lastDot ) {
  171.                 farthest = shape->convex->points[i];
  172.                 lastDot = dot;
  173.             }
  174.         }
  175.         farthest = Vec3_Add( farthest, shape->convex->position );
  176.         return farthest;
  177.     }
  178.     if( shape->sphere ) {
  179.         return Vec3_Add( shape->sphere->position, Vec3_Scale( Vec3_Normalize( dir ), shape->sphere->radius ));
  180.     }
  181.     return Vec3_Set( 0, 0, 0 );
  182. }
  183.  
  184. // ===========================
  185. // GJK ALGORITHM ROUTINE
  186. // ===========================
  187. TSupport GJK_GetSupport( TShape * shape1, TShape * shape2, TVec3 dir ) {
  188.     TSupport sup;
  189.     sup.supportA = ConvexShape_GetFarthestPointInDirection( shape1, dir );
  190.     sup.minkowskiDifPoint = Vec3_Sub( sup.supportA, ConvexShape_GetFarthestPointInDirection( shape2, Vec3_Negate( dir )));
  191.     return sup;
  192. }
  193.  
  194. bool GJK_ProcessLine( TSimplex * simplex, TVec3 * outDirection ) {
  195.     TVec3 a = simplex->points[1].minkowskiDifPoint;
  196.     TVec3 b = simplex->points[0].minkowskiDifPoint;
  197.     TVec3 ab = Vec3_Sub( b, a );
  198.     TVec3 aO = Vec3_Negate( a );
  199.    
  200.     if( Helper_SameDirection( ab, aO )) {
  201.         *outDirection = Vec3_Cross( Vec3_Cross( ab, aO ), ab );
  202.     } else {
  203.         Simplex_RemovePoint( simplex, 0 );
  204.         *outDirection = aO;
  205.     }
  206.     return false;
  207. }
  208.  
  209. bool GJK_ProcessTriangle( TSimplex * simplex, TVec3 * outDirection ) {
  210.     TVec3 a = simplex->points[2].minkowskiDifPoint;
  211.     TVec3 b = simplex->points[1].minkowskiDifPoint;
  212.     TVec3 c = simplex->points[0].minkowskiDifPoint;
  213.     TVec3 aO = Vec3_Negate( a );
  214.     TVec3 ab = Vec3_Sub( b, a );
  215.     TVec3 ac = Vec3_Sub( c, a );
  216.     TVec3 abc = Vec3_Cross( ab, ac );
  217.     TVec3 acNormal = Vec3_Cross( abc, ac );
  218.     TVec3 abNormal = Vec3_Cross( ab, abc );
  219.    
  220.     if( Helper_SameDirection( acNormal, aO )) {
  221.         if( Helper_SameDirection( ac, aO )) {
  222.             Simplex_RemovePoint( simplex, 1 );
  223.             *outDirection = Vec3_Cross( Vec3_Cross(ac, aO), ac );
  224.         } else {
  225.             if( Helper_SameDirection( ab, aO )) {
  226.                 Simplex_RemovePoint( simplex, 0 );
  227.                 *outDirection = Vec3_Cross( Vec3_Cross(ab, aO), ab);
  228.             } else {
  229.                 Simplex_RemovePoint( simplex, 1 );
  230.                 Simplex_RemovePoint( simplex, 0 );
  231.                 *outDirection = aO;
  232.             }
  233.         }
  234.     } else {
  235.         if( Helper_SameDirection( abNormal, aO )) {
  236.             if( Helper_SameDirection( ab, aO )) {
  237.                 Simplex_RemovePoint( simplex, 0 );
  238.                 *outDirection = Vec3_Cross( Vec3_Cross(ab, aO), ab);
  239.             } else {
  240.                 Simplex_RemovePoint( simplex, 1 );
  241.                 Simplex_RemovePoint( simplex, 0 );
  242.                 *outDirection = aO;
  243.             }
  244.         } else {
  245.             if( Helper_SameDirection( abc, aO )) {
  246.                 *outDirection = Vec3_Cross(Vec3_Cross(abc, aO), abc);
  247.             } else {
  248.                 *outDirection = Vec3_Cross(Vec3_Cross( Vec3_Negate( abc ), aO), Vec3_Negate( abc ) );
  249.             }
  250.         }
  251.     }
  252.    
  253.     return false;
  254. }
  255.  
  256. bool GJK_ProcessTetrahedron( TSimplex * simplex, TVec3 * outDirection ) {
  257.     TVec3 a = simplex->points[3].minkowskiDifPoint;
  258.     TVec3 b = simplex->points[2].minkowskiDifPoint;
  259.     TVec3 c = simplex->points[1].minkowskiDifPoint;
  260.     TVec3 d = simplex->points[0].minkowskiDifPoint;
  261.    
  262.     TVec3 ac = Vec3_Sub( c, a );
  263.     TVec3 ab = Vec3_Sub( b, a );
  264.     TVec3 ad = Vec3_Sub( d, a );
  265.    
  266.     TVec3 acd = Vec3_Cross( ad, ac );
  267.     TVec3 abd = Vec3_Cross( ab, ad );
  268.     TVec3 abc = Vec3_Cross( ac, ab );
  269.    
  270.     TVec3 aO = Vec3_Negate( a );
  271.    
  272.     if( Helper_SameDirection( abc, aO )) {
  273.         if( Helper_SameDirection( Vec3_Cross( abc, ac ), aO )) {
  274.             Simplex_RemovePoint( simplex, 2 );
  275.             Simplex_RemovePoint( simplex, 0 );
  276.             *outDirection = Vec3_Cross( Vec3_Cross( ac, aO ), ac );
  277.         } else if( Helper_SameDirection( Vec3_Cross( ab, abc ), aO )) {
  278.             Simplex_RemovePoint( simplex, 1 );
  279.             Simplex_RemovePoint( simplex, 0 );
  280.             *outDirection = Vec3_Cross( Vec3_Cross( ab, aO ), ab );
  281.         } else {
  282.             Simplex_RemovePoint( simplex, 0 );
  283.             *outDirection = abc;
  284.         }
  285.     } else if( Helper_SameDirection( acd, aO )) {
  286.         if( Helper_SameDirection( Vec3_Cross( acd, ad ), aO )) {
  287.             Simplex_RemovePoint( simplex, 2 );
  288.             Simplex_RemovePoint( simplex, 1 );
  289.             *outDirection = Vec3_Cross( Vec3_Cross( ad, aO ), ad );
  290.         } else if ( Helper_SameDirection( Vec3_Cross( ac, acd ), aO )) {
  291.             Simplex_RemovePoint( simplex, 2 );
  292.             Simplex_RemovePoint( simplex, 0 );
  293.             *outDirection = Vec3_Cross( Vec3_Cross( ac, aO ), ac );
  294.         } else {
  295.            Simplex_RemovePoint( simplex, 2 );
  296.             *outDirection = acd;
  297.         }
  298.     } else if( Helper_SameDirection( abd, aO )) {
  299.         if( Helper_SameDirection( Vec3_Cross( abd, ab ), aO )) {
  300.             Simplex_RemovePoint( simplex, 1 );
  301.             Simplex_RemovePoint( simplex, 0 );
  302.             *outDirection = Vec3_Cross( Vec3_Cross( ab, aO ), ab );
  303.         } else if( Helper_SameDirection( Vec3_Cross( ad, abd ), aO )) {
  304.             Simplex_RemovePoint( simplex, 2 );
  305.             Simplex_RemovePoint( simplex, 1 );
  306.             *outDirection = Vec3_Cross( Vec3_Cross( ad, aO ), ad );
  307.         } else {
  308.             Simplex_RemovePoint( simplex, 1 );
  309.             *outDirection = abd;
  310.         }
  311.     } else {
  312.         return true;
  313.     }
  314.    
  315.     return false;
  316. }
  317.  
  318. bool GJK_ProcessSimplex( TSimplex * simplex, TVec3 * outDirection ) {
  319.     if( simplex->size == 2 ) {
  320.         return GJK_ProcessLine( simplex, outDirection );
  321.     } else if ( simplex->size == 3 ) {
  322.         return GJK_ProcessTriangle( simplex, outDirection );
  323.     } else {
  324.         return GJK_ProcessTetrahedron( simplex, outDirection );
  325.     }
  326. }
  327.  
  328. bool GJK_IsIntersects( TShape * shape1, TShape * shape2, TSimplex * finalSimplex ) {
  329.     TVec3 dir = Vec3_Set( 1, 1, 1 );
  330.    
  331.     TSimplex simplex = { 0 };
  332.     Simplex_AddPoint( &simplex, GJK_GetSupport( shape1, shape2, dir ));
  333.    
  334.     dir = Vec3_Negate( dir );
  335.    
  336.     int convergenceLimit = 50;
  337.     for( int i = 0; i < convergenceLimit; i++ ) {
  338.         TSupport lastSupport = GJK_GetSupport( shape1, shape2, dir );              
  339.         if( Helper_SameDirection( dir, lastSupport.minkowskiDifPoint )) {
  340.             Simplex_AddPoint( &simplex, lastSupport );            
  341.             if( GJK_ProcessSimplex( &simplex, &dir )) {
  342.                 printf( "GJK: Intersection! %d iteration(s)!\n", i );
  343.                 if( finalSimplex ) {
  344.                     *finalSimplex = simplex;
  345.                 }
  346.                 return true;
  347.             }            
  348.         } else {
  349.             printf( "GJK: No intersection! %d iteration(s)!\n", i );            
  350.             return false;
  351.         }
  352.     }
  353.    
  354.     printf( "GJK: No intersection! Convergence limit has reached!\n" );
  355.     return false;
  356. }
  357.  
  358. // ===========================
  359. // EPA ALGORITHM ROUTINE
  360. // ===========================
  361. void Polytope_SetFromSimplex( TEPAPolytope * polytope, TSimplex * simplex ) {
  362.     polytope->vertexCount = 4;
  363.     polytope->vertices = calloc( polytope->vertexCount, sizeof( TSupport ));
  364.     for( int i = 0; i < polytope->vertexCount; i++ ) {
  365.         polytope->vertices[i] = simplex->points[i];
  366.     }
  367.     polytope->faceCount = 4;
  368.     polytope->faces = calloc( polytope->faceCount, sizeof( TEPAFace ));
  369.     polytope->faces[0] = (TEPAFace) { 0, 1, 2 };
  370.     polytope->faces[1] = (TEPAFace) { 0, 3, 1 };
  371.     polytope->faces[2] = (TEPAFace) { 0, 2, 3 };
  372.     polytope->faces[3] = (TEPAFace) { 2, 1, 3 };
  373. }
  374.  
  375. void EPA_CreateEdgeList( TEdgeList * edgeList, TEPAPolytope * polytope ) {
  376.     edgeList->count = polytope->faceCount * 3;
  377.     edgeList->edges = calloc( edgeList->count, sizeof( TEdge ));
  378. }
  379.  
  380. void EPA_FreeEdgeList( TEdgeList * edgeList ) {
  381.     free( edgeList->edges );
  382.     edgeList->count = 0;
  383. }
  384.  
  385. void EPA_FillEdgeList( TEdgeList * edgeList, TEPAPolytope * polytope ) {
  386.     for( int i = 0, j = 0; i < polytope->faceCount; i++, j += 3 ) {
  387.         edgeList->edges[j+0] = (TEdge) { .a = polytope->faces[i].a, .b = polytope->faces[i].b, .free = true };
  388.         edgeList->edges[j+1] = (TEdge) { .a = polytope->faces[i].b, .b = polytope->faces[i].c, .free = true };
  389.         edgeList->edges[j+2] = (TEdge) { .a = polytope->faces[i].c, .b = polytope->faces[i].a, .free = true };
  390.         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 );
  391.     }
  392. }
  393.  
  394. void EPA_MarkHoles( TEdgeList * edgeList ) {
  395.     for( int i = 0; i < edgeList->count; i++ ) {
  396.         for( int j = 0; j < edgeList->count; j++ ) {
  397.             if( edgeList->edges[i].free && edgeList->edges[j].free ) {
  398.                 if( edgeList->edges[j].a == edgeList->edges[i].b && edgeList->edges[j].b == edgeList->edges[i].a ) {
  399.                     edgeList->edges[i].free = false;
  400.                     edgeList->edges[j].free = false;
  401.                 }
  402.             }
  403.         }
  404.     }
  405.     edgeList->freeCount = 0;
  406.     for( int i = 0; i < edgeList->count; i++ ) {
  407.         if( edgeList->edges[i].free ) {
  408.             edgeList->freeCount++;
  409.         }
  410.     }
  411.     printf( "Free count: %d\n", edgeList->freeCount );
  412. }
  413.  
  414. int Polytope_ReserveFaces( TEPAPolytope * polytope, int faceCount ) {
  415.     int last = polytope->faceCount;
  416.     polytope->faceCount += faceCount;
  417.     polytope->faces = realloc( polytope->faces, polytope->faceCount * sizeof( TEPAFace ));
  418.     return last;
  419. }
  420.  
  421. void EPA_FillHoles( TEPAPolytope * polytope, TEdgeList * edgeList, int newPointIndex ) {
  422.     int lastFree = Polytope_ReserveFaces( polytope, edgeList->freeCount );
  423.     for( int i = 0; i < edgeList->count; i++ ) {
  424.         if( edgeList->edges[i].free ) {
  425.             polytope->faces[lastFree] = (TEPAFace) { .a = newPointIndex, .b = edgeList->edges[i].b, .c = edgeList->edges[i].a };
  426.             lastFree++;
  427.         }
  428.     }
  429. }
  430.  
  431. void Polytope_RemoveFace( TEPAPolytope * polytope, int n ) {
  432.     if( n == 0 ) {
  433.         polytope->faceCount--;
  434.         memmove( polytope->faces, polytope->faces + 1, sizeof( TEPAFace ) * polytope->faceCount );
  435.     } else if( n == polytope->faceCount - 1 ) {
  436.         // keep last face in array but reduce face count
  437.         polytope->faceCount--;
  438.     } else {
  439.         memmove( polytope->faces + n, polytope->faces + n + 1, sizeof( TEPAFace ) * ( polytope->faceCount - n ));
  440.         polytope->faceCount--;
  441.     }
  442. }
  443.  
  444. int Polytope_AddVertex( TEPAPolytope * polytope, TSupport newSupport ) {
  445.     int last = polytope->vertexCount;
  446.     polytope->vertexCount++;
  447.     polytope->vertices = realloc( polytope->vertices, polytope->vertexCount * sizeof( TSupport ));
  448.     polytope->vertices[last] = newSupport;
  449.     return last;
  450. }
  451.  
  452. TVec3 EPA_ProjectOriginOntoPlane( TVec3 planePoint, TVec3 planeNormal ) {
  453.     float t = -Vec3_Dot( planePoint, planeNormal );
  454.     return Vec3_Negate( Vec3_Scale( planeNormal, t ));
  455. }
  456.  
  457. float EPA_DistanceToOrigin( TVec3 normal, TVec3 point ) {
  458.     return fabs( Vec3_Dot( normal, Vec3_Negate( point )));
  459. }
  460.  
  461. void EPA_GetBarycentricCoords( TVec3 p, TVec3 a, TVec3 b, TVec3 c, float *u,float *v,float *w ) {
  462.      TVec3 v0 = Vec3_Sub( b, a );
  463.      TVec3 v1 = Vec3_Sub( c, a );
  464.      TVec3 v2 = Vec3_Sub( p, a );
  465.      float d00 = Vec3_Dot( v0, v0 );
  466.      float d01 = Vec3_Dot( v0, v1 );
  467.      float d11 = Vec3_Dot( v1, v1 );
  468.      float d20 = Vec3_Dot( v2, v0 );
  469.      float d21 = Vec3_Dot( v2, v1 );
  470.      float denom = d00 * d11 - d01 * d01;
  471.      *v = (d11 * d20 - d01 * d21) / denom;
  472.      *w = (d00 * d21 - d01 * d20) / denom;
  473.      *u = 1.0f - *v - *w;
  474. }
  475.  
  476. bool EPA_IsFaceSeenFromPoint( TEPAPolytope * polytope, int a, int b, int c, TVec3 point ) {
  477.     TVec3 va = polytope->vertices[ a ].minkowskiDifPoint;
  478.     TVec3 vb = polytope->vertices[ b ].minkowskiDifPoint;
  479.     TVec3 vc = polytope->vertices[ c ].minkowskiDifPoint;
  480.     TVec3 normal = Vec3_Cross( Vec3_Sub( vb, va ), Vec3_Sub( vc, va ));
  481.     return Vec3_Dot( normal, Vec3_Sub( point, va )) > 0;
  482. }
  483.  
  484. int EPA_GetFirstFaceSeenFromPoint( TEPAPolytope * polytope, TVec3 point ) {
  485.     for( int i = 0; i < polytope->faceCount; i++ ) {
  486.         if( EPA_IsFaceSeenFromPoint( polytope, polytope->faces[i].a, polytope->faces[i].b, polytope->faces[i].c, point )) {
  487.             return i;
  488.         }
  489.     }
  490.     return -1;
  491. }
  492.  
  493. TEPATriangle EPA_GetClosestTriangle( TEPAPolytope * polytope ) {
  494.     int closest;
  495.     float closestDistance = FLT_MAX;
  496.     TVec3 closestNormal;
  497.     for( int i = 0; i < polytope->faceCount; i++ ) {
  498.         TVec3 a = polytope->vertices[ polytope->faces[ i ].a ].minkowskiDifPoint;
  499.         TVec3 b = polytope->vertices[ polytope->faces[ i ].b ].minkowskiDifPoint;
  500.         TVec3 c = polytope->vertices[ polytope->faces[ i ].c ].minkowskiDifPoint;
  501.  
  502.         TVec3 normal = Vec3_Cross( Vec3_Sub( b, a ), Vec3_Sub( c, a ) );
  503.  
  504.         float d = EPA_DistanceToOrigin( normal, a );
  505.         if( d < closestDistance ) {
  506.             closestDistance = d;
  507.             closest = i;
  508.             closestNormal = normal;
  509.         }        
  510.     }
  511.     return (TEPATriangle) { .a = polytope->vertices[ polytope->faces[ closest ].a ],
  512.                             .b = polytope->vertices[ polytope->faces[ closest ].b ],
  513.                             .c = polytope->vertices[ polytope->faces[ closest ].c ],
  514.                             .normal = closestNormal,
  515.                             .dist = closestDistance,
  516.                             .numInPolytope = closest };
  517. }
  518.  
  519. bool EPA_ComputeContacts( TEPAPolytope * polytope, TShape * shape1, TShape * shape2, TEPAContact * outContact ) {
  520.     const int convergenceLimit = 25;
  521.     for( int i = 0; i < convergenceLimit; i++ ) {        
  522.         TEPATriangle closestTriangle = EPA_GetClosestTriangle( polytope );
  523.         TSupport p = GJK_GetSupport( shape1, shape2, closestTriangle.normal );
  524.         float d = Vec3_Dot( p.minkowskiDifPoint, closestTriangle.normal );
  525.         if( d - closestTriangle.dist < 0.001 ) {            
  526.             TVec3 proj = EPA_ProjectOriginOntoPlane( closestTriangle.a.minkowskiDifPoint, closestTriangle.normal );
  527.             float u, v, w;
  528.             EPA_GetBarycentricCoords( proj, closestTriangle.a.minkowskiDifPoint, closestTriangle.b.minkowskiDifPoint, closestTriangle.c.minkowskiDifPoint, &u, &v, &w );
  529.             TVec3 a = Vec3_Scale( closestTriangle.a.supportA, u );
  530.             TVec3 b = Vec3_Scale( closestTriangle.b.supportA, v );
  531.             TVec3 c = Vec3_Scale( closestTriangle.c.supportA, w );
  532.             TVec3 collPoint = Vec3_Add( Vec3_Add( a, b ), c );
  533.             closestTriangle.normal = Vec3_Normalize( Vec3_Negate( closestTriangle.normal ));
  534.             outContact->normal = closestTriangle.normal;
  535.             outContact->penetrationDepth = d;
  536.             printf( "EPA: Done in %d iteration(s)!\nClosest: %.3f, %.3f, %.3f\nNormal: %.3f, %.3f, %.3f\nPenetration depth: %f", i, collPoint.x, collPoint.y, collPoint.z, closestTriangle.normal.x, closestTriangle.normal.y, closestTriangle.normal.z, d );
  537.             return true;
  538.         } else {                
  539.             TEdgeList edgeList;            
  540.             while( true ) {
  541.                 int seenFace = EPA_GetFirstFaceSeenFromPoint( polytope, p.minkowskiDifPoint );
  542.                 if( seenFace < 0 ) {
  543.                     break;
  544.                 } else {
  545.                     Polytope_RemoveFace( polytope, seenFace );
  546.                 }
  547.             }        
  548.             EPA_CreateEdgeList( &edgeList, polytope );    
  549.             EPA_FillEdgeList( &edgeList, polytope );
  550.             EPA_MarkHoles( &edgeList );
  551.             EPA_FillHoles( polytope, &edgeList, Polytope_AddVertex( polytope, p ) );
  552.             EPA_FreeEdgeList( &edgeList );
  553.         }
  554.     }
  555.     printf( "EPA: Convergence limit has reached!\n" );
  556.     return false;
  557. }
  558.  
  559. // ===========================
  560. // RENDERING ROUTINE
  561. // ===========================
  562. #include <windows.h>
  563. #include "glut.h"
  564. #include "gl/gl.h"
  565.  
  566. #define WINDOW_SIZE 800
  567.  
  568. void Renderer_DrawShape( TShape * shape ) {
  569.     glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
  570.    
  571.     if( shape->convex ) {
  572.         if( shape->convex->count == 4 ) { // tetrahedron        
  573.             glPushMatrix();
  574.            
  575.             glTranslatef( shape->convex->position.x, shape->convex->position.y, shape->convex->position.z );
  576.            
  577.             TVec3 a = shape->convex->points[0];
  578.             TVec3 b = shape->convex->points[1];
  579.             TVec3 c = shape->convex->points[2];
  580.             TVec3 d = shape->convex->points[3];
  581.            
  582.             glBegin(GL_TRIANGLES);
  583.             glColor3ub( 255, 0, 0 );
  584.             glVertex3f( a.x, a.y, a.z );
  585.             glVertex3f( b.x, b.y, b.z );
  586.             glVertex3f( c.x, c.y, c.z );
  587.            
  588.             glColor3ub( 0, 255, 0 );
  589.             glVertex3f( a.x, a.y, a.z );
  590.             glVertex3f( d.x, d.y, d.z );
  591.             glVertex3f( b.x, b.y, b.z );
  592.            
  593.             glColor3ub( 0, 0, 255 );
  594.             glVertex3f( a.x, a.y, a.z );
  595.             glVertex3f( c.x, c.y, c.z );
  596.             glVertex3f( d.x, d.y, d.z );            
  597.            
  598.             glColor3ub( 255, 255, 0 );
  599.             glVertex3f( c.x, c.y, c.z );
  600.             glVertex3f( b.x, b.y, b.z );
  601.             glVertex3f( d.x, d.y, d.z );
  602.            
  603.             glEnd();
  604.            
  605.             glPopMatrix();
  606.         }
  607.     } else {
  608.         glPushMatrix();
  609.         glColor3ub( 255, 0, 0 );
  610.         glTranslatef( shape->sphere->position.x, shape->sphere->position.y, shape->sphere->position.z );        
  611.         glutSolidSphere( shape->sphere->radius, 10, 10 );        
  612.         glPopMatrix();
  613.     }
  614. }
  615.  
  616. void Renderer_Display() {
  617.     static float a = 0;
  618.     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  619.  
  620.     glMatrixMode(GL_MODELVIEW);
  621.     glLoadIdentity();
  622.     gluLookAt( 0, 0, 3, 0, 0, 0, 0, 1, 0 );
  623.     glRotatef( a, 0, 1, 0 );
  624.     a+=0.5;
  625.    
  626.     //glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
  627.     for( int i = 0; i < currentPolytope->faceCount; i++ ) {
  628.         int ia = currentPolytope->faces[i].a;
  629.         int ib = currentPolytope->faces[i].b;
  630.         int ic = currentPolytope->faces[i].c;
  631.  
  632.         TVec3 a = currentPolytope->vertices[ ia ].minkowskiDifPoint;
  633.         TVec3 b = currentPolytope->vertices[ ib ].minkowskiDifPoint;
  634.         TVec3 c = currentPolytope->vertices[ ic ].minkowskiDifPoint;
  635.        
  636.         TVec3 normal = Vec3_Normalize( Vec3_Cross( Vec3_Sub( b,a ), Vec3_Sub( c,a )));
  637.        
  638.         glBegin(GL_TRIANGLES);
  639.         glColor3ub( i * 8 + 50, i * 8 + 50, i * 8 + 50 );
  640.         glVertex3f( a.x, a.y, a.z );
  641.         glVertex3f( b.x, b.y, b.z );
  642.         glVertex3f( c.x, c.y, c.z );
  643.         glEnd();
  644.        
  645.         glBegin(GL_LINES);
  646.         glColor3ub( 255, 0, 255 );
  647.         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 );
  648.         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 );
  649.         glEnd();
  650.     }
  651.    
  652.     Renderer_DrawShape( currentShape1 );
  653.     Renderer_DrawShape( currentShape2 );
  654.    
  655.     glutSwapBuffers();
  656.     glutPostRedisplay();
  657. }
  658.  
  659. void Renderer_Init() {
  660.     glClearColor(0.000, 0.110, 0.392, 0.0);
  661.  
  662.     glMatrixMode(GL_PROJECTION);
  663.     glLoadIdentity();
  664.     gluPerspective( 80, 1, 0.01, 1024 );
  665.    
  666.  
  667.    
  668.     glClearDepth( 1.0f );
  669.     glEnable( GL_DEPTH_TEST );
  670.     glDepthFunc( GL_LEQUAL );
  671.     glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST );
  672.     glEnable( GL_TEXTURE_2D );
  673.     glShadeModel( GL_SMOOTH );
  674.    
  675.     glEnable( GL_CULL_FACE );
  676.    
  677.     glDisable( GL_STENCIL_TEST );
  678.     glCullFace( GL_BACK );    
  679.     glEnable( GL_ALPHA_TEST );
  680.     glAlphaFunc( GL_GREATER, 0.025f );
  681.  
  682.    
  683.     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 ));
  684.     currentShape2 = ConvexShape_CreateTetrahedron( Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ), Vec3_Set( 0, 0, 1 ));
  685.     //currentShape1 = ConvexShape_CreateSphere( Vec3_Set( 0,0,0 ), 0.5 );
  686.     //currentShape2 = ConvexShape_CreateSphere( Vec3_Set( 0.5,0,0.0), 0.55 );
  687.     //currentShape1 = ConvexShape_CreateTriangle( Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ));
  688.     //currentShape2 = ConvexShape_CreateTriangle( Vec3_Set( 0, 0.5, 0 ), Vec3_Set( 0, 1.5, 1 ), Vec3_Set( 1, 0.5, 1 ));
  689.     TSimplex finalSimplex;
  690.     while( GJK_IsIntersects( currentShape1, currentShape2, &finalSimplex )) {
  691.         if( currentPolytope ) {
  692.             free( currentPolytope );
  693.         }
  694.         currentPolytope = calloc( 1, sizeof( TEPAPolytope ));
  695.         Polytope_SetFromSimplex( currentPolytope, &finalSimplex );
  696.         TEPAContact contact;
  697.         if( EPA_ComputeContacts( currentPolytope, currentShape1, currentShape2, &contact )) {
  698.             //currentShape1->sphere->position = Vec3_Add( currentShape1->sphere->position, Vec3_Scale( contact.normal, contact.penetrationDepth ));
  699.             currentShape1->convex->position = Vec3_Add( currentShape1->convex->position, Vec3_Scale( contact.normal, contact.penetrationDepth ));
  700.         }        
  701.     }
  702. }
  703.  
  704. // ===========================
  705. // TESTS
  706. // ===========================
  707.  
  708.  
  709. int main(int argc, char **argv) {
  710.     glutInit(&argc, argv);
  711.     glutInitDisplayMode( GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA );
  712.     glutInitWindowSize( 800, 800 );
  713.     glutInitWindowPosition(0, 0);
  714.     glutCreateWindow("Test");
  715.     glutDisplayFunc(Renderer_Display);
  716.     Renderer_Init();
  717.  
  718.     glutMainLoop();
  719.     /*
  720.     {        
  721.         printf( "Triangle-Triangle Intersection Test - " );
  722.         TShape * shape1 = ConvexShape_CreateTriangle( Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ));
  723.         TShape * shape2 = ConvexShape_CreateTriangle( Vec3_Set( 0, 0.5, 0 ), Vec3_Set( 0, 1.5, 1 ), Vec3_Set( 1, 0.5, 1 ));
  724.         TSimplex finalSimplex;
  725.         GJK_IsIntersects( shape1, shape2, &finalSimplex );
  726.         EPA_ComputeContacts( &finalSimplex, shape1, shape2 );
  727.         ConvexShape_Delete( shape1 );
  728.         ConvexShape_Delete( shape2 );
  729.     }
  730.     {
  731.         printf( "Triangle-Tetrahedron Intersection Test - " );
  732.         TShape * shape1 = ConvexShape_CreateTriangle( Vec3_Set( 0, 0, 0.5 ), Vec3_Set( 0, 1, 0.5 ), Vec3_Set( 1, 0, 0.5 ));
  733.         TShape * shape2 = ConvexShape_CreateTetrahedron( Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ), Vec3_Set( 0, 0, 1 ));
  734.         TSimplex finalSimplex;
  735.         GJK_IsIntersects( shape1, shape2, &finalSimplex );
  736.         ConvexShape_Delete( shape1 );
  737.         ConvexShape_Delete( shape2 );
  738.     }
  739.     {        
  740.         printf( "Sphere-Tetrahedron Intersection Test - " );
  741.         TShape * shape1 = ConvexShape_CreateSphere( Vec3_Set( 0,0,0 ), 1 );
  742.         TShape * shape2 = ConvexShape_CreateTetrahedron( Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ), Vec3_Set( 0, 0, 1 ));
  743.         TSimplex finalSimplex;
  744.         GJK_IsIntersects( shape1, shape2, &finalSimplex );
  745.         ConvexShape_Delete( shape1 );
  746.         ConvexShape_Delete( shape2 );          
  747.     }
  748.     {
  749.         printf( "Tetrahedron-Tetrahedron Intersection Test - " );
  750.         TShape * shape1 = ConvexShape_CreateTetrahedron( Vec3_Set( 0, 0, 0.5 ), Vec3_Set( 0, 1, 0.5 ), Vec3_Set( 1, 0, 0.5 ), Vec3_Set( 0, 0, 1.5 ));
  751.         TShape * shape2 = ConvexShape_CreateTetrahedron( Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ), Vec3_Set( 0, 0, 1 ));
  752.         TSimplex finalSimplex;
  753.         GJK_IsIntersects( shape1, shape2, &finalSimplex );
  754.         ConvexShape_Delete( shape1 );
  755.         ConvexShape_Delete( shape2 );
  756.     }
  757.     {
  758.         printf( "Sphere-Sphere Intersection Test - " );
  759.         TShape * shape1 = ConvexShape_CreateSphere( Vec3_Set( 1,0,0 ), 1 );
  760.         TShape * shape2 = ConvexShape_CreateSphere( Vec3_Set( 0,0,0 ), 1 );
  761.         TSimplex finalSimplex;
  762.         GJK_IsIntersects( shape1, shape2, &finalSimplex );
  763.         ConvexShape_Delete( shape1 );
  764.         ConvexShape_Delete( shape2 );
  765.     }  */        
  766. }
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!