API tools faq
paste
mrDIMAS

GJK EPA 2

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