API tools faq
paste
mrDIMAS

GJK EPA Modified

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