API tools faq
paste
mrDIMAS

GJK EPA

mrDIMAS
Oct 4th, 2015
369
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 21.88 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.  
  6. #include "vector3.h"
  7.  
  8.  
  9.  
  10. typedef struct TSimplex {
  11.     TVec3 points[4];
  12.     int size;
  13. } TSimplex;
  14.  
  15. typedef struct TSphereShape {
  16.     float radius;
  17.     TVec3 position;
  18. } TSphereShape;
  19.  
  20. typedef struct TConvexShape {
  21.     TVec3 * points;
  22.     int count;
  23. } TConvexShape;
  24.  
  25. typedef struct TShape {
  26.     TConvexShape * convex;
  27.     TSphereShape * sphere;
  28. } TShape;
  29.  
  30. typedef struct TEPATriangle {
  31.     TVec3 a, b, c, normal;
  32.     float dist;
  33.     int numInPolytope;
  34. } TEPATriangle;
  35.  
  36. typedef struct TEPAContact {
  37.     TVec3 normal;
  38.     float penetrationDepth;
  39. } TEPAContact;
  40.  
  41. typedef struct TEPAFace {
  42.     int a, b, c;
  43. } TEPAFace;
  44.  
  45. typedef struct TEPAPolytope {
  46.     TVec3 * vertices;
  47.     int vertexCount;
  48.     TEPAFace * faces;
  49.     int faceCount;
  50. } TEPAPolytope;
  51.  
  52. #ifndef M_PI
  53. #define M_PI 3.14159
  54. #endif
  55.  
  56. void DrawPolytope( TEPAPolytope * polytope );
  57.  
  58. TEPAPolytope * currentPolytope;
  59. TShape * currentShape1;
  60. TShape * currentShape2;
  61. // ===========================
  62. // HELPERS
  63. // ===========================
  64. bool Helper_SameDirection( TVec3 a, TVec3 b ) {
  65.     return Vec3_Dot( a, b ) > 0;
  66. }
  67.  
  68. // ===========================
  69. // SIMPLEX ROUTINE
  70. // ===========================
  71. void Simplex_RemovePoint( TSimplex * s, int num ) {
  72.     if( s->size > 0 ) {
  73.         if( num == 0 ) {
  74.             s->points[0] = s->points[1];
  75.             s->points[1] = s->points[2];
  76.             s->points[2] = s->points[3];
  77.         }
  78.         if( num == 1 ) {
  79.             s->points[1] = s->points[2];
  80.             s->points[2] = s->points[3];
  81.         }
  82.         if( num == 2 ) {
  83.             s->points[2] = s->points[3];
  84.         }
  85.         s->size--;
  86.     }
  87. }
  88.  
  89. void Simplex_AddPoint( TSimplex * s, TVec3 p ) {
  90.     if( s->size < 4 ) {
  91.         s->points[ s->size ] = p;
  92.         s->size++;
  93.     }
  94. }
  95.  
  96. // ===========================
  97. // CONVEX SHAPE ROUTINE
  98. // ===========================
  99. TShape * ConvexShape_CreateTriangle( TVec3 a, TVec3 b, TVec3 c ) {
  100.     TShape * shape = calloc( 1, sizeof( TShape ));
  101.     shape->convex = calloc( 1, sizeof( TConvexShape ));
  102.     shape->convex->count = 3;
  103.     shape->convex->points = malloc( shape->convex->count * sizeof( TVec3 ));
  104.     shape->convex->points[0] = a;
  105.     shape->convex->points[1] = b;
  106.     shape->convex->points[2] = c;
  107.     return shape;
  108. }
  109.  
  110. TShape * ConvexShape_CreateTetrahedron( TVec3 a, TVec3 b, TVec3 c, TVec3 d ) {
  111.     TShape * shape = calloc( 1, sizeof( TShape ));
  112.     shape->convex = calloc( 1, sizeof( TConvexShape ));
  113.     shape->convex->count = 4;
  114.     shape->convex->points = malloc( shape->convex->count * sizeof( TVec3 ));
  115.     shape->convex->points[0] = a;
  116.     shape->convex->points[1] = b;
  117.     shape->convex->points[2] = c;
  118.     shape->convex->points[3] = d;
  119.     return shape;
  120. }
  121.  
  122. TShape * ConvexShape_CreateSphere( TVec3 position, float radius ) {
  123.     TShape * shape = calloc( 1, sizeof( TShape ));
  124.     shape->sphere = calloc( 1, sizeof( TSphereShape ));    
  125.     shape->sphere->position = position;
  126.     shape->sphere->radius = radius;
  127.     return shape;
  128. }
  129.  
  130. void ConvexShape_Delete( TShape * shape ) {
  131.     if( shape->convex ) {
  132.         free( shape->convex );
  133.     }
  134.     if( shape->sphere ) {
  135.         free( shape->sphere );
  136.     }
  137.     free( shape );
  138. }
  139.  
  140. TVec3 ConvexShape_GetFarthestPointInDirection( TShape * shape, TVec3 dir ) {
  141.     const float eps = 0.000001;
  142.     if( fabs( dir.x ) < eps && fabs( dir.y ) < eps && fabs( dir.z ) < eps ) {
  143.         printf( "GJK Warning: Zero direction passed!\n" );
  144.     }
  145.        
  146.     if( shape->convex ) {
  147.         TVec3 farthest;
  148.         float lastDot = -FLT_MAX;
  149.         for( int i = 0; i < shape->convex->count; i++ ) {
  150.             float dot = Vec3_Dot( dir, shape->convex->points[i] );
  151.             if( dot > lastDot ) {
  152.                 farthest = shape->convex->points[i];
  153.                 lastDot = dot;
  154.             }
  155.         }
  156.         return farthest;
  157.     }
  158.     if( shape->sphere ) {
  159.         return Vec3_Add( shape->sphere->position, Vec3_Scale( Vec3_Normalize( dir ), shape->sphere->radius ));
  160.     }
  161.     return Vec3_Set( 0, 0, 0 );
  162. }
  163.  
  164. // ===========================
  165. // GJK ALGORITHM ROUTINE
  166. // ===========================
  167. TVec3 GJK_GetSupport( TShape * shape1, TShape * shape2, TVec3 dir ) {
  168.     return Vec3_Sub( ConvexShape_GetFarthestPointInDirection( shape1, dir ), ConvexShape_GetFarthestPointInDirection( shape2, Vec3_Negate( dir )));
  169. }
  170.  
  171. bool GJK_ProcessLine( TSimplex * simplex, TVec3 * outDirection ) {
  172.     TVec3 a = simplex->points[1];
  173.     TVec3 b = simplex->points[0];
  174.     TVec3 ab = Vec3_Sub( b, a );
  175.     TVec3 aO = Vec3_Negate( a );
  176.    
  177.     if( Helper_SameDirection( ab, aO )) {
  178.         *outDirection = Vec3_Cross( Vec3_Cross( ab, aO ), ab );
  179.     } else {
  180.         Simplex_RemovePoint( simplex, 0 );
  181.         *outDirection = aO;
  182.     }
  183.     return false;
  184. }
  185.  
  186. bool GJK_ProcessTriangle( TSimplex * simplex, TVec3 * outDirection ) {
  187.     TVec3 a = simplex->points[2];
  188.     TVec3 b = simplex->points[1];
  189.     TVec3 c = simplex->points[0];
  190.     TVec3 aO = Vec3_Negate( a );
  191.     TVec3 ab = Vec3_Sub( b, a );
  192.     TVec3 ac = Vec3_Sub( c, a );
  193.     TVec3 abc = Vec3_Cross( ab, ac );
  194.     TVec3 acNormal = Vec3_Cross( abc, ac );
  195.     TVec3 abNormal = Vec3_Cross( ab, abc );
  196.    
  197.     if( Helper_SameDirection( acNormal, aO )) {
  198.         if( Helper_SameDirection( ac, aO )) {
  199.             Simplex_RemovePoint( simplex, 1 );
  200.             *outDirection = Vec3_Cross( Vec3_Cross(ac, aO), ac );
  201.         } else {
  202.             if( Helper_SameDirection( ab, aO )) {
  203.                 Simplex_RemovePoint( simplex, 0 );
  204.                 *outDirection = Vec3_Cross( Vec3_Cross(ab, aO), ab);
  205.             } else {
  206.                 Simplex_RemovePoint( simplex, 1 );
  207.                 Simplex_RemovePoint( simplex, 0 );
  208.                 *outDirection = aO;
  209.             }
  210.         }
  211.     } else {
  212.         if( Helper_SameDirection( abNormal, aO )) {
  213.             if( Helper_SameDirection( ab, aO )) {
  214.                 Simplex_RemovePoint( simplex, 0 );
  215.                 *outDirection = Vec3_Cross( Vec3_Cross(ab, aO), ab);
  216.             } else {
  217.                 Simplex_RemovePoint( simplex, 1 );
  218.                 Simplex_RemovePoint( simplex, 0 );
  219.                 *outDirection = aO;
  220.             }
  221.         } else {
  222.             if( Helper_SameDirection( abc, aO )) {
  223.                 *outDirection = Vec3_Cross(Vec3_Cross(abc, aO), abc);
  224.             } else {
  225.                 *outDirection = Vec3_Cross(Vec3_Cross( Vec3_Negate( abc ), aO), Vec3_Negate( abc ) );
  226.             }
  227.         }
  228.     }
  229.    
  230.     return false;
  231. }
  232.  
  233. bool GJK_ProcessTetrahedron( TSimplex * simplex, TVec3 * outDirection ) {
  234.     TVec3 a = simplex->points[3];
  235.     TVec3 b = simplex->points[2];
  236.     TVec3 c = simplex->points[1];
  237.     TVec3 d = simplex->points[0];
  238.    
  239.     TVec3 ac = Vec3_Sub( c, a );
  240.     TVec3 ab = Vec3_Sub( b, a );
  241.     TVec3 ad = Vec3_Sub( d, a );
  242.    
  243.     TVec3 acd = Vec3_Cross( ad, ac );
  244.     TVec3 abd = Vec3_Cross( ab, ad );
  245.     TVec3 abc = Vec3_Cross( ac, ab );
  246.    
  247.     TVec3 aO = Vec3_Negate( a );
  248.    
  249.     if( Helper_SameDirection( abc, aO )) {
  250.         if( Helper_SameDirection( Vec3_Cross( abc, ac ), aO )) {
  251.             Simplex_RemovePoint( simplex, 2 );
  252.             Simplex_RemovePoint( simplex, 0 );
  253.             *outDirection = Vec3_Cross( Vec3_Cross( ac, aO ), ac );
  254.         } else if( Helper_SameDirection( Vec3_Cross( ab, abc ), aO )) {
  255.             Simplex_RemovePoint( simplex, 1 );
  256.             Simplex_RemovePoint( simplex, 0 );
  257.             *outDirection = Vec3_Cross( Vec3_Cross( ab, aO ), ab );
  258.         } else {
  259.             Simplex_RemovePoint( simplex, 0 );
  260.             *outDirection = abc;
  261.         }
  262.     } else if( Helper_SameDirection( acd, aO )) {
  263.         if( Helper_SameDirection( Vec3_Cross( acd, ad ), aO )) {
  264.             Simplex_RemovePoint( simplex, 2 );
  265.             Simplex_RemovePoint( simplex, 1 );
  266.             *outDirection = Vec3_Cross( Vec3_Cross( ad, aO ), ad );
  267.         } else if ( Helper_SameDirection( Vec3_Cross( ac, acd ), aO )) {
  268.             Simplex_RemovePoint( simplex, 2 );
  269.             Simplex_RemovePoint( simplex, 0 );
  270.             *outDirection = Vec3_Cross( Vec3_Cross( ac, aO ), ac );
  271.         } else {
  272.            Simplex_RemovePoint( simplex, 2 );
  273.             *outDirection = acd;
  274.         }
  275.     } else if( Helper_SameDirection( abd, aO )) {
  276.         if( Helper_SameDirection( Vec3_Cross( abd, ab ), aO )) {
  277.             Simplex_RemovePoint( simplex, 1 );
  278.             Simplex_RemovePoint( simplex, 0 );
  279.             *outDirection = Vec3_Cross( Vec3_Cross( ab, aO ), ab );
  280.         } else if( Helper_SameDirection( Vec3_Cross( ad, abd ), aO )) {
  281.             Simplex_RemovePoint( simplex, 2 );
  282.             Simplex_RemovePoint( simplex, 1 );
  283.             *outDirection = Vec3_Cross( Vec3_Cross( ad, aO ), ad );
  284.         } else {
  285.             Simplex_RemovePoint( simplex, 1 );
  286.             *outDirection = abd;
  287.         }
  288.     } else {
  289.         return true;
  290.     }
  291.    
  292.     return false;
  293. }
  294.  
  295. bool GJK_ProcessSimplex( TSimplex * simplex, TVec3 * outDirection ) {
  296.     if( simplex->size == 2 ) {
  297.         return GJK_ProcessLine( simplex, outDirection );
  298.     } else if ( simplex->size == 3 ) {
  299.         return GJK_ProcessTriangle( simplex, outDirection );
  300.     } else {
  301.         return GJK_ProcessTetrahedron( simplex, outDirection );
  302.     }
  303. }
  304.  
  305. bool GJK_IsIntersects( TShape * shape1, TShape * shape2, TSimplex * finalSimplex ) {
  306.     TVec3 dir = Vec3_Set( 1, 1, 1 );
  307.    
  308.     TSimplex simplex = { 0 };
  309.     Simplex_AddPoint( &simplex, GJK_GetSupport( shape1, shape2, dir ));
  310.    
  311.     dir = Vec3_Negate( dir );
  312.    
  313.     int convergenceLimit = 50;
  314.     for( int i = 0; i < convergenceLimit; i++ ) {
  315.         TVec3 lastSupport = GJK_GetSupport( shape1, shape2, dir );              
  316.         if( Helper_SameDirection( dir, lastSupport )) {
  317.             Simplex_AddPoint( &simplex, lastSupport );            
  318.             if( GJK_ProcessSimplex( &simplex, &dir )) {
  319.                 printf( "GJK: Intersection! %d iteration(s)!\n", i );
  320.                 if( finalSimplex ) {
  321.                     *finalSimplex = simplex;
  322.                 }
  323.                 return true;
  324.             }            
  325.         } else {
  326.             printf( "GJK: No intersection! %d iteration(s)!\n", i );            
  327.             return false;
  328.         }
  329.     }
  330.    
  331.     printf( "GJK: No intersection! Convergence limit has reached!\n" );
  332.     return false;
  333. }
  334.  
  335. // ===========================
  336. // EPA ALGORITHM ROUTINE
  337. // ===========================
  338. void Polytope_SetFromSimplex( TEPAPolytope * polytope, TSimplex * simplex ) {
  339.     polytope->vertexCount = 4;
  340.     polytope->vertices = calloc( polytope->vertexCount, sizeof( TVec3 ));
  341.     for( int i = 0; i < polytope->vertexCount; i++ ) {
  342.         polytope->vertices[i] = simplex->points[i];
  343.     }
  344.     polytope->faceCount = 4;
  345.     polytope->faces = calloc( polytope->faceCount, sizeof( TEPAFace ));
  346.     polytope->faces[0] = (TEPAFace) { 0, 1, 2 };
  347.     polytope->faces[1] = (TEPAFace) { 0, 3, 1 };
  348.     polytope->faces[2] = (TEPAFace) { 0, 2, 3 };
  349.     polytope->faces[3] = (TEPAFace) { 2, 1, 3 };
  350. }
  351.  
  352. void Polytope_RemoveFaceAndPatchHole( TEPAPolytope * polytope, int n, TVec3 patchPoint ) {
  353.     int a = polytope->faces[n].a;
  354.     int b = polytope->faces[n].b;
  355.     int c = polytope->faces[n].c;
  356.    
  357.     // add patch point to point list
  358.     bool alreadyExist = false;
  359.     int patchPointIndex;
  360.     for( int i = 0; i < polytope->vertexCount; i++ ) {
  361.         if( polytope->vertices[i].x == patchPoint.x && polytope->vertices[i].y == patchPoint.y && polytope->vertices[i].z == patchPoint.z ) {
  362.             alreadyExist = true;
  363.             patchPointIndex = i;
  364.             break;
  365.         }
  366.     }
  367.    
  368.     if( !alreadyExist ) {
  369.         polytope->vertexCount++;
  370.         polytope->vertices = realloc( polytope->vertices, polytope->vertexCount * sizeof( TVec3 ));
  371.         patchPointIndex = polytope->vertexCount - 1;
  372.         polytope->vertices[ patchPointIndex ] = patchPoint;
  373.     }
  374.    
  375.     // build new faces
  376.     int lastFreeIndex = polytope->faceCount;
  377.     polytope->faceCount += 2;
  378.     polytope->faces = realloc( polytope->faces, polytope->faceCount * sizeof( TEPAFace ));
  379.  
  380.     polytope->faces[n] = (TEPAFace) { a, b, patchPointIndex };
  381.     polytope->faces[lastFreeIndex] = (TEPAFace) { patchPointIndex, c, a };
  382.     polytope->faces[lastFreeIndex+1] = (TEPAFace) { patchPointIndex, b, c };
  383. }
  384.  
  385. void Polytope_RemoveNonconvexity( TEPAPolytope * polytope, TVec3 lastPoint ) {    
  386.     while( true ) {
  387.         int seenNum = -1;
  388.         bool foundSeen = false;
  389.         for( int i = 0; i < polytope->faceCount; i++ ) {
  390.             TVec3 a = polytope->vertices[ polytope->faces[ i ].a ];
  391.             TVec3 b = polytope->vertices[ polytope->faces[ i ].b ];
  392.             TVec3 c = polytope->vertices[ polytope->faces[ i ].c ];
  393.             TVec3 normal = Vec3_Cross( Vec3_Sub( b, a ), Vec3_Sub( c, a ));
  394.             if( Vec3_Dot( normal, Vec3_Sub( lastPoint, a )) > 0.0001 ) {
  395.                 seenNum = i;
  396.                 foundSeen = true;
  397.             }
  398.         }        
  399.         if( !foundSeen ) {
  400.             break;
  401.         } else {
  402.             Polytope_RemoveFaceAndPatchHole( polytope, seenNum, lastPoint );
  403.         }
  404.     }
  405. }
  406.  
  407. TVec3 EPA_ProjectOriginOntoPlane( TVec3 planePoint, TVec3 planeNormal ) {
  408.     float t = -Vec3_Dot( planePoint, planeNormal );
  409.     return Vec3_Negate( Vec3_Scale( planeNormal, t ));
  410. }
  411.  
  412. float EPA_DistanceToOrigin( TVec3 normal, TVec3 point ) {
  413.     return fabs( Vec3_Dot( normal, Vec3_Negate( point )));
  414. }
  415.  
  416. TEPATriangle EPA_GetClosestTriangle( TEPAPolytope * polytope ) {
  417.     TEPATriangle closest;
  418.     float closestDistance = FLT_MAX;
  419.     for( int i = 0; i < polytope->faceCount; i++ ) {
  420.         TVec3 a = polytope->vertices[ polytope->faces[ i ].a ];
  421.         TVec3 b = polytope->vertices[ polytope->faces[ i ].b ];
  422.         TVec3 c = polytope->vertices[ polytope->faces[ i ].c ];
  423.  
  424.         TVec3 normal = Vec3_Cross( Vec3_Sub( b, a ), Vec3_Sub( c, a ) );
  425.  
  426.         float d = EPA_DistanceToOrigin( normal, a );
  427.         if( d < closestDistance ) {
  428.             closestDistance = d;
  429.             closest = (TEPATriangle) { .a = a, .b = b, .c = c, .normal = normal, .dist = d, .numInPolytope = i };
  430.         }        
  431.     }
  432.     return closest;
  433. }
  434.  
  435. bool EPA_ComputeContacts( TEPAPolytope * polytope, TShape * shape1, TShape * shape2 ) {
  436.     const int convergenceLimit = 100;
  437.     TEPATriangle lastClosest = { 0 };
  438.     lastClosest.dist = FLT_MAX;
  439.     for( int i = 0; i < convergenceLimit; i++ ) {        
  440.         TEPATriangle closestTriangle = EPA_GetClosestTriangle( polytope );
  441.         if( closestTriangle.dist > lastClosest.dist ) {            
  442.             TVec3 proj = EPA_ProjectOriginOntoPlane( lastClosest.a, lastClosest.normal );
  443.             printf( "EPA: Done in %d iteration(s)!\nClosest: %.3f, %.3f, %.3f\nPenetration depth: %.2f", i, proj.x, proj.y, proj.z, lastClosest.dist );
  444.             return true;
  445.         } else {
  446.             TVec3 p = GJK_GetSupport( shape1, shape2, closestTriangle.normal );
  447.             Polytope_RemoveFaceAndPatchHole( polytope, closestTriangle.numInPolytope, p );
  448.             Polytope_RemoveNonconvexity( polytope, p );
  449.         }
  450.         lastClosest = closestTriangle;
  451.     }
  452.     printf( "EPA: Convergence limit has reached!\n" );
  453.     return false;
  454. }
  455.  
  456. // ===========================
  457. // RENDERING ROUTINE
  458. // ===========================
  459. #include <windows.h>
  460. #include "glut.h"
  461. #include "gl/gl.h"
  462.  
  463. #define WINDOW_SIZE 800
  464.  
  465. void DrawShape( TShape * shape ) {
  466.     if( shape->convex ) {
  467.         if( shape->convex->count == 4 ) { // tetrahedron
  468.             TVec3 a = shape->convex->points[0];
  469.             TVec3 b = shape->convex->points[1];
  470.             TVec3 c = shape->convex->points[2];
  471.             TVec3 d = shape->convex->points[3];
  472.            
  473.             glBegin(GL_TRIANGLES);
  474.             glColor3ub( 255, 0, 0 );
  475.             glVertex3f( a.x, a.y, a.z );
  476.             glVertex3f( b.x, b.y, b.z );
  477.             glVertex3f( c.x, c.y, c.z );
  478.            
  479.             glColor3ub( 0, 255, 0 );
  480.             glVertex3f( a.x, a.y, a.z );
  481.             glVertex3f( d.x, d.y, d.z );
  482.             glVertex3f( b.x, b.y, b.z );
  483.            
  484.             glColor3ub( 0, 0, 255 );
  485.             glVertex3f( a.x, a.y, a.z );
  486.             glVertex3f( c.x, c.y, c.z );
  487.             glVertex3f( d.x, d.y, d.z );            
  488.            
  489.             glColor3ub( 255, 255, 0 );
  490.             glVertex3f( c.x, c.y, c.z );
  491.             glVertex3f( b.x, b.y, b.z );
  492.             glVertex3f( d.x, d.y, d.z );
  493.            
  494.             glEnd();
  495.         }
  496.     } else {
  497.         glPushMatrix();
  498.         glColor3ub( 255, 0, 0 );
  499.         glTranslatef( shape->sphere->position.x, shape->sphere->position.y, shape->sphere->position.z );
  500.         glutSolidSphere( shape->sphere->radius, 10, 10 );
  501.         glPopMatrix();
  502.     }
  503. }
  504.  
  505. void display() {
  506.     static float a = 0;
  507.     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  508.  
  509.     glMatrixMode(GL_MODELVIEW);
  510.     glLoadIdentity();
  511.     gluLookAt( 0, 0, 3, 0, 0, 0, 0, 1, 0 );
  512.     glRotatef( a, 0, 1, 0 );
  513.     a+=0.5;
  514.    
  515.    
  516.     for( int i = 0; i < currentPolytope->faceCount; i++ ) {
  517.         int ia = currentPolytope->faces[i].a;
  518.         int ib = currentPolytope->faces[i].b;
  519.         int ic = currentPolytope->faces[i].c;
  520.  
  521.         TVec3 a = currentPolytope->vertices[ ia ];
  522.         TVec3 b = currentPolytope->vertices[ ib ];
  523.         TVec3 c = currentPolytope->vertices[ ic ];
  524.        
  525.         glBegin(GL_TRIANGLES);
  526.         glColor3ub( i * 8 + 50, i * 8 + 50, i * 8 + 50 );
  527.         glVertex3f( a.x, a.y, a.z );
  528.         glVertex3f( b.x, b.y, b.z );
  529.         glVertex3f( c.x, c.y, c.z );
  530.         glEnd();
  531.     }
  532.    
  533.     DrawShape( currentShape1 );
  534.     DrawShape( currentShape2 );
  535.    
  536.     glutSwapBuffers();
  537.     glutPostRedisplay();
  538. }
  539.  
  540. void init() {
  541.     glClearColor(0.000, 0.110, 0.392, 0.0);
  542.  
  543.     glMatrixMode(GL_PROJECTION);
  544.     glLoadIdentity();
  545.     gluPerspective( 80, 1, 0.01, 1024 );
  546.    
  547.  
  548.    
  549.     glClearDepth( 1.0f );
  550.     glEnable( GL_DEPTH_TEST );
  551.     glDepthFunc( GL_LEQUAL );
  552.     glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST );
  553.     glEnable( GL_TEXTURE_2D );
  554.     glShadeModel( GL_SMOOTH );
  555.    
  556.     glEnable( GL_CULL_FACE );
  557.    
  558.     glDisable( GL_STENCIL_TEST );
  559.     glCullFace( GL_BACK );    
  560.     glEnable( GL_ALPHA_TEST );
  561.     glAlphaFunc( GL_GREATER, 0.025f );
  562.  
  563.    
  564.     //currentShape1 = ConvexShape_CreateTetrahedron( Vec3_Set( -1, .5, 0 ), Vec3_Set( -1, 1.5, 0 ), Vec3_Set( 0.2, 0.5, 0 ), Vec3_Set( -1, 0.5, 1 ));
  565.     //currentShape2 = ConvexShape_CreateTetrahedron( Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ), Vec3_Set( 0, 0, 1 ));
  566.     currentShape1 = ConvexShape_CreateSphere( Vec3_Set( 1,0,0 ), 0.6 );
  567.     currentShape2 = ConvexShape_CreateSphere( Vec3_Set( 0,0,0 ), 0.5 );
  568.     TSimplex finalSimplex;
  569.     GJK_IsIntersects( currentShape1, currentShape2, &finalSimplex );
  570.     currentPolytope = calloc( 1, sizeof( TEPAPolytope ));
  571.     Polytope_SetFromSimplex( currentPolytope, &finalSimplex );
  572.     EPA_ComputeContacts( currentPolytope, currentShape1, currentShape2 );
  573. }
  574.  
  575. // ===========================
  576. // TESTS
  577. // ===========================
  578.  
  579.  
  580. int main(int argc, char **argv) {
  581.     glutInit(&argc, argv);
  582.     glutInitDisplayMode( GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA );
  583.     glutInitWindowSize( 800, 800 );
  584.     glutInitWindowPosition(0, 0);
  585.     glutCreateWindow("Test");
  586.     glutDisplayFunc(display);
  587.     init();
  588.  
  589.     glutMainLoop();
  590.     /*
  591.     {        
  592.         printf( "Triangle-Triangle Intersection Test - " );
  593.         TShape * shape1 = ConvexShape_CreateTriangle( Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ));
  594.         TShape * shape2 = ConvexShape_CreateTriangle( Vec3_Set( 0, 0.5, 0 ), Vec3_Set( 0, 1.5, 1 ), Vec3_Set( 1, 0.5, 1 ));
  595.         TSimplex finalSimplex;
  596.         GJK_IsIntersects( shape1, shape2, &finalSimplex );
  597.         EPA_ComputeContacts( &finalSimplex, shape1, shape2 );
  598.         ConvexShape_Delete( shape1 );
  599.         ConvexShape_Delete( shape2 );
  600.     }
  601.     {
  602.         printf( "Triangle-Tetrahedron Intersection Test - " );
  603.         TShape * shape1 = ConvexShape_CreateTriangle( Vec3_Set( 0, 0, 0.5 ), Vec3_Set( 0, 1, 0.5 ), Vec3_Set( 1, 0, 0.5 ));
  604.         TShape * shape2 = ConvexShape_CreateTetrahedron( Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ), Vec3_Set( 0, 0, 1 ));
  605.         TSimplex finalSimplex;
  606.         GJK_IsIntersects( shape1, shape2, &finalSimplex );
  607.         ConvexShape_Delete( shape1 );
  608.         ConvexShape_Delete( shape2 );
  609.     }
  610.     {        
  611.         printf( "Sphere-Tetrahedron Intersection Test - " );
  612.         TShape * shape1 = ConvexShape_CreateSphere( Vec3_Set( 0,0,0 ), 1 );
  613.         TShape * shape2 = ConvexShape_CreateTetrahedron( Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ), Vec3_Set( 0, 0, 1 ));
  614.         TSimplex finalSimplex;
  615.         GJK_IsIntersects( shape1, shape2, &finalSimplex );
  616.         ConvexShape_Delete( shape1 );
  617.         ConvexShape_Delete( shape2 );          
  618.     }
  619.     {
  620.         printf( "Tetrahedron-Tetrahedron Intersection Test - " );
  621.         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 ));
  622.         TShape * shape2 = ConvexShape_CreateTetrahedron( Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ), Vec3_Set( 0, 0, 1 ));
  623.         TSimplex finalSimplex;
  624.         GJK_IsIntersects( shape1, shape2, &finalSimplex );
  625.         ConvexShape_Delete( shape1 );
  626.         ConvexShape_Delete( shape2 );
  627.     }
  628.     {
  629.         printf( "Sphere-Sphere Intersection Test - " );
  630.         TShape * shape1 = ConvexShape_CreateSphere( Vec3_Set( 1,0,0 ), 1 );
  631.         TShape * shape2 = ConvexShape_CreateSphere( Vec3_Set( 0,0,0 ), 1 );
  632.         TSimplex finalSimplex;
  633.         GJK_IsIntersects( shape1, shape2, &finalSimplex );
  634.         ConvexShape_Delete( shape1 );
  635.         ConvexShape_Delete( shape2 );
  636.     }  */        
  637. }
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!