API tools faq
paste
mrDIMAS

GJK EPA WORKING!

mrDIMAS
Oct 17th, 2015
192
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 41.67 KB | None | 0 0
raw download clone embed print report
  1. //////////////////////////////////////////
  2. //         GJK-EPA Algorithm impl.      //
  3. //            (C) mrDIMAS 2015          //
  4. //////////////////////////////////////////
  5. #include <stdio.h>
  6. #include <stdlib.h>
  7. #include <float.h>
  8. #include <stdbool.h>
  9. #include <string.h>
  10.  
  11. #include "vector3.h"
  12.  
  13. bool useDebugOutput = false;
  14.  
  15. typedef struct TSupport {
  16.     TVec3 supportA;
  17.     TVec3 minkowskiDifPoint;
  18. } TSupport;
  19.  
  20. typedef struct TSimplex {
  21.     TSupport points[4];
  22.     int size;
  23. } TSimplex;
  24.  
  25. typedef struct TEPAFace {
  26.     int a, b, c;
  27. } TEPAFace;
  28.  
  29. typedef struct TSphereShape {
  30.     float radius;
  31. } TSphereShape;
  32.  
  33. typedef struct TConvexShape {
  34.     TVec3 * points;
  35.     int count;    
  36. } TConvexShape;
  37.  
  38. typedef struct TTriangleShape {
  39.     TVec3 a, b, c;
  40. } TTriangleShape;
  41.  
  42. typedef struct TTriangleMeshShape {
  43.     TVec3 * vertices;
  44.     int vertexCount;
  45.     TEPAFace * faces;
  46.     int faceCount;
  47. } TTriangleMeshShape;
  48.  
  49. typedef struct TBoxShape {
  50.     float size;
  51.     TVec3 vertices[8];
  52. } TBoxShape;
  53.  
  54. typedef struct TShape {
  55.     TConvexShape * convex;
  56.     TSphereShape * sphere;
  57.     TTriangleMeshShape * triMesh;
  58.     TTriangleShape * triangle;
  59.     TBoxShape * box;
  60.     TVec3 position;
  61. } TShape;
  62.  
  63. typedef struct TEPATriangle {
  64.     TSupport a, b, c;
  65.     TVec3 normal;
  66.     float dist;
  67.     int numInPolytope;
  68. } TEPATriangle;
  69.  
  70. typedef struct TEPAContact {
  71.     TVec3 normal;
  72.     float penetrationDepth;
  73. } TEPAContact;
  74.  
  75. typedef struct TEPAPolytope {
  76.     TSupport * vertices;
  77.     int vertexCount;
  78.     TEPAFace * faces;
  79.     int faceCount;
  80. } TEPAPolytope;
  81.  
  82. typedef struct TEdge {
  83.     int a;
  84.     int b;
  85.     bool free;
  86. } TEdge;
  87.  
  88. typedef struct TEdgeList {
  89.     TEdge * edges;
  90.     int count;
  91.     int freeCount;
  92. } TEdgeList;
  93.  
  94. typedef struct TBody {
  95.     TVec3 position;
  96.     TShape * shape;
  97. } TBody;
  98.  
  99. typedef struct TMat3 {
  100.     float m[3][3];
  101. } TMat3;
  102.  
  103. #ifndef M_PI
  104. #   define M_PI 3.14159
  105. #endif
  106.  
  107.  
  108. TEPAPolytope * currentPolytope;
  109. TShape * currentShape1;
  110. TShape * currentShape2;
  111. TShape * currentShape3;
  112.  
  113. TBody * body1;
  114. TBody * body2;
  115. TBody * body3;
  116. // ===========================
  117. // HELPERS
  118. // ===========================
  119. bool Helper_SameDirection( TVec3 a, TVec3 b ) {
  120.     return Vec3_Dot( a, b ) > 0;
  121. }
  122.  
  123. // ===========================
  124. // SIMPLEX ROUTINE
  125. // ===========================
  126. void Simplex_RemovePoint( TSimplex * s, int num ) {
  127.     if( s->size > 0 ) {
  128.         if( num == 0 ) {
  129.             s->points[0] = s->points[1];
  130.             s->points[1] = s->points[2];
  131.             s->points[2] = s->points[3];
  132.         }
  133.         if( num == 1 ) {
  134.             s->points[1] = s->points[2];
  135.             s->points[2] = s->points[3];
  136.         }
  137.         if( num == 2 ) {
  138.             s->points[2] = s->points[3];
  139.         }
  140.         s->size--;
  141.     }
  142. }
  143.  
  144. void Simplex_AddPoint( TSimplex * s, TSupport p ) {
  145.     if( s->size < 4 ) {
  146.         s->points[ s->size ] = p;
  147.         s->size++;
  148.     } else {
  149.         if( useDebugOutput ) {
  150.             printf( "SIMPLEX WARNING! Not enough space!\n" );
  151.         }
  152.     }
  153. }
  154.  
  155. // ===========================
  156. // CONVEX SHAPE ROUTINE
  157. // ===========================
  158. TShape * ConvexShape_CreateTriangle( TVec3 a, TVec3 b, TVec3 c ) {
  159.     TShape * shape = calloc( 1, sizeof( TShape ));
  160.     shape->convex = calloc( 1, sizeof( TConvexShape ));
  161.     shape->convex->count = 3;
  162.     shape->convex->points = malloc( shape->convex->count * sizeof( TVec3 ));
  163.     shape->convex->points[0] = a;
  164.     shape->convex->points[1] = b;
  165.     shape->convex->points[2] = c;
  166.     return shape;
  167. }
  168.  
  169. TShape * ConvexShape_CreateTetrahedron( TVec3 a, TVec3 b, TVec3 c, TVec3 d ) {
  170.     TShape * shape = calloc( 1, sizeof( TShape ));
  171.     shape->convex = calloc( 1, sizeof( TConvexShape ));
  172.     shape->convex->count = 4;
  173.     shape->convex->points = malloc( shape->convex->count * sizeof( TVec3 ));
  174.     shape->convex->points[0] = a;
  175.     shape->convex->points[1] = b;
  176.     shape->convex->points[2] = c;
  177.     shape->convex->points[3] = d;
  178.     shape->position = Vec3_Set( 0,0,0 );
  179.     return shape;
  180. }
  181.  
  182. TShape * ConvexShape_CreateSphere( TVec3 position, float radius ) {
  183.     TShape * shape = calloc( 1, sizeof( TShape ));
  184.     shape->sphere = calloc( 1, sizeof( TSphereShape ));    
  185.     shape->position = position;
  186.     shape->sphere->radius = radius;
  187.     return shape;
  188. }
  189.  
  190. void ConvexShape_Delete( TShape * shape ) {
  191.     if( shape->convex ) {
  192.         free( shape->convex );
  193.     }
  194.     if( shape->sphere ) {
  195.         free( shape->sphere );
  196.     }
  197.     free( shape );
  198. }
  199.  
  200. TVec3 ConvexShape_GetFarthestPointInDirection( TShape * shape, TVec3 dir ) {
  201.     const float eps = 0.000001;
  202.     if( fabs( dir.x ) < eps && fabs( dir.y ) < eps && fabs( dir.z ) < eps ) {
  203.         if( useDebugOutput ) {
  204.             printf( "GJK Warning: Zero direction passed!\n" );
  205.         }
  206.     }
  207.     dir = Vec3_Normalize( dir );
  208.     if( shape->convex ) {
  209.         TVec3 farthest;
  210.         float lastDot = -FLT_MAX;
  211.         for( int i = 0; i < shape->convex->count; i++ ) {
  212.             float dot = Vec3_Dot( dir, shape->convex->points[i] );
  213.             if( dot > lastDot ) {
  214.                 farthest = shape->convex->points[i];
  215.                 lastDot = dot;
  216.             }
  217.         }
  218.         farthest = Vec3_Add( farthest, shape->position );
  219.         return farthest;
  220.     }
  221.     if( shape->sphere ) {
  222.         return Vec3_Add( shape->position, Vec3_Scale( dir, shape->sphere->radius ));
  223.     }
  224.     if( shape->box ) {
  225.         TVec3 farthest;
  226.         float lastDot = -FLT_MAX;
  227.         for( int i = 0; i < 8; i++ ) {
  228.             float dot = Vec3_Dot( dir, shape->box->vertices[i] );
  229.             if( dot > lastDot ) {
  230.                 farthest = shape->box->vertices[i];
  231.                 lastDot = dot;
  232.             }
  233.         }
  234.         farthest = Vec3_Add( farthest, shape->position );
  235.         return farthest;
  236.     }
  237.     if( shape->triangle ) {
  238.         int n;
  239.         float lastDot = -FLT_MAX;
  240.         float dots[3] = { Vec3_Dot( dir, shape->triangle->a ), Vec3_Dot( dir, shape->triangle->b ), Vec3_Dot( dir, shape->triangle->c ) };
  241.         for( int i = 0; i < 3; i++ ) {
  242.             if( dots[i] > lastDot ) {
  243.                 n = i;
  244.                 lastDot = dots[i];
  245.             }
  246.         }
  247.         TVec3 farthest;
  248.         if( n == 0 ) {
  249.             farthest = shape->triangle->a;
  250.         }
  251.         if( n == 1 ) {
  252.             farthest = shape->triangle->b;
  253.         }
  254.         if( n == 2 ) {
  255.             farthest = shape->triangle->c;
  256.         }
  257.         farthest = Vec3_Add( farthest, shape->position );
  258.         return farthest;
  259.     }
  260.     return Vec3_Zero();
  261. }
  262.  
  263. TShape * BoxShape_Create( float size ) {
  264.     float hs = size / 2;
  265.     TShape * shape = calloc( 1, sizeof( TShape ));
  266.     shape->box = calloc( 1, sizeof( TBoxShape ));
  267.     shape->box->vertices[0] = Vec3_Set( -hs, -hs, -hs );
  268.     shape->box->vertices[1] = Vec3_Set( -hs, -hs,  hs );
  269.     shape->box->vertices[2] = Vec3_Set(  hs, -hs, -hs );
  270.     shape->box->vertices[3] = Vec3_Set(  hs, -hs,  hs );
  271.    
  272.     shape->box->vertices[4] = Vec3_Set( -hs,  hs, -hs );
  273.     shape->box->vertices[5] = Vec3_Set( -hs,  hs,  hs );
  274.     shape->box->vertices[6] = Vec3_Set(  hs,  hs, -hs );
  275.     shape->box->vertices[7] = Vec3_Set(  hs,  hs,  hs );
  276.     shape->box->size = size;
  277.     return shape;
  278. }
  279.  
  280. // ===========================
  281. // GJK ALGORITHM ROUTINE
  282. // ===========================
  283. TSupport GJK_GetSupport( TShape * shape1, TShape * shape2, TVec3 dir ) {
  284.     TSupport sup;
  285.     sup.supportA = ConvexShape_GetFarthestPointInDirection( shape1, dir );
  286.     sup.minkowskiDifPoint = Vec3_Sub( sup.supportA, ConvexShape_GetFarthestPointInDirection( shape2, Vec3_Negate( dir )));
  287.     return sup;
  288. }
  289.  
  290. bool GJK_ProcessLine( TSimplex * simplex, TVec3 * outDirection ) {
  291.     TVec3 a = simplex->points[1].minkowskiDifPoint;
  292.     TVec3 b = simplex->points[0].minkowskiDifPoint;
  293.     TVec3 ab = Vec3_Sub( b, a );
  294.     TVec3 aO = Vec3_Negate( a );
  295.    
  296.     if( Helper_SameDirection( ab, aO )) {
  297.         *outDirection = Vec3_Cross( Vec3_Cross( ab, aO ), ab );
  298.     } else {
  299.         Simplex_RemovePoint( simplex, 0 );
  300.         *outDirection = aO;
  301.     }
  302.     return false;
  303. }
  304.  
  305. bool GJK_ProcessTriangle( TSimplex * simplex, TVec3 * outDirection ) {
  306.     TVec3 a = simplex->points[2].minkowskiDifPoint;
  307.     TVec3 b = simplex->points[1].minkowskiDifPoint;
  308.     TVec3 c = simplex->points[0].minkowskiDifPoint;
  309.     TVec3 aO = Vec3_Negate( a );
  310.     TVec3 ab = Vec3_Sub( b, a );
  311.     TVec3 ac = Vec3_Sub( c, a );
  312.     TVec3 abc = Vec3_Cross( ab, ac );
  313.     TVec3 acNormal = Vec3_Cross( abc, ac );
  314.     TVec3 abNormal = Vec3_Cross( ab, abc );
  315.    
  316.     if( Helper_SameDirection( acNormal, aO )) {
  317.         if( Helper_SameDirection( ac, aO )) {
  318.             Simplex_RemovePoint( simplex, 1 );
  319.             *outDirection = Vec3_Cross( Vec3_Cross(ac, aO), ac );
  320.         } else {
  321.             if( Helper_SameDirection( ab, aO )) {
  322.                 Simplex_RemovePoint( simplex, 0 );
  323.                 *outDirection = Vec3_Cross( Vec3_Cross(ab, aO), ab);
  324.             } else {
  325.                 Simplex_RemovePoint( simplex, 1 );
  326.                 Simplex_RemovePoint( simplex, 0 );
  327.                 *outDirection = aO;
  328.             }
  329.         }
  330.     } else {
  331.         if( Helper_SameDirection( abNormal, aO )) {
  332.             if( Helper_SameDirection( ab, aO )) {
  333.                 Simplex_RemovePoint( simplex, 0 );
  334.                 *outDirection = Vec3_Cross( Vec3_Cross(ab, aO), ab);
  335.             } else {
  336.                 Simplex_RemovePoint( simplex, 1 );
  337.                 Simplex_RemovePoint( simplex, 0 );
  338.                 *outDirection = aO;
  339.             }
  340.         } else {
  341.             if( Helper_SameDirection( abc, aO )) {
  342.                 *outDirection = Vec3_Cross(Vec3_Cross(abc, aO), abc);
  343.             } else {
  344.                 *outDirection = Vec3_Cross(Vec3_Cross( Vec3_Negate( abc ), aO), Vec3_Negate( abc ) );
  345.             }
  346.         }
  347.     }
  348.    
  349.     return false;
  350. }
  351.  
  352. bool GJK_ProcessTetrahedron( TSimplex * simplex, TVec3 * outDirection ) {
  353.     TVec3 a = simplex->points[3].minkowskiDifPoint;
  354.     TVec3 b = simplex->points[2].minkowskiDifPoint;
  355.     TVec3 c = simplex->points[1].minkowskiDifPoint;
  356.     TVec3 d = simplex->points[0].minkowskiDifPoint;
  357.    
  358.     TVec3 ac = Vec3_Sub( c, a );
  359.     TVec3 ab = Vec3_Sub( b, a );
  360.     TVec3 ad = Vec3_Sub( d, a );
  361.    
  362.     TVec3 acd = Vec3_Cross( ad, ac );
  363.     TVec3 abd = Vec3_Cross( ab, ad );
  364.     TVec3 abc = Vec3_Cross( ac, ab );
  365.    
  366.     TVec3 aO = Vec3_Negate( a );
  367.    
  368.     if( Helper_SameDirection( abc, aO )) {
  369.         if( Helper_SameDirection( Vec3_Cross( abc, ac ), aO )) {
  370.             Simplex_RemovePoint( simplex, 2 );
  371.             Simplex_RemovePoint( simplex, 0 );
  372.             *outDirection = Vec3_Cross( Vec3_Cross( ac, aO ), ac );
  373.         } else if( Helper_SameDirection( Vec3_Cross( ab, abc ), aO )) {
  374.             Simplex_RemovePoint( simplex, 1 );
  375.             Simplex_RemovePoint( simplex, 0 );
  376.             *outDirection = Vec3_Cross( Vec3_Cross( ab, aO ), ab );
  377.         } else {
  378.             Simplex_RemovePoint( simplex, 0 );
  379.             *outDirection = abc;
  380.         }
  381.     } else if( Helper_SameDirection( acd, aO )) {
  382.         if( Helper_SameDirection( Vec3_Cross( acd, ad ), aO )) {
  383.             Simplex_RemovePoint( simplex, 2 );
  384.             Simplex_RemovePoint( simplex, 1 );
  385.             *outDirection = Vec3_Cross( Vec3_Cross( ad, aO ), ad );
  386.         } else if ( Helper_SameDirection( Vec3_Cross( ac, acd ), aO )) {
  387.             Simplex_RemovePoint( simplex, 2 );
  388.             Simplex_RemovePoint( simplex, 0 );
  389.             *outDirection = Vec3_Cross( Vec3_Cross( ac, aO ), ac );
  390.         } else {
  391.            Simplex_RemovePoint( simplex, 2 );
  392.             *outDirection = acd;
  393.         }
  394.     } else if( Helper_SameDirection( abd, aO )) {
  395.         if( Helper_SameDirection( Vec3_Cross( abd, ab ), aO )) {
  396.             Simplex_RemovePoint( simplex, 1 );
  397.             Simplex_RemovePoint( simplex, 0 );
  398.             *outDirection = Vec3_Cross( Vec3_Cross( ab, aO ), ab );
  399.         } else if( Helper_SameDirection( Vec3_Cross( ad, abd ), aO )) {
  400.             Simplex_RemovePoint( simplex, 2 );
  401.             Simplex_RemovePoint( simplex, 1 );
  402.             *outDirection = Vec3_Cross( Vec3_Cross( ad, aO ), ad );
  403.         } else {
  404.             Simplex_RemovePoint( simplex, 1 );
  405.             *outDirection = abd;
  406.         }
  407.     } else {
  408.         return true;
  409.     }
  410.    
  411.     return false;
  412. }
  413.  
  414. bool GJK_ProcessSimplex( TSimplex * simplex, TVec3 * outDirection ) {
  415.     if( simplex->size == 2 ) {
  416.         return GJK_ProcessLine( simplex, outDirection );
  417.     } else if ( simplex->size == 3 ) {
  418.         return GJK_ProcessTriangle( simplex, outDirection );
  419.     } else if( simplex->size == 4 ) {
  420.         return GJK_ProcessTetrahedron( simplex, outDirection );
  421.     }
  422.     return false;
  423. }
  424.  
  425. int Math_LeastSignificantComponent( TVec3 v ) {
  426.     const float epsilon = 0.0001f;
  427.     if( v.x >= epsilon && v.y > epsilon && v.z > epsilon ) {
  428.         return 0;
  429.     } else if ( v.x > epsilon && v.y >= epsilon && v.z > epsilon ) {
  430.         return 1;
  431.     } else if ( v.x > epsilon && v.y > epsilon && v.z >= epsilon ) {
  432.         return 2;
  433.     } else {
  434.         return -1; // should never happen
  435.     }
  436. }
  437.  
  438. TMat3 Mat3_AxisAngle( TVec3 axis, float angle ) {
  439.     float cosTheta = cosf( angle );
  440.     float sinTheta = sinf( angle );
  441.     return (TMat3) { .m[0][0] = cosTheta + axis.x * axis.x * ( 1 - cosTheta ),
  442.                      .m[0][1] = axis.x * axis.y * ( 1 - cosTheta ) - axis.z * sinTheta,
  443.                      .m[0][2] = axis.x * axis.z * ( 1 - cosTheta ) + axis.y * sinTheta,
  444.                      
  445.                      .m[1][0] = axis.y * axis.x * ( 1 - cosTheta ) + axis.z * sinTheta,
  446.                      .m[1][1] = cosTheta + axis.y * axis.y * ( 1 - cosTheta ),
  447.                      .m[1][2] = axis.y * axis.z * ( 1 - cosTheta ) - axis.x * sinTheta,
  448.                      
  449.                      .m[2][0] = axis.z * axis.x * ( 1 - cosTheta ) - axis.y * sinTheta,
  450.                      .m[2][1] = axis.z * axis.y * ( 1 - cosTheta ) + axis.x * sinTheta,
  451.                      .m[2][2] = cosTheta + axis.z * axis.z * ( 1 - cosTheta ) };
  452. }
  453.  
  454. TVec3 Vec3_Rotate( TVec3 v, TMat3 mat ) {
  455.     return (TVec3) { .x = mat.m[0][0] * v.x + mat.m[1][0] * v.y + mat.m[2][0] * v.z,
  456.                      .y = mat.m[0][1] * v.x + mat.m[1][1] * v.y + mat.m[2][1] * v.z,
  457.                      .z = mat.m[0][2] * v.x + mat.m[1][2] * v.y + mat.m[2][2] * v.z };
  458. }
  459.  
  460. void GJK_FixSimplex( TSimplex * simplex, TShape * shape1, TShape * shape2 ) {
  461.     float epsilon = 0.0001f;
  462.     static const TVec3 directions[6] = {
  463.         { .x =  1.0f,  .y =  0.0f,  .z =  0.0f },
  464.         { .x = -1.0f,  .y =  0.0f,  .z =  0.0f },
  465.         { .x =  0.0f,  .y =  1.0f,  .z =  0.0f },
  466.         { .x =  0.0f,  .y = -1.0f,  .z =  0.0f },
  467.         { .x =  0.0f,  .y =  0.0f,  .z =  1.0f },
  468.         { .x =  0.0f,  .y =  0.0f,  .z = -1.0f }};    
  469.     static const TVec3 axes[3] = {
  470.         { .x =  1.0f,  .y = 0.0f,   .z = 0.0f  },
  471.         { .x =  0.0f,  .y = 1.0f,   .z = 0.0f  },
  472.         { .x =  0.0f,  .y = 0.0f,   .z = 1.0f  }};
  473.     switch( simplex->size ) {
  474.         // case 'fall-through' is need to continuously add new points to the simplex
  475.        
  476.         // our simplex is a point
  477.         case 1: {
  478.             TSupport additionalSupport;
  479.             for( int i = 0; i < 6; i++ ) {
  480.                 additionalSupport = GJK_GetSupport( shape1, shape2, directions[i] );
  481.                 if( Vec3_SqrDistance( additionalSupport.minkowskiDifPoint, simplex->points[0].minkowskiDifPoint ) > epsilon ) {
  482.                     Simplex_AddPoint( simplex, additionalSupport );
  483.                     break;
  484.                 }
  485.             }
  486.         }
  487.  
  488.         // our simplex is a line
  489.         case 2: {
  490.             TVec3 line = Vec3_Sub( simplex->points[1].minkowskiDifPoint, simplex->points[0].minkowskiDifPoint );            
  491.             int lsc = Math_LeastSignificantComponent( line );
  492.             TVec3 dir = Vec3_Cross( line, axes[lsc] );            
  493.             TMat3 mat60 = Mat3_AxisAngle( line, M_PI / 3 );            
  494.             for( int i = 0; i < 6; i++ ) {
  495.                 TSupport additionalSupport = GJK_GetSupport( shape1, shape2, dir );
  496.                 if( Vec3_SqrLength( additionalSupport.minkowskiDifPoint ) > epsilon ) {
  497.                     Simplex_AddPoint( simplex, additionalSupport );
  498.                     break;
  499.                 }                
  500.                 dir = Vec3_Rotate( dir, mat60 );
  501.             }
  502.         }
  503.        
  504.         // our simplex is a triangle
  505.         case 3: {
  506.             TVec3 ab = Vec3_Sub( simplex->points[1].minkowskiDifPoint, simplex->points[0].minkowskiDifPoint );
  507.             TVec3 ac = Vec3_Sub( simplex->points[2].minkowskiDifPoint, simplex->points[0].minkowskiDifPoint );
  508.             TVec3 normal = Vec3_Cross( ab, ac );
  509.             TSupport additionalSupport = GJK_GetSupport( shape1, shape2, normal );
  510.             if( Vec3_SqrLength( additionalSupport.minkowskiDifPoint ) < epsilon ) {
  511.                 normal = Vec3_Negate( normal );
  512.                 additionalSupport = GJK_GetSupport( shape1, shape2, normal );
  513.             }
  514.             Simplex_AddPoint( simplex, additionalSupport );
  515.         }
  516.     }
  517. }
  518.  
  519. bool GJK_IsIntersects( TShape * shape1, TShape * shape2, TSimplex * finalSimplex ) {
  520.     TVec3 dir = Vec3_Set( 1, 1, 1 );
  521.    
  522.     TSimplex simplex = { 0 };
  523.     Simplex_AddPoint( &simplex, GJK_GetSupport( shape1, shape2, dir ) );
  524.    
  525.     dir = Vec3_Negate( dir );
  526.    
  527.     int convergenceLimit = 128;
  528.     for( int i = 0; i < convergenceLimit; i++ ) {
  529.         TSupport lastSupport = GJK_GetSupport( shape1, shape2, dir );              
  530.         if( Helper_SameDirection( dir, lastSupport.minkowskiDifPoint )) {
  531.             Simplex_AddPoint( &simplex, lastSupport );            
  532.             if( GJK_ProcessSimplex( &simplex, &dir )) {
  533.                 if( useDebugOutput ) {
  534.                     printf( "GJK: Intersection! %d iteration(s)!\n", i );
  535.                 }
  536.                 if( finalSimplex ) {
  537.                     *finalSimplex = simplex;
  538.                 }
  539.                 return true;
  540.             }            
  541.         } else {
  542.             if( useDebugOutput ) {
  543.                 printf( "GJK: No intersection! %d iteration(s)!\n", i );                
  544.             }
  545.             return false;
  546.         }
  547.     }
  548.    
  549.     // The GJK algorithm can end up with a non-tetrahedron result, this happens when point, line or plane of triangle
  550.     // contains the origin, so we must 'blow-up' simplex to a tetrahedron before pass simplex to EPA
  551.     if( simplex.size > 0 ) {
  552.         if( useDebugOutput ) {
  553.             printf( "GJK: Degenerated case! Fixing simplex!\n" );        
  554.         }
  555.         GJK_FixSimplex( &simplex, shape1, shape2 );
  556.         if( finalSimplex ) {
  557.             *finalSimplex = simplex;
  558.         }
  559.         return true;
  560.     } else {
  561.         return false;
  562.     }
  563. }
  564.  
  565. // ===========================
  566. // EPA ALGORITHM ROUTINE
  567. // ===========================
  568. void Polytope_SetFromSimplex( TEPAPolytope * polytope, TSimplex * simplex ) {
  569.     polytope->vertexCount = 4;
  570.     polytope->vertices = calloc( polytope->vertexCount, sizeof( TSupport ));
  571.     for( int i = 0; i < polytope->vertexCount; i++ ) {
  572.         polytope->vertices[i] = simplex->points[i];
  573.     }
  574.     polytope->faceCount = 4;
  575.     polytope->faces = calloc( polytope->faceCount, sizeof( TEPAFace ));
  576.     polytope->faces[0] = (TEPAFace) { 0, 1, 2 };
  577.     polytope->faces[1] = (TEPAFace) { 0, 3, 1 };
  578.     polytope->faces[2] = (TEPAFace) { 0, 2, 3 };
  579.     polytope->faces[3] = (TEPAFace) { 2, 1, 3 };
  580. }
  581.  
  582. void Polytope_Free( TEPAPolytope * polytope ) {
  583.     free( polytope->faces );
  584.     free( polytope->vertices );
  585.     polytope->faceCount = -1;
  586.     polytope->vertexCount = -1;
  587. }
  588.  
  589. void EdgeList_Create( TEdgeList * edgeList, TEPAPolytope * polytope ) {
  590.     edgeList->count = polytope->faceCount * 3;
  591.     edgeList->edges = calloc( edgeList->count, sizeof( TEdge ));
  592. }
  593.  
  594. void EdgeList_Free( TEdgeList * edgeList ) {
  595.     free( edgeList->edges );
  596.     edgeList->count = 0;
  597. }
  598.  
  599. void EdgeList_Fill( TEdgeList * edgeList, TEPAPolytope * polytope ) {
  600.     for( int i = 0, j = 0; i < polytope->faceCount; i++, j += 3 ) {
  601.         edgeList->edges[j+0] = (TEdge) { .a = polytope->faces[i].a, .b = polytope->faces[i].b, .free = true };
  602.         edgeList->edges[j+1] = (TEdge) { .a = polytope->faces[i].b, .b = polytope->faces[i].c, .free = true };
  603.         edgeList->edges[j+2] = (TEdge) { .a = polytope->faces[i].c, .b = polytope->faces[i].a, .free = true };
  604.        // 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 );
  605.     }
  606. }
  607.  
  608. void EdgeList_MarkHoles( TEdgeList * edgeList ) {
  609.     for( int i = 0; i < edgeList->count; i++ ) {
  610.         for( int j = 0; j < edgeList->count; j++ ) {
  611.             if( edgeList->edges[i].free && edgeList->edges[j].free ) {
  612.                 if( edgeList->edges[j].a == edgeList->edges[i].b && edgeList->edges[j].b == edgeList->edges[i].a ) {
  613.                     edgeList->edges[i].free = false;
  614.                     edgeList->edges[j].free = false;
  615.                 }
  616.             }
  617.         }
  618.     }
  619.     edgeList->freeCount = 0;
  620.     for( int i = 0; i < edgeList->count; i++ ) {
  621.         if( edgeList->edges[i].free ) {
  622.             edgeList->freeCount++;
  623.         }
  624.     }
  625.     //printf( "Free count: %d\n", edgeList->freeCount );
  626. }
  627.  
  628. int Polytope_ReserveFaces( TEPAPolytope * polytope, int faceCount ) {
  629.     int last = polytope->faceCount;
  630.     polytope->faceCount += faceCount;
  631.     polytope->faces = realloc( polytope->faces, polytope->faceCount * sizeof( TEPAFace ));
  632.     return last;
  633. }
  634.  
  635. void Polytope_PatchHoles( TEPAPolytope * polytope, TEdgeList * edgeList, int newPointIndex ) {
  636.     int lastFree = Polytope_ReserveFaces( polytope, edgeList->freeCount );
  637.     for( int i = 0; i < edgeList->count; i++ ) {
  638.         if( edgeList->edges[i].free ) {
  639.             polytope->faces[lastFree] = (TEPAFace) { .a = newPointIndex, .b = edgeList->edges[i].b, .c = edgeList->edges[i].a };
  640.             lastFree++;
  641.         }
  642.     }
  643. }
  644.  
  645. void Polytope_RemoveFace( TEPAPolytope * polytope, int n ) {
  646.     if( n == 0 ) {
  647.         polytope->faceCount--;
  648.         memmove( polytope->faces, polytope->faces + 1, sizeof( TEPAFace ) * polytope->faceCount );
  649.     } else if( n == polytope->faceCount - 1 ) {
  650.         // keep last face in array but reduce face count
  651.         polytope->faceCount--;
  652.     } else {
  653.         memmove( polytope->faces + n, polytope->faces + n + 1, sizeof( TEPAFace ) * ( polytope->faceCount - n ));
  654.         polytope->faceCount--;
  655.     }
  656. }
  657.  
  658. int Polytope_AddVertex( TEPAPolytope * polytope, TSupport newSupport ) {
  659.     int last = polytope->vertexCount;
  660.     polytope->vertexCount++;
  661.     polytope->vertices = realloc( polytope->vertices, polytope->vertexCount * sizeof( TSupport ));
  662.     polytope->vertices[last] = newSupport;
  663.     return last;
  664. }
  665.  
  666. TVec3 Math_ProjectOriginOntoPlane( TVec3 planePoint, TVec3 planeNormal ) {
  667.     float t = -Vec3_Dot( planePoint, planeNormal );
  668.     return Vec3_Negate( Vec3_Scale( planeNormal, t ));
  669. }
  670.  
  671. float Math_DistanceToOrigin( TVec3 normal, TVec3 point ) {
  672.     return Vec3_Dot( normal, point );
  673. }
  674.  
  675. void Math_GetBarycentricCoords( TVec3 p, TVec3 a, TVec3 b, TVec3 c, float *u,float *v,float *w ) {
  676.      TVec3 v0 = Vec3_Sub( b, a );
  677.      TVec3 v1 = Vec3_Sub( c, a );
  678.      TVec3 v2 = Vec3_Sub( p, a );
  679.      float d00 = Vec3_Dot( v0, v0 );
  680.      float d01 = Vec3_Dot( v0, v1 );
  681.      float d11 = Vec3_Dot( v1, v1 );
  682.      float d20 = Vec3_Dot( v2, v0 );
  683.      float d21 = Vec3_Dot( v2, v1 );
  684.      float denom = d00 * d11 - d01 * d01;
  685.      *v = (d11 * d20 - d01 * d21) / denom;
  686.      *w = (d00 * d21 - d01 * d20) / denom;
  687.      *u = 1.0f - *v - *w;
  688. }
  689.  
  690. bool Polytope_IsFaceSeenFromPoint( TEPAPolytope * polytope, int a, int b, int c, TVec3 point ) {
  691.     TVec3 va = polytope->vertices[ a ].minkowskiDifPoint;
  692.     TVec3 vb = polytope->vertices[ b ].minkowskiDifPoint;
  693.     TVec3 vc = polytope->vertices[ c ].minkowskiDifPoint;
  694.     TVec3 normal = Vec3_Cross( Vec3_Sub( vb, va ), Vec3_Sub( vc, va ));
  695.     return Vec3_Dot( normal, Vec3_Sub( point, va )) > 0;
  696. }
  697.  
  698. int Polytope_GetFirstFaceSeenFromPoint( TEPAPolytope * polytope, TVec3 point ) {
  699.     for( int i = 0; i < polytope->faceCount; i++ ) {
  700.         if( Polytope_IsFaceSeenFromPoint( polytope, polytope->faces[i].a, polytope->faces[i].b, polytope->faces[i].c, point )) {
  701.             return i;
  702.         }
  703.     }
  704.     return -1;
  705. }
  706.  
  707. bool Polytope_IsFaceDegenerated( TEPAPolytope * polytope, int n ) {
  708.     TVec3 a = polytope->vertices[ polytope->faces[ n ].a ].minkowskiDifPoint;
  709.     TVec3 b = polytope->vertices[ polytope->faces[ n ].b ].minkowskiDifPoint;
  710.     TVec3 c = polytope->vertices[ polytope->faces[ n ].c ].minkowskiDifPoint;
  711.  
  712.     TVec3 normal = Vec3_Cross( Vec3_Sub( b, a ), Vec3_Sub( c, a ) );
  713.  
  714.     // degenerated triangle, ignore
  715.     return Vec3_SqrLength( normal ) < 0.00001;
  716. }
  717.  
  718. void Polytope_RemoveDegeneratedFaces( TEPAPolytope * polytope ) {
  719.     while( true ) {
  720.         int n = -1;
  721.         for( int i = 0; i < polytope->faceCount; i++ ) {
  722.             if( Polytope_IsFaceDegenerated( polytope, i )) {
  723.                 Polytope_RemoveFace( polytope, i );
  724.                 n = i;
  725.                 break;
  726.             }
  727.         }
  728.         if( n < 0 ) {
  729.             break;
  730.         }
  731.     }
  732. }
  733.  
  734. TEPATriangle Polytope_GetClosestTriangleToOrigin( TEPAPolytope * polytope ) {
  735.     int closest = -1;
  736.     float closestDistance = FLT_MAX;
  737.     TVec3 closestNormal;
  738.     for( int i = 0; i < polytope->faceCount; i++ ) {
  739.         TVec3 a = polytope->vertices[ polytope->faces[ i ].a ].minkowskiDifPoint;
  740.         TVec3 b = polytope->vertices[ polytope->faces[ i ].b ].minkowskiDifPoint;
  741.         TVec3 c = polytope->vertices[ polytope->faces[ i ].c ].minkowskiDifPoint;
  742.  
  743.         TVec3 normal = Vec3_Cross( Vec3_Sub( b, a ), Vec3_Sub( c, a ) );
  744.  
  745.         // degenerated triangle, ignore
  746.         if( Vec3_SqrLength( normal ) < 0.00001 ) {
  747.             continue;
  748.         }
  749.         float d = Math_DistanceToOrigin( normal, a );
  750.         if( d < closestDistance ) {
  751.             closestDistance = d;
  752.             closest = i;
  753.             closestNormal = normal;
  754.         }        
  755.     }
  756.     if( closest >= 0 ) {
  757.         return (TEPATriangle) { .a = polytope->vertices[ polytope->faces[ closest ].a ], .b = polytope->vertices[ polytope->faces[ closest ].b ],
  758.                                 .c = polytope->vertices[ polytope->faces[ closest ].c ], .normal = closestNormal, .dist = closestDistance, .numInPolytope = closest };
  759.     } else {
  760.          return (TEPATriangle) { .numInPolytope = -1 };      
  761.     }
  762. }
  763.  
  764. bool EPA_ComputeContacts( TEPAPolytope * polytope, TShape * shape1, TShape * shape2, TEPAContact * outContact ) {
  765.     const int convergenceLimit = 50;
  766.     for( int i = 0; i < convergenceLimit; i++ ) {      
  767.        // Polytope_RemoveDegeneratedFaces( polytope );  
  768.         TEPATriangle closestTriangle = Polytope_GetClosestTriangleToOrigin( polytope );
  769.         if( closestTriangle.numInPolytope < 0 ) {
  770.             return false;
  771.         }
  772.         TSupport p = GJK_GetSupport( shape1, shape2, closestTriangle.normal );
  773.         float d = Vec3_Dot( p.minkowskiDifPoint, closestTriangle.normal );
  774.         if( d - closestTriangle.dist < 0.001 ) {      
  775.             if( d < 0.0001 ) {
  776.                 return false;
  777.             }      
  778.             TVec3 proj = Math_ProjectOriginOntoPlane( closestTriangle.a.minkowskiDifPoint, closestTriangle.normal );
  779.             float u, v, w;
  780.             Math_GetBarycentricCoords( proj, closestTriangle.a.minkowskiDifPoint, closestTriangle.b.minkowskiDifPoint, closestTriangle.c.minkowskiDifPoint, &u, &v, &w );
  781.             TVec3 a = Vec3_Scale( closestTriangle.a.supportA, u );
  782.             TVec3 b = Vec3_Scale( closestTriangle.b.supportA, v );
  783.             TVec3 c = Vec3_Scale( closestTriangle.c.supportA, w );
  784.             TVec3 collPoint = Vec3_Add( Vec3_Add( a, b ), c );
  785.             closestTriangle.normal = Vec3_Normalize( Vec3_Negate( closestTriangle.normal ));
  786.             outContact->normal = closestTriangle.normal;
  787.             outContact->penetrationDepth = d;
  788.             if( useDebugOutput ) {
  789.                 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 );
  790.             }
  791.             return true;
  792.         } else {                
  793.             TEdgeList edgeList;            
  794.             while( true ) {
  795.                 int seenFace = Polytope_GetFirstFaceSeenFromPoint( polytope, p.minkowskiDifPoint );
  796.                 if( seenFace < 0 ) {
  797.                     break;
  798.                 } else {
  799.                     Polytope_RemoveFace( polytope, seenFace );
  800.                 }
  801.             }        
  802.             EdgeList_Create( &edgeList, polytope );    
  803.             EdgeList_Fill( &edgeList, polytope );
  804.             EdgeList_MarkHoles( &edgeList );
  805.             Polytope_PatchHoles( polytope, &edgeList, Polytope_AddVertex( polytope, p ) );
  806.             EdgeList_Free( &edgeList );            
  807.         }
  808.     }
  809.     if( useDebugOutput ) {
  810.         printf( "EPA: Convergence limit has reached!\n" );
  811.     }
  812.     return false;
  813. }
  814.  
  815. // ===========================
  816. // TRIANGLE MESH SHAPE ROUTINE
  817. // ===========================
  818. TShape * TriangleMesh_CreatePlane( float width, float depth ) {
  819.     TShape * shape = calloc( 1, sizeof( TShape ));
  820.     TTriangleMeshShape * triMesh = calloc( 1, sizeof( TTriangleMeshShape ));
  821.     triMesh->vertexCount = 4;
  822.     triMesh->vertices = calloc( triMesh->vertexCount, sizeof( TVec3 ));
  823.     float hw = width / 2, hd = depth / 2;
  824.     triMesh->vertices[0] = Vec3_Set( -hw, 0.0, -hd );
  825.     triMesh->vertices[1] = Vec3_Set( -hw, 0.0,  hd );
  826.     triMesh->vertices[2] = Vec3_Set(  hw, 0.0,  hd );
  827.     triMesh->vertices[3] = Vec3_Set(  hw, 0.0, -hd );    
  828.     triMesh->faceCount = 2;
  829.     triMesh->faces = calloc( triMesh->faceCount, sizeof( TEPAFace ));
  830.     triMesh->faces[0] = (TEPAFace) { 0, 1, 2 };
  831.     triMesh->faces[1] = (TEPAFace) { 0, 2, 3 };
  832.     shape->triMesh = triMesh;
  833.     return shape;
  834. }
  835.  
  836. // ===========================
  837. // BODY ROUTINE
  838. // ===========================
  839. TBody * Body_Create( TShape * shape ) {
  840.     TBody * body = calloc( 1, sizeof( TBody ));
  841.     body->position = shape->position;
  842.     body->shape = shape;
  843.     return body;
  844. }
  845.  
  846. void Body_ProcessCollision( TBody * body1, TShape * shape ) {
  847.     TSimplex finalSimplex;
  848.     TEPAContact contact;
  849.     while( GJK_IsIntersects( body1->shape, shape, &finalSimplex )) {
  850.         if( finalSimplex.size == 4 ) {
  851.             if( currentPolytope ) {
  852.                 Polytope_Free( currentPolytope );
  853.                 free( currentPolytope );
  854.             }
  855.             currentPolytope = calloc( 1, sizeof( TEPAPolytope ));
  856.             Polytope_SetFromSimplex( currentPolytope, &finalSimplex );        
  857.             if( EPA_ComputeContacts( currentPolytope, body1->shape, shape, &contact )) {
  858.                 body1->position = Vec3_Add( body1->position, Vec3_Scale( contact.normal, contact.penetrationDepth / 2));
  859.             } else {
  860.                 if( useDebugOutput ) {
  861.                     printf( "EPA: Unable to compute contacts!\n" );
  862.                 }
  863.                 break;
  864.             }    
  865.             body1->shape->position = body1->position;  
  866.         } else {
  867.             if( useDebugOutput ) {
  868.                 printf( "Wrong simplex size! Must be tetrahedron, got %d\n", finalSimplex.size );
  869.             }
  870.             break;
  871.         }
  872.     }
  873. }
  874.  
  875. void Body_SolveCollision( TBody * body1, TBody * body2 ) {
  876.     if( body1->shape->triMesh ) {
  877.         TBody * temp = body1;
  878.         body1 = body2;
  879.         body2 = temp;
  880.     }
  881.    
  882.     currentShape1 = body1->shape;
  883.     currentShape2 = body2->shape;
  884.  
  885.     body1->shape->position = body1->position;
  886.     body2->shape->position = body2->position;
  887.    
  888.     if( body2->shape->triMesh ) {
  889.         TTriangleMeshShape * triMesh = body2->shape->triMesh;
  890.         for( int i = 0; i < triMesh->faceCount; i++ ) {
  891.             TTriangleShape triangle = { .a = triMesh->vertices[ triMesh->faces[i].a ], .b = triMesh->vertices[ triMesh->faces[i].b ], .c = triMesh->vertices[ triMesh->faces[i].c ] };
  892.             TShape shape = { .position = body2->position, .triangle = &triangle };
  893.             Body_ProcessCollision( body1, &shape );
  894.         }
  895.     } else {
  896.         Body_ProcessCollision( body1, body2->shape );
  897.     }
  898. }
  899.  
  900. // ===========================
  901. // RENDERING ROUTINE
  902. // ===========================
  903. #include <windows.h>
  904. #include "glut.h"
  905. #include "gl/gl.h"
  906.  
  907. #define WINDOW_SIZE 800
  908.  
  909. void Renderer_DrawShape( TShape * shape ) {
  910.    // glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
  911.    
  912.     if( shape->convex ) {
  913.         if( shape->convex->count == 3 ) { // triangle        
  914.             glPushMatrix();
  915.            
  916.             glTranslatef( shape->position.x, shape->position.y, shape->position.z );
  917.            
  918.             TVec3 a = shape->convex->points[0];
  919.             TVec3 b = shape->convex->points[1];
  920.             TVec3 c = shape->convex->points[2];
  921.            
  922.             glBegin(GL_TRIANGLES);
  923.             glColor3ub( 0, 0, 255 );
  924.             glVertex3f( a.x, a.y, a.z );
  925.             glVertex3f( b.x, b.y, b.z );
  926.             glVertex3f( c.x, c.y, c.z );    
  927.            
  928.             glEnd();
  929.            
  930.             glPopMatrix();
  931.         }
  932.        
  933.         if( shape->convex->count == 4 ) { // tetrahedron        
  934.             glPushMatrix();
  935.            
  936.             glTranslatef( shape->position.x, shape->position.y, shape->position.z );
  937.            
  938.             TVec3 a = shape->convex->points[0];
  939.             TVec3 b = shape->convex->points[1];
  940.             TVec3 c = shape->convex->points[2];
  941.             TVec3 d = shape->convex->points[3];
  942.            
  943.             glBegin(GL_TRIANGLES);
  944.             glColor3ub( 255, 0, 0 );
  945.             glVertex3f( a.x, a.y, a.z );
  946.             glVertex3f( b.x, b.y, b.z );
  947.             glVertex3f( c.x, c.y, c.z );
  948.            
  949.             glColor3ub( 0, 255, 0 );
  950.             glVertex3f( a.x, a.y, a.z );
  951.             glVertex3f( d.x, d.y, d.z );
  952.             glVertex3f( b.x, b.y, b.z );
  953.            
  954.             glColor3ub( 0, 0, 255 );
  955.             glVertex3f( a.x, a.y, a.z );
  956.             glVertex3f( c.x, c.y, c.z );
  957.             glVertex3f( d.x, d.y, d.z );            
  958.            
  959.             glColor3ub( 255, 255, 0 );
  960.             glVertex3f( c.x, c.y, c.z );
  961.             glVertex3f( b.x, b.y, b.z );
  962.             glVertex3f( d.x, d.y, d.z );
  963.            
  964.             glEnd();
  965.            
  966.             glPopMatrix();
  967.         }
  968.     } else if( shape->sphere ) {
  969.        
  970.         glPushMatrix();        
  971.         glTranslatef( shape->position.x, shape->position.y, shape->position.z );        
  972.         glutSolidSphere( shape->sphere->radius, 32, 32 );        
  973.         glPopMatrix();
  974.        
  975.     } else if( shape->box ) {        
  976.         glPushMatrix();        
  977.         glScalef( shape->box->size, shape->box->size, shape->box->size );
  978.         glTranslatef( shape->position.x, shape->position.y, shape->position.z );        
  979.         glutSolidCube( 1 );        
  980.         glPopMatrix();        
  981.     } else if( shape->triMesh ) {
  982.         glPushMatrix();        
  983.         glTranslatef( shape->position.x, shape->position.y, shape->position.z );  
  984.        
  985.         glColor3ub( 125, 200, 80 );
  986.        
  987.         glBegin(GL_TRIANGLES);
  988.         for( int i = 0; i < shape->triMesh->faceCount; i++ ) {    
  989.            
  990.             TVec3 a = shape->triMesh->vertices[ shape->triMesh->faces[ i ].a ];
  991.             TVec3 b = shape->triMesh->vertices[ shape->triMesh->faces[ i ].b ];
  992.             TVec3 c = shape->triMesh->vertices[ shape->triMesh->faces[ i ].c ];
  993.            
  994.             TVec3 normal = Vec3_Cross( Vec3_Sub( b, a), Vec3_Sub( c, a ));
  995.            
  996.             glNormal3f( normal.x, normal.y, normal.z );
  997.            
  998.             glVertex3f( a.x, a.y, a.z );
  999.             glVertex3f( b.x, b.y, b.z );
  1000.             glVertex3f( c.x, c.y, c.z );          
  1001.            
  1002.         }
  1003.         glEnd();
  1004.        
  1005.         glPopMatrix();
  1006.     }
  1007. }
  1008.  
  1009. float sceneAngle = 0;
  1010.  
  1011. void Renderer_Display() {
  1012.     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  1013.  
  1014.     glMatrixMode(GL_MODELVIEW);
  1015.     glLoadIdentity();
  1016.     gluLookAt( 0, 0.78, 3.5, 0, 0, 0, 0, 1, 0 );
  1017.     glRotatef( sceneAngle, 0, 1, 0 );
  1018.    // a+=0.725;
  1019.    
  1020.    /*
  1021.     if( currentPolytope ) {
  1022.         //glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
  1023.        
  1024.         for( int i = 0; i < currentPolytope->faceCount; i++ ) {
  1025.             int ia = currentPolytope->faces[i].a;
  1026.             int ib = currentPolytope->faces[i].b;
  1027.             int ic = currentPolytope->faces[i].c;
  1028.      
  1029.             TVec3 a = currentPolytope->vertices[ ia ].minkowskiDifPoint;
  1030.             TVec3 b = currentPolytope->vertices[ ib ].minkowskiDifPoint;
  1031.             TVec3 c = currentPolytope->vertices[ ic ].minkowskiDifPoint;
  1032.            
  1033.             TVec3 normal = Vec3_Normalize( Vec3_Cross( Vec3_Sub( b,a ), Vec3_Sub( c,a )));
  1034.            
  1035.             glBegin(GL_TRIANGLES);
  1036.             //glColor3ub( i * 8 + 50, i * 8 + 50, i * 8 + 50 );
  1037.             glVertex3f( a.x, a.y, a.z );
  1038.             glVertex3f( b.x, b.y, b.z );
  1039.             glVertex3f( c.x, c.y, c.z );
  1040.             glEnd();
  1041.            
  1042.             glBegin(GL_LINES);
  1043.             glColor3ub( 255, 0, 255 );
  1044.             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 );
  1045.             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 );
  1046.             glEnd();
  1047.         }
  1048.     }*/
  1049.     glColor3ub( 255, 0, 0 );
  1050.     Renderer_DrawShape( currentShape1 );
  1051.     glColor3ub( 255, 255, 0 );
  1052.     Renderer_DrawShape( currentShape2 );
  1053.     glColor3ub( 0, 255, 255 );
  1054.     Renderer_DrawShape( currentShape3 );  
  1055.     glutSwapBuffers();
  1056.     glutPostRedisplay();
  1057. }
  1058.  
  1059. void Renderer_Init() {
  1060.     glClearColor(0.000, 0.110, 0.392, 0.0);
  1061.  
  1062.     glMatrixMode(GL_PROJECTION);
  1063.     glLoadIdentity();
  1064.     gluPerspective( 80, 1, 0.01, 1024 );
  1065.    
  1066.  
  1067.    
  1068.     glClearDepth( 1.0f );
  1069.     glEnable( GL_DEPTH_TEST );
  1070.     glDepthFunc( GL_LEQUAL );
  1071.     glHint( GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST );
  1072.     glEnable( GL_TEXTURE_2D );
  1073.     glShadeModel( GL_SMOOTH );
  1074.     glEnable( GL_LIGHTING );
  1075.     glEnable( GL_LIGHT0 );
  1076.    
  1077.     glEnable( GL_CULL_FACE );
  1078.    
  1079.     float dir[4] = { 1, 1, 1, 0 };
  1080.     glLightfv( GL_LIGHT0, GL_POSITION, dir );
  1081.    
  1082.     glDisable( GL_STENCIL_TEST );
  1083.     glCullFace( GL_BACK );    
  1084.     glEnable( GL_ALPHA_TEST );
  1085.     glAlphaFunc( GL_GREATER, 0.025f );
  1086.  
  1087.    
  1088.     //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 ));
  1089.     //currentShape1 = ConvexShape_CreateTetrahedron( Vec3_Set( 0, 0, 0 ), Vec3_Set( 0, 1, 0 ), Vec3_Set( 1, 0, 0 ), Vec3_Set( 0, 0, 1 ));
  1090.    
  1091.     //currentShape2 = ConvexShape_CreateSphere( Vec3_Set( 0.5,0,0.0), 0.8 );
  1092.     //currentShape2 = ConvexShape_CreateTriangle( Vec3_Set( 0, 0, 0.5 ), Vec3_Set( 0.5, 1, 0.5 ), Vec3_Set( 0.5, 0, 0 ));
  1093.    // currentShape2 = ConvexShape_CreateTriangle( Vec3_Set( 0, 0.1, 0 ), Vec3_Set( 0, 0.1, 1 ), Vec3_Set( 1, 0.1, 0 ));
  1094.    
  1095.     currentShape1 = ConvexShape_CreateSphere( Vec3_Set( 0,0,0 ), 0.2 );//BoxShape_Create( 1 );
  1096.     currentShape2 = TriangleMesh_CreatePlane( 5, 5 );
  1097.     currentShape3 = BoxShape_Create( 1 );//ConvexShape_CreateSphere( Vec3_Set( 0,0,0 ), 0.2 );
  1098.      
  1099.     body1 = Body_Create( currentShape1 );
  1100.     body1->position = Vec3_Set( 0, 0.5, 0 );
  1101.    
  1102.     body2 = Body_Create( currentShape2 );    
  1103.    
  1104.     body3 = Body_Create( currentShape3 );
  1105.     body3->position = Vec3_Set( 0, 1, 0 );
  1106.      
  1107.     Body_SolveCollision( body1, body3 );
  1108.     Body_SolveCollision( body1, body2 );
  1109.     //Body_SolveCollision( body3, body2 );
  1110. }
  1111.  
  1112. void KeyFunc( unsigned char key, int x, int y ) {
  1113.     bool move = false;
  1114.     if( key == 'A' || key == 'a' ) {
  1115.         body1->position = Vec3_Add( body1->position, Vec3_Set( -0.01, 0.0, 0.0 ));
  1116.         move = true;
  1117.     }
  1118.     if( key == 'D' || key == 'd' ) {
  1119.         body1->position = Vec3_Add( body1->position, Vec3_Set(  0.01, 0.0, 0.0 ));
  1120.         move = true;
  1121.     }
  1122.     if( key == 'W' || key == 'w' ) {
  1123.         body1->position = Vec3_Add( body1->position, Vec3_Set( 0.0, 0.0, -0.01 ));
  1124.         move = true;
  1125.     }
  1126.     if( key == 'S' || key == 's' ) {
  1127.         body1->position = Vec3_Add( body1->position, Vec3_Set( 0, 0.0, 0.01 ));
  1128.         move = true;
  1129.     }
  1130.     if( key == 'X' || key == 'x' ) {
  1131.         body1->position = Vec3_Add( body1->position, Vec3_Set( 0.0, 0.01, 0.0 ));
  1132.         move = true;
  1133.     }
  1134.     if( key == 'Z' || key == 'z' ) {
  1135.         body1->position = Vec3_Add( body1->position, Vec3_Set( 0, -0.01, 0.0 ));
  1136.         move = true;
  1137.     }
  1138.    
  1139.     if( key == 'Q' || key == 'q' ) {
  1140.         sceneAngle += 1.0f;
  1141.     }
  1142.     if( key == 'E' || key == 'e' ) {
  1143.         sceneAngle -= 1.0f;
  1144.     }
  1145.    
  1146.     if( move ) {
  1147.         Body_SolveCollision( body1, body3 );
  1148.         Body_SolveCollision( body1, body2 );
  1149.         //Body_SolveCollision( body3, body2 );
  1150.     }
  1151. }
  1152.  
  1153. // ===========================
  1154. // TESTS
  1155. // ===========================
  1156. int main(int argc, char **argv) {
  1157.     glutInit(&argc, argv);
  1158.     glutInitDisplayMode( GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA );
  1159.     glutInitWindowSize( 800, 800 );
  1160.     glutInitWindowPosition(0, 0);
  1161.     glutCreateWindow("Test");
  1162.     glutDisplayFunc(Renderer_Display);
  1163.     glutKeyboardFunc( KeyFunc );
  1164.    
  1165.     Renderer_Init();
  1166.  
  1167.     glutMainLoop();
  1168. }
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!