git.sesse.net Git - pistorm/blob - raylib_pi4_test/physac.h

   1 /**********************************************************************************************
   2 *
   3 *   Physac v1.1 - 2D Physics library for videogames
   4 *
   5 *   DESCRIPTION:
   6 *
   7 *   Physac is a small 2D physics engine written in pure C. The engine uses a fixed time-step thread loop
   8 *   to simluate physics. A physics step contains the following phases: get collision information,
   9 *   apply dynamics, collision solving and position correction. It uses a very simple struct for physic
  10 *   bodies with a position vector to be used in any 3D rendering API.
  11 *
  12 *   CONFIGURATION:
  13 *
  14 *   #define PHYSAC_IMPLEMENTATION
  15 *       Generates the implementation of the library into the included file.
  16 *       If not defined, the library is in header only mode and can be included in other headers
  17 *       or source files without problems. But only ONE file should hold the implementation.
  18 *
  19 *   #define PHYSAC_STATIC (defined by default)
  20 *       The generated implementation will stay private inside implementation file and all
  21 *       internal symbols and functions will only be visible inside that file.
  22 *
  23 *   #define PHYSAC_DEBUG
  24 *       Show debug traces log messages about physic bodies creation/destruction, physic system errors,
  25 *       some calculations results and NULL reference exceptions
  26 *
  27 *   #define PHYSAC_DEFINE_VECTOR2_TYPE
  28 *       Forces library to define struct Vector2 data type (float x; float y)
  29 *
  30 *   #define PHYSAC_AVOID_TIMMING_SYSTEM
  31 *       Disables internal timming system, used by UpdatePhysics() to launch timmed physic steps,
  32 *       it allows just running UpdatePhysics() automatically on a separate thread at a desired time step.
  33 *       In case physics steps update needs to be controlled by user with a custom timming mechanism,
  34 *       just define this flag and the internal timming mechanism will be avoided, in that case,
  35 *       timming libraries are neither required by the module.
  36 *
  37 *   #define PHYSAC_MALLOC()
  38 *   #define PHYSAC_CALLOC()
  39 *   #define PHYSAC_FREE()
  40 *       You can define your own malloc/free implementation replacing stdlib.h malloc()/free() functions.
  41 *       Otherwise it will include stdlib.h and use the C standard library malloc()/free() function.
  42 *
  43 *   COMPILATION:
  44 *
  45 *   Use the following code to compile with GCC:
  46 *       gcc -o $(NAME_PART).exe $(FILE_NAME) -s -static -lraylib -lopengl32 -lgdi32 -lwinmm -std=c99
  47 *
  48 *   VERSIONS HISTORY:
  49 *       1.1 (20-Jan-2021) @raysan5: Library general revision
  50 *               Removed threading system (up to the user)
  51 *               Support MSVC C++ compilation using CLITERAL()
  52 *               Review DEBUG mechanism for TRACELOG() and all TRACELOG() messages
  53 *               Review internal variables/functions naming for consistency
  54 *               Allow option to avoid internal timming system, to allow app manage the steps
  55 *       1.0 (12-Jun-2017) First release of the library
  56 *
  57 *
  58 *   LICENSE: zlib/libpng
  59 *
  60 *   Copyright (c) 2016-2021 Victor Fisac (@victorfisac) and Ramon Santamaria (@raysan5)
  61 *
  62 *   This software is provided "as-is", without any express or implied warranty. In no event
  63 *   will the authors be held liable for any damages arising from the use of this software.
  64 *
  65 *   Permission is granted to anyone to use this software for any purpose, including commercial
  66 *   applications, and to alter it and redistribute it freely, subject to the following restrictions:
  67 *
  68 *     1. The origin of this software must not be misrepresented; you must not claim that you
  69 *     wrote the original software. If you use this software in a product, an acknowledgment
  70 *     in the product documentation would be appreciated but is not required.
  71 *
  72 *     2. Altered source versions must be plainly marked as such, and must not be misrepresented
  73 *     as being the original software.
  74 *
  75 *     3. This notice may not be removed or altered from any source distribution.
  76 *
  77 **********************************************************************************************/
  78
  79 #if !defined(PHYSAC_H)
  80 #define PHYSAC_H
  81
  82 #if defined(PHYSAC_STATIC)
  83     #define PHYSACDEF static            // Functions just visible to module including this file
  84 #else
  85     #if defined(__cplusplus)
  86         #define PHYSACDEF extern "C"    // Functions visible from other files (no name mangling of functions in C++)
  87     #else
  88         #define PHYSACDEF extern        // Functions visible from other files
  89     #endif
  90 #endif
  91
  92 // Allow custom memory allocators
  93 #ifndef PHYSAC_MALLOC
  94     #define PHYSAC_MALLOC(size)         malloc(size)
  95 #endif
  96 #ifndef PHYSAC_CALLOC
  97     #define PHYSAC_CALLOC(size, n)      calloc(size, n)
  98 #endif
  99 #ifndef PHYSAC_FREE
 100     #define PHYSAC_FREE(ptr)            free(ptr)
 101 #endif
 102
 103 //----------------------------------------------------------------------------------
 104 // Defines and Macros
 105 //----------------------------------------------------------------------------------
 106 #define PHYSAC_MAX_BODIES               64          // Maximum number of physic bodies supported
 107 #define PHYSAC_MAX_MANIFOLDS            4096        // Maximum number of physic bodies interactions (64x64)
 108 #define PHYSAC_MAX_VERTICES             24          // Maximum number of vertex for polygons shapes
 109 #define PHYSAC_DEFAULT_CIRCLE_VERTICES  24          // Default number of vertices for circle shapes
 110
 111 #define PHYSAC_COLLISION_ITERATIONS     100
 112 #define PHYSAC_PENETRATION_ALLOWANCE    0.05f
 113 #define PHYSAC_PENETRATION_CORRECTION   0.4f
 114
 115 #define PHYSAC_PI                       3.14159265358979323846f
 116 #define PHYSAC_DEG2RAD                  (PHYSAC_PI/180.0f)
 117
 118 //----------------------------------------------------------------------------------
 119 // Data Types Structure Definition
 120 //----------------------------------------------------------------------------------
 121 #if defined(__STDC__) && __STDC_VERSION__ >= 199901L
 122     #include <stdbool.h>
 123 #endif
 124
 125 typedef enum PhysicsShapeType { PHYSICS_CIRCLE = 0, PHYSICS_POLYGON } PhysicsShapeType;
 126
 127 // Previously defined to be used in PhysicsShape struct as circular dependencies
 128 typedef struct PhysicsBodyData *PhysicsBody;
 129
 130 #if defined(PHYSAC_DEFINE_VECTOR2_TYPE)
 131 // Vector2 type
 132 typedef struct Vector2 {
 133     float x;
 134     float y;
 135 } Vector2;
 136 #endif
 137
 138 // Matrix2x2 type (used for polygon shape rotation matrix)
 139 typedef struct Matrix2x2 {
 140     float m00;
 141     float m01;
 142     float m10;
 143     float m11;
 144 } Matrix2x2;
 145
 146 typedef struct PhysicsVertexData {
 147     unsigned int vertexCount;                   // Vertex count (positions and normals)
 148     Vector2 positions[PHYSAC_MAX_VERTICES];     // Vertex positions vectors
 149     Vector2 normals[PHYSAC_MAX_VERTICES];       // Vertex normals vectors
 150 } PhysicsVertexData;
 151
 152 typedef struct PhysicsShape {
 153     PhysicsShapeType type;                      // Shape type (circle or polygon)
 154     PhysicsBody body;                           // Shape physics body data pointer
 155     PhysicsVertexData vertexData;               // Shape vertices data (used for polygon shapes)
 156     float radius;                               // Shape radius (used for circle shapes)
 157     Matrix2x2 transform;                        // Vertices transform matrix 2x2
 158 } PhysicsShape;
 159
 160 typedef struct PhysicsBodyData {
 161     unsigned int id;                            // Unique identifier
 162     bool enabled;                               // Enabled dynamics state (collisions are calculated anyway)
 163     Vector2 position;                           // Physics body shape pivot
 164     Vector2 velocity;                           // Current linear velocity applied to position
 165     Vector2 force;                              // Current linear force (reset to 0 every step)
 166     float angularVelocity;                      // Current angular velocity applied to orient
 167     float torque;                               // Current angular force (reset to 0 every step)
 168     float orient;                               // Rotation in radians
 169     float inertia;                              // Moment of inertia
 170     float inverseInertia;                       // Inverse value of inertia
 171     float mass;                                 // Physics body mass
 172     float inverseMass;                          // Inverse value of mass
 173     float staticFriction;                       // Friction when the body has not movement (0 to 1)
 174     float dynamicFriction;                      // Friction when the body has movement (0 to 1)
 175     float restitution;                          // Restitution coefficient of the body (0 to 1)
 176     bool useGravity;                            // Apply gravity force to dynamics
 177     bool isGrounded;                            // Physics grounded on other body state
 178     bool freezeOrient;                          // Physics rotation constraint
 179     PhysicsShape shape;                         // Physics body shape information (type, radius, vertices, transform)
 180 } PhysicsBodyData;
 181
 182 typedef struct PhysicsManifoldData {
 183     unsigned int id;                            // Unique identifier
 184     PhysicsBody bodyA;                          // Manifold first physics body reference
 185     PhysicsBody bodyB;                          // Manifold second physics body reference
 186     float penetration;                          // Depth of penetration from collision
 187     Vector2 normal;                             // Normal direction vector from 'a' to 'b'
 188     Vector2 contacts[2];                        // Points of contact during collision
 189     unsigned int contactsCount;                 // Current collision number of contacts
 190     float restitution;                          // Mixed restitution during collision
 191     float dynamicFriction;                      // Mixed dynamic friction during collision
 192     float staticFriction;                       // Mixed static friction during collision
 193 } PhysicsManifoldData, *PhysicsManifold;
 194
 195 #if defined(__cplusplus)
 196 extern "C" {                                    // Prevents name mangling of functions
 197 #endif
 198
 199 //----------------------------------------------------------------------------------
 200 // Module Functions Declaration
 201 //----------------------------------------------------------------------------------
 202 // Physics system management
 203 PHYSACDEF void InitPhysics(void);                                                                           // Initializes physics system
 204 PHYSACDEF void UpdatePhysics(void);                                                                         // Update physics system
 205 PHYSACDEF void ResetPhysics(void);                                                                          // Reset physics system (global variables)
 206 PHYSACDEF void ClosePhysics(void);                                                                          // Close physics system and unload used memory
 207 PHYSACDEF void SetPhysicsTimeStep(double delta);                                                            // Sets physics fixed time step in milliseconds. 1.666666 by default
 208 PHYSACDEF void SetPhysicsGravity(float x, float y);                                                         // Sets physics global gravity force
 209
 210 // Physic body creation/destroy
 211 PHYSACDEF PhysicsBody CreatePhysicsBodyCircle(Vector2 pos, float radius, float density);                    // Creates a new circle physics body with generic parameters
 212 PHYSACDEF PhysicsBody CreatePhysicsBodyRectangle(Vector2 pos, float width, float height, float density);    // Creates a new rectangle physics body with generic parameters
 213 PHYSACDEF PhysicsBody CreatePhysicsBodyPolygon(Vector2 pos, float radius, int sides, float density);        // Creates a new polygon physics body with generic parameters
 214 PHYSACDEF void DestroyPhysicsBody(PhysicsBody body);                                                        // Destroy a physics body
 215
 216 // Physic body forces
 217 PHYSACDEF void PhysicsAddForce(PhysicsBody body, Vector2 force);                                            // Adds a force to a physics body
 218 PHYSACDEF void PhysicsAddTorque(PhysicsBody body, float amount);                                            // Adds an angular force to a physics body
 219 PHYSACDEF void PhysicsShatter(PhysicsBody body, Vector2 position, float force);                             // Shatters a polygon shape physics body to little physics bodies with explosion force
 220 PHYSACDEF void SetPhysicsBodyRotation(PhysicsBody body, float radians);                                     // Sets physics body shape transform based on radians parameter
 221
 222 // Query physics info
 223 PHYSACDEF PhysicsBody GetPhysicsBody(int index);                                                            // Returns a physics body of the bodies pool at a specific index
 224 PHYSACDEF int GetPhysicsBodiesCount(void);                                                                  // Returns the current amount of created physics bodies
 225 PHYSACDEF int GetPhysicsShapeType(int index);                                                               // Returns the physics body shape type (PHYSICS_CIRCLE or PHYSICS_POLYGON)
 226 PHYSACDEF int GetPhysicsShapeVerticesCount(int index);                                                      // Returns the amount of vertices of a physics body shape
 227 PHYSACDEF Vector2 GetPhysicsShapeVertex(PhysicsBody body, int vertex);                                      // Returns transformed position of a body shape (body position + vertex transformed position)
 228
 229 #if defined(__cplusplus)
 230 }
 231 #endif
 232
 233 #endif // PHYSAC_H
 234
 235 /***********************************************************************************
 236 *
 237 *   PHYSAC IMPLEMENTATION
 238 *
 239 ************************************************************************************/
 240
 241 #if defined(PHYSAC_IMPLEMENTATION)
 242
 243 // Support TRACELOG macros
 244 #if defined(PHYSAC_DEBUG)
 245     #include <stdio.h>              // Required for: printf()
 246     #define TRACELOG(...) printf(__VA_ARGS__)
 247 #else
 248     #define TRACELOG(...) (void)0;
 249 #endif
 250
 251 #include <stdlib.h>                 // Required for: malloc(), calloc(), free()
 252 #include <math.h>                   // Required for: cosf(), sinf(), fabs(), sqrtf()
 253
 254 #if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
 255     // Time management functionality
 256     #include <time.h>                   // Required for: time(), clock_gettime()
 257     #if defined(_WIN32)
 258         // Functions required to query time on Windows
 259         int __stdcall QueryPerformanceCounter(unsigned long long int *lpPerformanceCount);
 260         int __stdcall QueryPerformanceFrequency(unsigned long long int *lpFrequency);
 261     #endif
 262     #if defined(__linux__) || defined(__FreeBSD__)
 263         #if _POSIX_C_SOURCE < 199309L
 264             #undef _POSIX_C_SOURCE
 265             #define _POSIX_C_SOURCE 199309L // Required for CLOCK_MONOTONIC if compiled with c99 without gnu ext.
 266         #endif
 267         #include <sys/time.h>           // Required for: timespec
 268     #endif
 269     #if defined(__APPLE__)              // macOS also defines __MACH__
 270         #include <mach/mach_time.h>     // Required for: mach_absolute_time()
 271     #endif
 272 #endif
 273
 274 // NOTE: MSVC C++ compiler does not support compound literals (C99 feature)
 275 // Plain structures in C++ (without constructors) can be initialized from { } initializers.
 276 #if defined(__cplusplus)
 277     #define CLITERAL(type)      type
 278 #else
 279     #define CLITERAL(type)      (type)
 280 #endif
 281
 282 //----------------------------------------------------------------------------------
 283 // Defines and Macros
 284 //----------------------------------------------------------------------------------
 285 #define PHYSAC_MIN(a,b)         (((a)<(b))?(a):(b))
 286 #define PHYSAC_MAX(a,b)         (((a)>(b))?(a):(b))
 287 #define PHYSAC_FLT_MAX          3.402823466e+38f
 288 #define PHYSAC_EPSILON          0.000001f
 289 #define PHYSAC_K                1.0f/3.0f
 290 #define PHYSAC_VECTOR_ZERO      CLITERAL(Vector2){ 0.0f, 0.0f }
 291
 292 //----------------------------------------------------------------------------------
 293 // Global Variables Definition
 294 //----------------------------------------------------------------------------------
 295 static double deltaTime = 1.0/60.0/10.0 * 1000;             // Delta time in milliseconds used for physics steps
 296
 297 #if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
 298 // Time measure variables
 299 static double baseClockTicks = 0.0;                         // Offset clock ticks for MONOTONIC clock
 300 static unsigned long long int frequency = 0;                // Hi-res clock frequency
 301 static double startTime = 0.0;                              // Start time in milliseconds
 302 static double currentTime = 0.0;                            // Current time in milliseconds
 303 #endif
 304
 305 // Physics system configuration
 306 static PhysicsBody bodies[PHYSAC_MAX_BODIES];               // Physics bodies pointers array
 307 static unsigned int physicsBodiesCount = 0;                 // Physics world current bodies counter
 308 static PhysicsManifold contacts[PHYSAC_MAX_MANIFOLDS];      // Physics bodies pointers array
 309 static unsigned int physicsManifoldsCount = 0;              // Physics world current manifolds counter
 310
 311 static Vector2 gravityForce = { 0.0f, 9.81f };              // Physics world gravity force
 312
 313 // Utilities variables
 314 static unsigned int usedMemory = 0;                         // Total allocated dynamic memory
 315
 316 //----------------------------------------------------------------------------------
 317 // Module Internal Functions Declaration
 318 //----------------------------------------------------------------------------------
 319 #if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
 320 // Timming measure functions
 321 static void InitTimer(void);                                                                                // Initializes hi-resolution MONOTONIC timer
 322 static unsigned long long int GetClockTicks(void);                                                          // Get hi-res MONOTONIC time measure in mseconds
 323 static double GetCurrentTime(void);                                                                         // Get current time measure in milliseconds
 324 #endif
 325
 326 static void UpdatePhysicsStep(void);                                                                        // Update physics step (dynamics, collisions and position corrections)
 327
 328 static int FindAvailableBodyIndex();                                                                        // Finds a valid index for a new physics body initialization
 329 static int FindAvailableManifoldIndex();                                                                    // Finds a valid index for a new manifold initialization
 330 static PhysicsVertexData CreateDefaultPolygon(float radius, int sides);                                     // Creates a random polygon shape with max vertex distance from polygon pivot
 331 static PhysicsVertexData CreateRectanglePolygon(Vector2 pos, Vector2 size);                                 // Creates a rectangle polygon shape based on a min and max positions
 332
 333 static void InitializePhysicsManifolds(PhysicsManifold manifold);                                           // Initializes physics manifolds to solve collisions
 334 static PhysicsManifold CreatePhysicsManifold(PhysicsBody a, PhysicsBody b);                                 // Creates a new physics manifold to solve collision
 335 static void DestroyPhysicsManifold(PhysicsManifold manifold);                                               // Unitializes and destroys a physics manifold
 336
 337 static void SolvePhysicsManifold(PhysicsManifold manifold);                                                 // Solves a created physics manifold between two physics bodies
 338 static void SolveCircleToCircle(PhysicsManifold manifold);                                                  // Solves collision between two circle shape physics bodies
 339 static void SolveCircleToPolygon(PhysicsManifold manifold);                                                 // Solves collision between a circle to a polygon shape physics bodies
 340 static void SolvePolygonToCircle(PhysicsManifold manifold);                                                 // Solves collision between a polygon to a circle shape physics bodies
 341 static void SolvePolygonToPolygon(PhysicsManifold manifold);                                                // Solves collision between two polygons shape physics bodies
 342 static void IntegratePhysicsForces(PhysicsBody body);                                                       // Integrates physics forces into velocity
 343 static void IntegratePhysicsVelocity(PhysicsBody body);                                                     // Integrates physics velocity into position and forces
 344 static void IntegratePhysicsImpulses(PhysicsManifold manifold);                                             // Integrates physics collisions impulses to solve collisions
 345 static void CorrectPhysicsPositions(PhysicsManifold manifold);                                              // Corrects physics bodies positions based on manifolds collision information
 346 static void FindIncidentFace(Vector2 *v0, Vector2 *v1, PhysicsShape ref, PhysicsShape inc, int index);      // Finds two polygon shapes incident face
 347 static float FindAxisLeastPenetration(int *faceIndex, PhysicsShape shapeA, PhysicsShape shapeB);            // Finds polygon shapes axis least penetration
 348
 349 // Math required functions
 350 static Vector2 MathVector2Product(Vector2 vector, float value);                                             // Returns the product of a vector and a value
 351 static float MathVector2CrossProduct(Vector2 v1, Vector2 v2);                                               // Returns the cross product of two vectors
 352 static float MathVector2SqrLen(Vector2 vector);                                                             // Returns the len square root of a vector
 353 static float MathVector2DotProduct(Vector2 v1, Vector2 v2);                                                 // Returns the dot product of two vectors
 354 static inline float MathVector2SqrDistance(Vector2 v1, Vector2 v2);                                         // Returns the square root of distance between two vectors
 355 static void MathVector2Normalize(Vector2 *vector);                                                          // Returns the normalized values of a vector
 356 static Vector2 MathVector2Add(Vector2 v1, Vector2 v2);                                                      // Returns the sum of two given vectors
 357 static Vector2 MathVector2Subtract(Vector2 v1, Vector2 v2);                                                 // Returns the subtract of two given vectors
 358 static Matrix2x2 MathMatFromRadians(float radians);                                                         // Returns a matrix 2x2 from a given radians value
 359 static inline Matrix2x2 MathMatTranspose(Matrix2x2 matrix);                                                 // Returns the transpose of a given matrix 2x2
 360 static inline Vector2 MathMatVector2Product(Matrix2x2 matrix, Vector2 vector);                              // Returns product between matrix 2x2 and vector
 361 static int MathVector2Clip(Vector2 normal, Vector2 *faceA, Vector2 *faceB, float clip);                     // Returns clipping value based on a normal and two faces
 362 static Vector2 MathTriangleBarycenter(Vector2 v1, Vector2 v2, Vector2 v3);                                  // Returns the barycenter of a triangle given by 3 points
 363
 364 //----------------------------------------------------------------------------------
 365 // Module Functions Definition
 366 //----------------------------------------------------------------------------------
 367
 368 // Initializes physics values, pointers and creates physics loop thread
 369 PHYSACDEF void InitPhysics(void)
 370 {
 371 #if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
 372     // Initialize high resolution timer
 373     InitTimer();
 374 #endif
 375
 376     TRACELOG("[PHYSAC] Physics module initialized successfully\n");
 377 }
 378
 379 // Sets physics global gravity force
 380 PHYSACDEF void SetPhysicsGravity(float x, float y)
 381 {
 382     gravityForce.x = x;
 383     gravityForce.y = y;
 384 }
 385
 386 // Creates a new circle physics body with generic parameters
 387 PHYSACDEF PhysicsBody CreatePhysicsBodyCircle(Vector2 pos, float radius, float density)
 388 {
 389     PhysicsBody body = CreatePhysicsBodyPolygon(pos, radius, PHYSAC_DEFAULT_CIRCLE_VERTICES, density);
 390     return body;
 391 }
 392
 393 // Creates a new rectangle physics body with generic parameters
 394 PHYSACDEF PhysicsBody CreatePhysicsBodyRectangle(Vector2 pos, float width, float height, float density)
 395 {
 396     // NOTE: Make sure body data is initialized to 0
 397     PhysicsBody body = (PhysicsBody)PHYSAC_CALLOC(sizeof(PhysicsBodyData), 1);
 398     usedMemory += sizeof(PhysicsBodyData);
 399
 400     int id = FindAvailableBodyIndex();
 401     if (id != -1)
 402     {
 403         // Initialize new body with generic values
 404         body->id = id;
 405         body->enabled = true;
 406         body->position = pos;
 407         body->shape.type = PHYSICS_POLYGON;
 408         body->shape.body = body;
 409         body->shape.transform = MathMatFromRadians(0.0f);
 410         body->shape.vertexData = CreateRectanglePolygon(pos, CLITERAL(Vector2){ width, height });
 411
 412         // Calculate centroid and moment of inertia
 413         Vector2 center = { 0.0f, 0.0f };
 414         float area = 0.0f;
 415         float inertia = 0.0f;
 416
 417         for (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++)
 418         {
 419             // Triangle vertices, third vertex implied as (0, 0)
 420             Vector2 p1 = body->shape.vertexData.positions[i];
 421             unsigned int nextIndex = (((i + 1) < body->shape.vertexData.vertexCount) ? (i + 1) : 0);
 422             Vector2 p2 = body->shape.vertexData.positions[nextIndex];
 423
 424             float D = MathVector2CrossProduct(p1, p2);
 425             float triangleArea = D/2;
 426
 427             area += triangleArea;
 428
 429             // Use area to weight the centroid average, not just vertex position
 430             center.x += triangleArea*PHYSAC_K*(p1.x + p2.x);
 431             center.y += triangleArea*PHYSAC_K*(p1.y + p2.y);
 432
 433             float intx2 = p1.x*p1.x + p2.x*p1.x + p2.x*p2.x;
 434             float inty2 = p1.y*p1.y + p2.y*p1.y + p2.y*p2.y;
 435             inertia += (0.25f*PHYSAC_K*D)*(intx2 + inty2);
 436         }
 437
 438         center.x *= 1.0f/area;
 439         center.y *= 1.0f/area;
 440
 441         // Translate vertices to centroid (make the centroid (0, 0) for the polygon in model space)
 442         // Note: this is not really necessary
 443         for (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++)
 444         {
 445             body->shape.vertexData.positions[i].x -= center.x;
 446             body->shape.vertexData.positions[i].y -= center.y;
 447         }
 448
 449         body->mass = density*area;
 450         body->inverseMass = ((body->mass != 0.0f) ? 1.0f/body->mass : 0.0f);
 451         body->inertia = density*inertia;
 452         body->inverseInertia = ((body->inertia != 0.0f) ? 1.0f/body->inertia : 0.0f);
 453         body->staticFriction = 0.4f;
 454         body->dynamicFriction = 0.2f;
 455         body->restitution = 0.0f;
 456         body->useGravity = true;
 457         body->isGrounded = false;
 458         body->freezeOrient = false;
 459
 460         // Add new body to bodies pointers array and update bodies count
 461         bodies[physicsBodiesCount] = body;
 462         physicsBodiesCount++;
 463
 464         TRACELOG("[PHYSAC] Physic body created successfully (id: %i)\n", body->id);
 465     }
 466     else TRACELOG("[PHYSAC] Physic body could not be created, PHYSAC_MAX_BODIES reached\n");
 467
 468     return body;
 469 }
 470
 471 // Creates a new polygon physics body with generic parameters
 472 PHYSACDEF PhysicsBody CreatePhysicsBodyPolygon(Vector2 pos, float radius, int sides, float density)
 473 {
 474     PhysicsBody body = (PhysicsBody)PHYSAC_MALLOC(sizeof(PhysicsBodyData));
 475     usedMemory += sizeof(PhysicsBodyData);
 476
 477     int id = FindAvailableBodyIndex();
 478     if (id != -1)
 479     {
 480         // Initialize new body with generic values
 481         body->id = id;
 482         body->enabled = true;
 483         body->position = pos;
 484         body->velocity = PHYSAC_VECTOR_ZERO;
 485         body->force = PHYSAC_VECTOR_ZERO;
 486         body->angularVelocity = 0.0f;
 487         body->torque = 0.0f;
 488         body->orient = 0.0f;
 489         body->shape.type = PHYSICS_POLYGON;
 490         body->shape.body = body;
 491         body->shape.transform = MathMatFromRadians(0.0f);
 492         body->shape.vertexData = CreateDefaultPolygon(radius, sides);
 493
 494         // Calculate centroid and moment of inertia
 495         Vector2 center = { 0.0f, 0.0f };
 496         float area = 0.0f;
 497         float inertia = 0.0f;
 498
 499         for (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++)
 500         {
 501             // Triangle vertices, third vertex implied as (0, 0)
 502             Vector2 position1 = body->shape.vertexData.positions[i];
 503             unsigned int nextIndex = (((i + 1) < body->shape.vertexData.vertexCount) ? (i + 1) : 0);
 504             Vector2 position2 = body->shape.vertexData.positions[nextIndex];
 505
 506             float cross = MathVector2CrossProduct(position1, position2);
 507             float triangleArea = cross/2;
 508
 509             area += triangleArea;
 510
 511             // Use area to weight the centroid average, not just vertex position
 512             center.x += triangleArea*PHYSAC_K*(position1.x + position2.x);
 513             center.y += triangleArea*PHYSAC_K*(position1.y + position2.y);
 514
 515             float intx2 = position1.x*position1.x + position2.x*position1.x + position2.x*position2.x;
 516             float inty2 = position1.y*position1.y + position2.y*position1.y + position2.y*position2.y;
 517             inertia += (0.25f*PHYSAC_K*cross)*(intx2 + inty2);
 518         }
 519
 520         center.x *= 1.0f/area;
 521         center.y *= 1.0f/area;
 522
 523         // Translate vertices to centroid (make the centroid (0, 0) for the polygon in model space)
 524         // Note: this is not really necessary
 525         for (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++)
 526         {
 527             body->shape.vertexData.positions[i].x -= center.x;
 528             body->shape.vertexData.positions[i].y -= center.y;
 529         }
 530
 531         body->mass = density*area;
 532         body->inverseMass = ((body->mass != 0.0f) ? 1.0f/body->mass : 0.0f);
 533         body->inertia = density*inertia;
 534         body->inverseInertia = ((body->inertia != 0.0f) ? 1.0f/body->inertia : 0.0f);
 535         body->staticFriction = 0.4f;
 536         body->dynamicFriction = 0.2f;
 537         body->restitution = 0.0f;
 538         body->useGravity = true;
 539         body->isGrounded = false;
 540         body->freezeOrient = false;
 541
 542         // Add new body to bodies pointers array and update bodies count
 543         bodies[physicsBodiesCount] = body;
 544         physicsBodiesCount++;
 545
 546         TRACELOG("[PHYSAC] Physic body created successfully (id: %i)\n", body->id);
 547     }
 548     else TRACELOG("[PHYSAC] Physics body could not be created, PHYSAC_MAX_BODIES reached\n");
 549
 550     return body;
 551 }
 552
 553 // Adds a force to a physics body
 554 PHYSACDEF void PhysicsAddForce(PhysicsBody body, Vector2 force)
 555 {
 556     if (body != NULL) body->force = MathVector2Add(body->force, force);
 557 }
 558
 559 // Adds an angular force to a physics body
 560 PHYSACDEF void PhysicsAddTorque(PhysicsBody body, float amount)
 561 {
 562     if (body != NULL) body->torque += amount;
 563 }
 564
 565 // Shatters a polygon shape physics body to little physics bodies with explosion force
 566 PHYSACDEF void PhysicsShatter(PhysicsBody body, Vector2 position, float force)
 567 {
 568     if (body != NULL)
 569     {
 570         if (body->shape.type == PHYSICS_POLYGON)
 571         {
 572             PhysicsVertexData vertexData = body->shape.vertexData;
 573             bool collision = false;
 574
 575             for (unsigned int i = 0; i < vertexData.vertexCount; i++)
 576             {
 577                 Vector2 positionA = body->position;
 578                 Vector2 positionB = MathMatVector2Product(body->shape.transform, MathVector2Add(body->position, vertexData.positions[i]));
 579                 unsigned int nextIndex = (((i + 1) < vertexData.vertexCount) ? (i + 1) : 0);
 580                 Vector2 positionC = MathMatVector2Product(body->shape.transform, MathVector2Add(body->position, vertexData.positions[nextIndex]));
 581
 582                 // Check collision between each triangle
 583                 float alpha = ((positionB.y - positionC.y)*(position.x - positionC.x) + (positionC.x - positionB.x)*(position.y - positionC.y))/
 584                               ((positionB.y - positionC.y)*(positionA.x - positionC.x) + (positionC.x - positionB.x)*(positionA.y - positionC.y));
 585
 586                 float beta = ((positionC.y - positionA.y)*(position.x - positionC.x) + (positionA.x - positionC.x)*(position.y - positionC.y))/
 587                              ((positionB.y - positionC.y)*(positionA.x - positionC.x) + (positionC.x - positionB.x)*(positionA.y - positionC.y));
 588
 589                 float gamma = 1.0f - alpha - beta;
 590
 591                 if ((alpha > 0.0f) && (beta > 0.0f) & (gamma > 0.0f))
 592                 {
 593                     collision = true;
 594                     break;
 595                 }
 596             }
 597
 598             if (collision)
 599             {
 600                 int count = vertexData.vertexCount;
 601                 Vector2 bodyPos = body->position;
 602                 Vector2 *vertices = (Vector2 *)PHYSAC_MALLOC(sizeof(Vector2)*count);
 603                 Matrix2x2 trans = body->shape.transform;
 604                 for (int i = 0; i < count; i++) vertices[i] = vertexData.positions[i];
 605
 606                 // Destroy shattered physics body
 607                 DestroyPhysicsBody(body);
 608
 609                 for (int i = 0; i < count; i++)
 610                 {
 611                     int nextIndex = (((i + 1) < count) ? (i + 1) : 0);
 612                     Vector2 center = MathTriangleBarycenter(vertices[i], vertices[nextIndex], PHYSAC_VECTOR_ZERO);
 613                     center = MathVector2Add(bodyPos, center);
 614                     Vector2 offset = MathVector2Subtract(center, bodyPos);
 615
 616                     PhysicsBody body = CreatePhysicsBodyPolygon(center, 10, 3, 10);     // Create polygon physics body with relevant values
 617
 618                     PhysicsVertexData vertexData = { 0 };
 619                     vertexData.vertexCount = 3;
 620
 621                     vertexData.positions[0] = MathVector2Subtract(vertices[i], offset);
 622                     vertexData.positions[1] = MathVector2Subtract(vertices[nextIndex], offset);
 623                     vertexData.positions[2] = MathVector2Subtract(position, center);
 624
 625                     // Separate vertices to avoid unnecessary physics collisions
 626                     vertexData.positions[0].x *= 0.95f;
 627                     vertexData.positions[0].y *= 0.95f;
 628                     vertexData.positions[1].x *= 0.95f;
 629                     vertexData.positions[1].y *= 0.95f;
 630                     vertexData.positions[2].x *= 0.95f;
 631                     vertexData.positions[2].y *= 0.95f;
 632
 633                     // Calculate polygon faces normals
 634                     for (unsigned int j = 0; j < vertexData.vertexCount; j++)
 635                     {
 636                         unsigned int nextVertex = (((j + 1) < vertexData.vertexCount) ? (j + 1) : 0);
 637                         Vector2 face = MathVector2Subtract(vertexData.positions[nextVertex], vertexData.positions[j]);
 638
 639                         vertexData.normals[j] = CLITERAL(Vector2){ face.y, -face.x };
 640                         MathVector2Normalize(&vertexData.normals[j]);
 641                     }
 642
 643                     // Apply computed vertex data to new physics body shape
 644                     body->shape.vertexData = vertexData;
 645                     body->shape.transform = trans;
 646
 647                     // Calculate centroid and moment of inertia
 648                     center = PHYSAC_VECTOR_ZERO;
 649                     float area = 0.0f;
 650                     float inertia = 0.0f;
 651
 652                     for (unsigned int j = 0; j < body->shape.vertexData.vertexCount; j++)
 653                     {
 654                         // Triangle vertices, third vertex implied as (0, 0)
 655                         Vector2 p1 = body->shape.vertexData.positions[j];
 656                         unsigned int nextVertex = (((j + 1) < body->shape.vertexData.vertexCount) ? (j + 1) : 0);
 657                         Vector2 p2 = body->shape.vertexData.positions[nextVertex];
 658
 659                         float D = MathVector2CrossProduct(p1, p2);
 660                         float triangleArea = D/2;
 661
 662                         area += triangleArea;
 663
 664                         // Use area to weight the centroid average, not just vertex position
 665                         center.x += triangleArea*PHYSAC_K*(p1.x + p2.x);
 666                         center.y += triangleArea*PHYSAC_K*(p1.y + p2.y);
 667
 668                         float intx2 = p1.x*p1.x + p2.x*p1.x + p2.x*p2.x;
 669                         float inty2 = p1.y*p1.y + p2.y*p1.y + p2.y*p2.y;
 670                         inertia += (0.25f*PHYSAC_K*D)*(intx2 + inty2);
 671                     }
 672
 673                     center.x *= 1.0f/area;
 674                     center.y *= 1.0f/area;
 675
 676                     body->mass = area;
 677                     body->inverseMass = ((body->mass != 0.0f) ? 1.0f/body->mass : 0.0f);
 678                     body->inertia = inertia;
 679                     body->inverseInertia = ((body->inertia != 0.0f) ? 1.0f/body->inertia : 0.0f);
 680
 681                     // Calculate explosion force direction
 682                     Vector2 pointA = body->position;
 683                     Vector2 pointB = MathVector2Subtract(vertexData.positions[1], vertexData.positions[0]);
 684                     pointB.x /= 2.0f;
 685                     pointB.y /= 2.0f;
 686                     Vector2 forceDirection = MathVector2Subtract(MathVector2Add(pointA, MathVector2Add(vertexData.positions[0], pointB)), body->position);
 687                     MathVector2Normalize(&forceDirection);
 688                     forceDirection.x *= force;
 689                     forceDirection.y *= force;
 690
 691                     // Apply force to new physics body
 692                     PhysicsAddForce(body, forceDirection);
 693                 }
 694
 695                 PHYSAC_FREE(vertices);
 696             }
 697         }
 698     }
 699     else TRACELOG("[PHYSAC] WARNING: PhysicsShatter: NULL physic body\n");
 700 }
 701
 702 // Returns the current amount of created physics bodies
 703 PHYSACDEF int GetPhysicsBodiesCount(void)
 704 {
 705     return physicsBodiesCount;
 706 }
 707
 708 // Returns a physics body of the bodies pool at a specific index
 709 PHYSACDEF PhysicsBody GetPhysicsBody(int index)
 710 {
 711     PhysicsBody body = NULL;
 712
 713     if (index < (int)physicsBodiesCount)
 714     {
 715         body = bodies[index];
 716
 717         if (body == NULL) TRACELOG("[PHYSAC] WARNING: GetPhysicsBody: NULL physic body\n");
 718     }
 719     else TRACELOG("[PHYSAC] WARNING: Physic body index is out of bounds\n");
 720
 721     return body;
 722 }
 723
 724 // Returns the physics body shape type (PHYSICS_CIRCLE or PHYSICS_POLYGON)
 725 PHYSACDEF int GetPhysicsShapeType(int index)
 726 {
 727     int result = -1;
 728
 729     if (index < (int)physicsBodiesCount)
 730     {
 731         PhysicsBody body = bodies[index];
 732
 733         if (body != NULL) result = body->shape.type;
 734         else TRACELOG("[PHYSAC] WARNING: GetPhysicsShapeType: NULL physic body\n");
 735     }
 736     else TRACELOG("[PHYSAC] WARNING: Physic body index is out of bounds\n");
 737
 738     return result;
 739 }
 740
 741 // Returns the amount of vertices of a physics body shape
 742 PHYSACDEF int GetPhysicsShapeVerticesCount(int index)
 743 {
 744     int result = 0;
 745
 746     if (index < (int)physicsBodiesCount)
 747     {
 748         PhysicsBody body = bodies[index];
 749
 750         if (body != NULL)
 751         {
 752             switch (body->shape.type)
 753             {
 754                 case PHYSICS_CIRCLE: result = PHYSAC_DEFAULT_CIRCLE_VERTICES; break;
 755                 case PHYSICS_POLYGON: result = body->shape.vertexData.vertexCount; break;
 756                 default: break;
 757             }
 758         }
 759         else TRACELOG("[PHYSAC] WARNING: GetPhysicsShapeVerticesCount: NULL physic body\n");
 760     }
 761     else TRACELOG("[PHYSAC] WARNING: Physic body index is out of bounds\n");
 762
 763     return result;
 764 }
 765
 766 // Returns transformed position of a body shape (body position + vertex transformed position)
 767 PHYSACDEF Vector2 GetPhysicsShapeVertex(PhysicsBody body, int vertex)
 768 {
 769     Vector2 position = { 0.0f, 0.0f };
 770
 771     if (body != NULL)
 772     {
 773         switch (body->shape.type)
 774         {
 775             case PHYSICS_CIRCLE:
 776             {
 777                 position.x = body->position.x + cosf(360.0f/PHYSAC_DEFAULT_CIRCLE_VERTICES*vertex*PHYSAC_DEG2RAD)*body->shape.radius;
 778                 position.y = body->position.y + sinf(360.0f/PHYSAC_DEFAULT_CIRCLE_VERTICES*vertex*PHYSAC_DEG2RAD)*body->shape.radius;
 779             } break;
 780             case PHYSICS_POLYGON:
 781             {
 782                 PhysicsVertexData vertexData = body->shape.vertexData;
 783                 position = MathVector2Add(body->position, MathMatVector2Product(body->shape.transform, vertexData.positions[vertex]));
 784             } break;
 785             default: break;
 786         }
 787     }
 788     else TRACELOG("[PHYSAC] WARNING: GetPhysicsShapeVertex: NULL physic body\n");
 789
 790     return position;
 791 }
 792
 793 // Sets physics body shape transform based on radians parameter
 794 PHYSACDEF void SetPhysicsBodyRotation(PhysicsBody body, float radians)
 795 {
 796     if (body != NULL)
 797     {
 798         body->orient = radians;
 799
 800         if (body->shape.type == PHYSICS_POLYGON) body->shape.transform = MathMatFromRadians(radians);
 801     }
 802 }
 803
 804 // Unitializes and destroys a physics body
 805 PHYSACDEF void DestroyPhysicsBody(PhysicsBody body)
 806 {
 807     if (body != NULL)
 808     {
 809         int id = body->id;
 810         int index = -1;
 811
 812         for (unsigned int i = 0; i < physicsBodiesCount; i++)
 813         {
 814             if (bodies[i]->id == id)
 815             {
 816                 index = i;
 817                 break;
 818             }
 819         }
 820
 821         if (index == -1)
 822         {
 823             TRACELOG("[PHYSAC] WARNING: Requested body (id: %i) can not be found\n", id);
 824             return;     // Prevent access to index -1
 825         }
 826
 827         // Free body allocated memory
 828         PHYSAC_FREE(body);
 829         usedMemory -= sizeof(PhysicsBodyData);
 830         bodies[index] = NULL;
 831
 832         // Reorder physics bodies pointers array and its catched index
 833         for (unsigned int i = index; i < physicsBodiesCount; i++)
 834         {
 835             if ((i + 1) < physicsBodiesCount) bodies[i] = bodies[i + 1];
 836         }
 837
 838         // Update physics bodies count
 839         physicsBodiesCount--;
 840
 841         TRACELOG("[PHYSAC] Physic body destroyed successfully (id: %i)\n", id);
 842     }
 843     else TRACELOG("[PHYSAC] WARNING: DestroyPhysicsBody: NULL physic body\n");
 844 }
 845
 846 // Destroys created physics bodies and manifolds and resets global values
 847 PHYSACDEF void ResetPhysics(void)
 848 {
 849     if (physicsBodiesCount > 0)
 850     {
 851         // Unitialize physics bodies dynamic memory allocations
 852         for (int i = physicsBodiesCount - 1; i >= 0; i--)
 853         {
 854             PhysicsBody body = bodies[i];
 855
 856             if (body != NULL)
 857             {
 858                 PHYSAC_FREE(body);
 859                 bodies[i] = NULL;
 860                 usedMemory -= sizeof(PhysicsBodyData);
 861             }
 862         }
 863
 864         physicsBodiesCount = 0;
 865     }
 866
 867     if (physicsManifoldsCount > 0)
 868     {
 869         // Unitialize physics manifolds dynamic memory allocations
 870         for (int i = physicsManifoldsCount - 1; i >= 0; i--)
 871         {
 872             PhysicsManifold manifold = contacts[i];
 873
 874             if (manifold != NULL)
 875             {
 876                 PHYSAC_FREE(manifold);
 877                 contacts[i] = NULL;
 878                 usedMemory -= sizeof(PhysicsManifoldData);
 879             }
 880         }
 881
 882         physicsManifoldsCount = 0;
 883     }
 884
 885     TRACELOG("[PHYSAC] Physics module reseted successfully\n");
 886 }
 887
 888 // Unitializes physics pointers and exits physics loop thread
 889 PHYSACDEF void ClosePhysics(void)
 890 {
 891     // Unitialize physics manifolds dynamic memory allocations
 892     if (physicsManifoldsCount > 0)
 893     {
 894         for (unsigned int i = physicsManifoldsCount - 1; i >= 0; i--)
 895             DestroyPhysicsManifold(contacts[i]);
 896     }
 897
 898     // Unitialize physics bodies dynamic memory allocations
 899     if (physicsBodiesCount > 0)
 900     {
 901         for (unsigned int i = physicsBodiesCount - 1; i >= 0; i--)
 902             DestroyPhysicsBody(bodies[i]);
 903     }
 904
 905     // Trace log info
 906     if ((physicsBodiesCount > 0) || (usedMemory != 0))
 907     {
 908         TRACELOG("[PHYSAC] WARNING: Physics module closed with unallocated bodies (BODIES: %i, MEMORY: %i bytes)\n", physicsBodiesCount, usedMemory);
 909     }
 910     else if ((physicsManifoldsCount > 0) || (usedMemory != 0))
 911     {
 912         TRACELOG("[PHYSAC] WARNING: Pysics module closed with unallocated manifolds (MANIFOLDS: %i, MEMORY: %i bytes)\n", physicsManifoldsCount, usedMemory);
 913     }
 914     else TRACELOG("[PHYSAC] Physics module closed successfully\n");
 915 }
 916
 917 //----------------------------------------------------------------------------------
 918 // Module Internal Functions Definition
 919 //----------------------------------------------------------------------------------
 920 // Finds a valid index for a new physics body initialization
 921 static int FindAvailableBodyIndex()
 922 {
 923     int index = -1;
 924     for (int i = 0; i < PHYSAC_MAX_BODIES; i++)
 925     {
 926         int currentId = i;
 927
 928         // Check if current id already exist in other physics body
 929         for (unsigned int k = 0; k < physicsBodiesCount; k++)
 930         {
 931             if (bodies[k]->id == currentId)
 932             {
 933                 currentId++;
 934                 break;
 935             }
 936         }
 937
 938         // If it is not used, use it as new physics body id
 939         if (currentId == (int)i)
 940         {
 941             index = (int)i;
 942             break;
 943         }
 944     }
 945
 946     return index;
 947 }
 948
 949 // Creates a default polygon shape with max vertex distance from polygon pivot
 950 static PhysicsVertexData CreateDefaultPolygon(float radius, int sides)
 951 {
 952     PhysicsVertexData data = { 0 };
 953     data.vertexCount = sides;
 954
 955     // Calculate polygon vertices positions
 956     for (unsigned int i = 0; i < data.vertexCount; i++)
 957     {
 958         data.positions[i].x = (float)cosf(360.0f/sides*i*PHYSAC_DEG2RAD)*radius;
 959         data.positions[i].y = (float)sinf(360.0f/sides*i*PHYSAC_DEG2RAD)*radius;
 960     }
 961
 962     // Calculate polygon faces normals
 963     for (int i = 0; i < (int)data.vertexCount; i++)
 964     {
 965         int nextIndex = (((i + 1) < sides) ? (i + 1) : 0);
 966         Vector2 face = MathVector2Subtract(data.positions[nextIndex], data.positions[i]);
 967
 968         data.normals[i] = CLITERAL(Vector2){ face.y, -face.x };
 969         MathVector2Normalize(&data.normals[i]);
 970     }
 971
 972     return data;
 973 }
 974
 975 // Creates a rectangle polygon shape based on a min and max positions
 976 static PhysicsVertexData CreateRectanglePolygon(Vector2 pos, Vector2 size)
 977 {
 978     PhysicsVertexData data = { 0 };
 979     data.vertexCount = 4;
 980
 981     // Calculate polygon vertices positions
 982     data.positions[0] = CLITERAL(Vector2){ pos.x + size.x/2, pos.y - size.y/2 };
 983     data.positions[1] = CLITERAL(Vector2){ pos.x + size.x/2, pos.y + size.y/2 };
 984     data.positions[2] = CLITERAL(Vector2){ pos.x - size.x/2, pos.y + size.y/2 };
 985     data.positions[3] = CLITERAL(Vector2){ pos.x - size.x/2, pos.y - size.y/2 };
 986
 987     // Calculate polygon faces normals
 988     for (unsigned int i = 0; i < data.vertexCount; i++)
 989     {
 990         int nextIndex = (((i + 1) < data.vertexCount) ? (i + 1) : 0);
 991         Vector2 face = MathVector2Subtract(data.positions[nextIndex], data.positions[i]);
 992
 993         data.normals[i] = CLITERAL(Vector2){ face.y, -face.x };
 994         MathVector2Normalize(&data.normals[i]);
 995     }
 996
 997     return data;
 998 }
 999
1000 // Update physics step (dynamics, collisions and position corrections)
1001 void UpdatePhysicsStep(void)
1002 {
1003     // Clear previous generated collisions information
1004     for (int i = (int)physicsManifoldsCount - 1; i >= 0; i--)
1005     {
1006         PhysicsManifold manifold = contacts[i];
1007         if (manifold != NULL) DestroyPhysicsManifold(manifold);
1008     }
1009
1010     // Reset physics bodies grounded state
1011     for (unsigned int i = 0; i < physicsBodiesCount; i++)
1012     {
1013         PhysicsBody body = bodies[i];
1014         body->isGrounded = false;
1015     }
1016
1017     // Generate new collision information
1018     for (unsigned int i = 0; i < physicsBodiesCount; i++)
1019     {
1020         PhysicsBody bodyA = bodies[i];
1021
1022         if (bodyA != NULL)
1023         {
1024             for (unsigned int j = i + 1; j < physicsBodiesCount; j++)
1025             {
1026                 PhysicsBody bodyB = bodies[j];
1027
1028                 if (bodyB != NULL)
1029                 {
1030                     if ((bodyA->inverseMass == 0) && (bodyB->inverseMass == 0)) continue;
1031
1032                     PhysicsManifold manifold = CreatePhysicsManifold(bodyA, bodyB);
1033                     SolvePhysicsManifold(manifold);
1034
1035                     if (manifold->contactsCount > 0)
1036                     {
1037                         // Create a new manifold with same information as previously solved manifold and add it to the manifolds pool last slot
1038                         PhysicsManifold manifold = CreatePhysicsManifold(bodyA, bodyB);
1039                         manifold->penetration = manifold->penetration;
1040                         manifold->normal = manifold->normal;
1041                         manifold->contacts[0] = manifold->contacts[0];
1042                         manifold->contacts[1] = manifold->contacts[1];
1043                         manifold->contactsCount = manifold->contactsCount;
1044                         manifold->restitution = manifold->restitution;
1045                         manifold->dynamicFriction = manifold->dynamicFriction;
1046                         manifold->staticFriction = manifold->staticFriction;
1047                     }
1048                 }
1049             }
1050         }
1051     }
1052
1053     // Integrate forces to physics bodies
1054     for (unsigned int i = 0; i < physicsBodiesCount; i++)
1055     {
1056         PhysicsBody body = bodies[i];
1057         if (body != NULL) IntegratePhysicsForces(body);
1058     }
1059
1060     // Initialize physics manifolds to solve collisions
1061     for (unsigned int i = 0; i < physicsManifoldsCount; i++)
1062     {
1063         PhysicsManifold manifold = contacts[i];
1064         if (manifold != NULL) InitializePhysicsManifolds(manifold);
1065     }
1066
1067     // Integrate physics collisions impulses to solve collisions
1068     for (unsigned int i = 0; i < PHYSAC_COLLISION_ITERATIONS; i++)
1069     {
1070         for (unsigned int j = 0; j < physicsManifoldsCount; j++)
1071         {
1072             PhysicsManifold manifold = contacts[i];
1073             if (manifold != NULL) IntegratePhysicsImpulses(manifold);
1074         }
1075     }
1076
1077     // Integrate velocity to physics bodies
1078     for (unsigned int i = 0; i < physicsBodiesCount; i++)
1079     {
1080         PhysicsBody body = bodies[i];
1081         if (body != NULL) IntegratePhysicsVelocity(body);
1082     }
1083
1084     // Correct physics bodies positions based on manifolds collision information
1085     for (unsigned int i = 0; i < physicsManifoldsCount; i++)
1086     {
1087         PhysicsManifold manifold = contacts[i];
1088         if (manifold != NULL) CorrectPhysicsPositions(manifold);
1089     }
1090
1091     // Clear physics bodies forces
1092     for (unsigned int i = 0; i < physicsBodiesCount; i++)
1093     {
1094         PhysicsBody body = bodies[i];
1095         if (body != NULL)
1096         {
1097             body->force = PHYSAC_VECTOR_ZERO;
1098             body->torque = 0.0f;
1099         }
1100     }
1101 }
1102
1103 // Update physics system
1104 // Physics steps are launched at a fixed time step if enabled
1105 PHYSACDEF void UpdatePhysics(void)
1106 {
1107 #if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
1108     static double deltaTimeAccumulator = 0.0;
1109
1110     // Calculate current time (ms)
1111     currentTime = GetCurrentTime();
1112
1113     // Calculate current delta time (ms)
1114     const double delta = currentTime - startTime;
1115
1116     // Store the time elapsed since the last frame began
1117     deltaTimeAccumulator += delta;
1118
1119     // Fixed time stepping loop
1120     while (deltaTimeAccumulator >= deltaTime)
1121     {
1122         UpdatePhysicsStep();
1123         deltaTimeAccumulator -= deltaTime;
1124     }
1125
1126     // Record the starting of this frame
1127     startTime = currentTime;
1128 #else
1129     UpdatePhysicsStep();
1130 #endif
1131 }
1132
1133 PHYSACDEF void SetPhysicsTimeStep(double delta)
1134 {
1135     deltaTime = delta;
1136 }
1137
1138 // Finds a valid index for a new manifold initialization
1139 static int FindAvailableManifoldIndex()
1140 {
1141     int index = -1;
1142     for (int i = 0; i < PHYSAC_MAX_MANIFOLDS; i++)
1143     {
1144         int currentId = i;
1145
1146         // Check if current id already exist in other physics body
1147         for (unsigned int k = 0; k < physicsManifoldsCount; k++)
1148         {
1149             if (contacts[k]->id == currentId)
1150             {
1151                 currentId++;
1152                 break;
1153             }
1154         }
1155
1156         // If it is not used, use it as new physics body id
1157         if (currentId == i)
1158         {
1159             index = i;
1160             break;
1161         }
1162     }
1163
1164     return index;
1165 }
1166
1167 // Creates a new physics manifold to solve collision
1168 static PhysicsManifold CreatePhysicsManifold(PhysicsBody a, PhysicsBody b)
1169 {
1170     PhysicsManifold manifold = (PhysicsManifold)PHYSAC_MALLOC(sizeof(PhysicsManifoldData));
1171     usedMemory += sizeof(PhysicsManifoldData);
1172
1173     int id = FindAvailableManifoldIndex();
1174     if (id != -1)
1175     {
1176         // Initialize new manifold with generic values
1177         manifold->id = id;
1178         manifold->bodyA = a;
1179         manifold->bodyB = b;
1180         manifold->penetration = 0;
1181         manifold->normal = PHYSAC_VECTOR_ZERO;
1182         manifold->contacts[0] = PHYSAC_VECTOR_ZERO;
1183         manifold->contacts[1] = PHYSAC_VECTOR_ZERO;
1184         manifold->contactsCount = 0;
1185         manifold->restitution = 0.0f;
1186         manifold->dynamicFriction = 0.0f;
1187         manifold->staticFriction = 0.0f;
1188
1189         // Add new body to bodies pointers array and update bodies count
1190         contacts[physicsManifoldsCount] = manifold;
1191         physicsManifoldsCount++;
1192     }
1193     else TRACELOG("[PHYSAC] Physic manifold could not be created, PHYSAC_MAX_MANIFOLDS reached\n");
1194
1195     return manifold;
1196 }
1197
1198 // Unitializes and destroys a physics manifold
1199 static void DestroyPhysicsManifold(PhysicsManifold manifold)
1200 {
1201     if (manifold != NULL)
1202     {
1203         int id = manifold->id;
1204         int index = -1;
1205
1206         for (unsigned int i = 0; i < physicsManifoldsCount; i++)
1207         {
1208             if (contacts[i]->id == id)
1209             {
1210                 index = i;
1211                 break;
1212             }
1213         }
1214
1215         if (index == -1) return;     // Prevent access to index -1
1216
1217         // Free manifold allocated memory
1218         PHYSAC_FREE(manifold);
1219         usedMemory -= sizeof(PhysicsManifoldData);
1220         contacts[index] = NULL;
1221
1222         // Reorder physics manifolds pointers array and its catched index
1223         for (unsigned int i = index; i < physicsManifoldsCount; i++)
1224         {
1225             if ((i + 1) < physicsManifoldsCount) contacts[i] = contacts[i + 1];
1226         }
1227
1228         // Update physics manifolds count
1229         physicsManifoldsCount--;
1230     }
1231     else TRACELOG("[PHYSAC] WARNING: DestroyPhysicsManifold: NULL physic manifold\n");
1232 }
1233
1234 // Solves a created physics manifold between two physics bodies
1235 static void SolvePhysicsManifold(PhysicsManifold manifold)
1236 {
1237     switch (manifold->bodyA->shape.type)
1238     {
1239         case PHYSICS_CIRCLE:
1240         {
1241             switch (manifold->bodyB->shape.type)
1242             {
1243                 case PHYSICS_CIRCLE: SolveCircleToCircle(manifold); break;
1244                 case PHYSICS_POLYGON: SolveCircleToPolygon(manifold); break;
1245                 default: break;
1246             }
1247         } break;
1248         case PHYSICS_POLYGON:
1249         {
1250             switch (manifold->bodyB->shape.type)
1251             {
1252                 case PHYSICS_CIRCLE: SolvePolygonToCircle(manifold); break;
1253                 case PHYSICS_POLYGON: SolvePolygonToPolygon(manifold); break;
1254                 default: break;
1255             }
1256         } break;
1257         default: break;
1258     }
1259
1260     // Update physics body grounded state if normal direction is down and grounded state is not set yet in previous manifolds
1261     if (!manifold->bodyB->isGrounded) manifold->bodyB->isGrounded = (manifold->normal.y < 0);
1262 }
1263
1264 // Solves collision between two circle shape physics bodies
1265 static void SolveCircleToCircle(PhysicsManifold manifold)
1266 {
1267     PhysicsBody bodyA = manifold->bodyA;
1268     PhysicsBody bodyB = manifold->bodyB;
1269
1270     if ((bodyA == NULL) || (bodyB == NULL)) return;
1271
1272     // Calculate translational vector, which is normal
1273     Vector2 normal = MathVector2Subtract(bodyB->position, bodyA->position);
1274
1275     float distSqr = MathVector2SqrLen(normal);
1276     float radius = bodyA->shape.radius + bodyB->shape.radius;
1277
1278     // Check if circles are not in contact
1279     if (distSqr >= radius*radius)
1280     {
1281         manifold->contactsCount = 0;
1282         return;
1283     }
1284
1285     float distance = sqrtf(distSqr);
1286     manifold->contactsCount = 1;
1287
1288     if (distance == 0.0f)
1289     {
1290         manifold->penetration = bodyA->shape.radius;
1291         manifold->normal = CLITERAL(Vector2){ 1.0f, 0.0f };
1292         manifold->contacts[0] = bodyA->position;
1293     }
1294     else
1295     {
1296         manifold->penetration = radius - distance;
1297         manifold->normal = CLITERAL(Vector2){ normal.x/distance, normal.y/distance }; // Faster than using MathVector2Normalize() due to sqrt is already performed
1298         manifold->contacts[0] = CLITERAL(Vector2){ manifold->normal.x*bodyA->shape.radius + bodyA->position.x, manifold->normal.y*bodyA->shape.radius + bodyA->position.y };
1299     }
1300
1301     // Update physics body grounded state if normal direction is down
1302     if (!bodyA->isGrounded) bodyA->isGrounded = (manifold->normal.y < 0);
1303 }
1304
1305 // Solves collision between a circle to a polygon shape physics bodies
1306 static void SolveCircleToPolygon(PhysicsManifold manifold)
1307 {
1308     PhysicsBody bodyA = manifold->bodyA;
1309     PhysicsBody bodyB = manifold->bodyB;
1310
1311     if ((bodyA == NULL) || (bodyB == NULL)) return;
1312
1313     manifold->contactsCount = 0;
1314
1315     // Transform circle center to polygon transform space
1316     Vector2 center = bodyA->position;
1317     center = MathMatVector2Product(MathMatTranspose(bodyB->shape.transform), MathVector2Subtract(center, bodyB->position));
1318
1319     // Find edge with minimum penetration
1320     // It is the same concept as using support points in SolvePolygonToPolygon
1321     float separation = -PHYSAC_FLT_MAX;
1322     int faceNormal = 0;
1323     PhysicsVertexData vertexData = bodyB->shape.vertexData;
1324
1325     for (unsigned int i = 0; i < vertexData.vertexCount; i++)
1326     {
1327         float currentSeparation = MathVector2DotProduct(vertexData.normals[i], MathVector2Subtract(center, vertexData.positions[i]));
1328
1329         if (currentSeparation > bodyA->shape.radius) return;
1330
1331         if (currentSeparation > separation)
1332         {
1333             separation = currentSeparation;
1334             faceNormal = i;
1335         }
1336     }
1337
1338     // Grab face's vertices
1339     Vector2 v1 = vertexData.positions[faceNormal];
1340     int nextIndex = (((faceNormal + 1) < (int)vertexData.vertexCount) ? (faceNormal + 1) : 0);
1341     Vector2 v2 = vertexData.positions[nextIndex];
1342
1343     // Check to see if center is within polygon
1344     if (separation < PHYSAC_EPSILON)
1345     {
1346         manifold->contactsCount = 1;
1347         Vector2 normal = MathMatVector2Product(bodyB->shape.transform, vertexData.normals[faceNormal]);
1348         manifold->normal = CLITERAL(Vector2){ -normal.x, -normal.y };
1349         manifold->contacts[0] = CLITERAL(Vector2){ manifold->normal.x*bodyA->shape.radius + bodyA->position.x, manifold->normal.y*bodyA->shape.radius + bodyA->position.y };
1350         manifold->penetration = bodyA->shape.radius;
1351         return;
1352     }
1353
1354     // Determine which voronoi region of the edge center of circle lies within
1355     float dot1 = MathVector2DotProduct(MathVector2Subtract(center, v1), MathVector2Subtract(v2, v1));
1356     float dot2 = MathVector2DotProduct(MathVector2Subtract(center, v2), MathVector2Subtract(v1, v2));
1357     manifold->penetration = bodyA->shape.radius - separation;
1358
1359     if (dot1 <= 0.0f) // Closest to v1
1360     {
1361         if (MathVector2SqrDistance(center, v1) > bodyA->shape.radius*bodyA->shape.radius) return;
1362
1363         manifold->contactsCount = 1;
1364         Vector2 normal = MathVector2Subtract(v1, center);
1365         normal = MathMatVector2Product(bodyB->shape.transform, normal);
1366         MathVector2Normalize(&normal);
1367         manifold->normal = normal;
1368         v1 = MathMatVector2Product(bodyB->shape.transform, v1);
1369         v1 = MathVector2Add(v1, bodyB->position);
1370         manifold->contacts[0] = v1;
1371     }
1372     else if (dot2 <= 0.0f) // Closest to v2
1373     {
1374         if (MathVector2SqrDistance(center, v2) > bodyA->shape.radius*bodyA->shape.radius) return;
1375
1376         manifold->contactsCount = 1;
1377         Vector2 normal = MathVector2Subtract(v2, center);
1378         v2 = MathMatVector2Product(bodyB->shape.transform, v2);
1379         v2 = MathVector2Add(v2, bodyB->position);
1380         manifold->contacts[0] = v2;
1381         normal = MathMatVector2Product(bodyB->shape.transform, normal);
1382         MathVector2Normalize(&normal);
1383         manifold->normal = normal;
1384     }
1385     else // Closest to face
1386     {
1387         Vector2 normal = vertexData.normals[faceNormal];
1388
1389         if (MathVector2DotProduct(MathVector2Subtract(center, v1), normal) > bodyA->shape.radius) return;
1390
1391         normal = MathMatVector2Product(bodyB->shape.transform, normal);
1392         manifold->normal = CLITERAL(Vector2){ -normal.x, -normal.y };
1393         manifold->contacts[0] = CLITERAL(Vector2){ manifold->normal.x*bodyA->shape.radius + bodyA->position.x, manifold->normal.y*bodyA->shape.radius + bodyA->position.y };
1394         manifold->contactsCount = 1;
1395     }
1396 }
1397
1398 // Solves collision between a polygon to a circle shape physics bodies
1399 static void SolvePolygonToCircle(PhysicsManifold manifold)
1400 {
1401     PhysicsBody bodyA = manifold->bodyA;
1402     PhysicsBody bodyB = manifold->bodyB;
1403
1404     if ((bodyA == NULL) || (bodyB == NULL)) return;
1405
1406     manifold->bodyA = bodyB;
1407     manifold->bodyB = bodyA;
1408     SolveCircleToPolygon(manifold);
1409
1410     manifold->normal.x *= -1.0f;
1411     manifold->normal.y *= -1.0f;
1412 }
1413
1414 // Solves collision between two polygons shape physics bodies
1415 static void SolvePolygonToPolygon(PhysicsManifold manifold)
1416 {
1417     if ((manifold->bodyA == NULL) || (manifold->bodyB == NULL)) return;
1418
1419     PhysicsShape bodyA = manifold->bodyA->shape;
1420     PhysicsShape bodyB = manifold->bodyB->shape;
1421     manifold->contactsCount = 0;
1422
1423     // Check for separating axis with A shape's face planes
1424     int faceA = 0;
1425     float penetrationA = FindAxisLeastPenetration(&faceA, bodyA, bodyB);
1426     if (penetrationA >= 0.0f) return;
1427
1428     // Check for separating axis with B shape's face planes
1429     int faceB = 0;
1430     float penetrationB = FindAxisLeastPenetration(&faceB, bodyB, bodyA);
1431     if (penetrationB >= 0.0f) return;
1432
1433     int referenceIndex = 0;
1434     bool flip = false;  // Always point from A shape to B shape
1435
1436     PhysicsShape refPoly; // Reference
1437     PhysicsShape incPoly; // Incident
1438
1439     // Determine which shape contains reference face
1440     // Checking bias range for penetration
1441     if (penetrationA >= (penetrationB*0.95f + penetrationA*0.01f))
1442     {
1443         refPoly = bodyA;
1444         incPoly = bodyB;
1445         referenceIndex = faceA;
1446     }
1447     else
1448     {
1449         refPoly = bodyB;
1450         incPoly = bodyA;
1451         referenceIndex = faceB;
1452         flip = true;
1453     }
1454
1455     // World space incident face
1456     Vector2 incidentFace[2];
1457     FindIncidentFace(&incidentFace[0], &incidentFace[1], refPoly, incPoly, referenceIndex);
1458
1459     // Setup reference face vertices
1460     PhysicsVertexData refData = refPoly.vertexData;
1461     Vector2 v1 = refData.positions[referenceIndex];
1462     referenceIndex = (((referenceIndex + 1) < (int)refData.vertexCount) ? (referenceIndex + 1) : 0);
1463     Vector2 v2 = refData.positions[referenceIndex];
1464
1465     // Transform vertices to world space
1466     v1 = MathMatVector2Product(refPoly.transform, v1);
1467     v1 = MathVector2Add(v1, refPoly.body->position);
1468     v2 = MathMatVector2Product(refPoly.transform, v2);
1469     v2 = MathVector2Add(v2, refPoly.body->position);
1470
1471     // Calculate reference face side normal in world space
1472     Vector2 sidePlaneNormal = MathVector2Subtract(v2, v1);
1473     MathVector2Normalize(&sidePlaneNormal);
1474
1475     // Orthogonalize
1476     Vector2 refFaceNormal = { sidePlaneNormal.y, -sidePlaneNormal.x };
1477     float refC = MathVector2DotProduct(refFaceNormal, v1);
1478     float negSide = MathVector2DotProduct(sidePlaneNormal, v1)*-1;
1479     float posSide = MathVector2DotProduct(sidePlaneNormal, v2);
1480
1481     // MathVector2Clip incident face to reference face side planes (due to floating point error, possible to not have required points
1482     if (MathVector2Clip(CLITERAL(Vector2){ -sidePlaneNormal.x, -sidePlaneNormal.y }, &incidentFace[0], &incidentFace[1], negSide) < 2) return;
1483     if (MathVector2Clip(sidePlaneNormal, &incidentFace[0], &incidentFace[1], posSide) < 2) return;
1484
1485     // Flip normal if required
1486     manifold->normal = (flip ? CLITERAL(Vector2){ -refFaceNormal.x, -refFaceNormal.y } : refFaceNormal);
1487
1488     // Keep points behind reference face
1489     int currentPoint = 0; // MathVector2Clipped points behind reference face
1490     float separation = MathVector2DotProduct(refFaceNormal, incidentFace[0]) - refC;
1491     if (separation <= 0.0f)
1492     {
1493         manifold->contacts[currentPoint] = incidentFace[0];
1494         manifold->penetration = -separation;
1495         currentPoint++;
1496     }
1497     else manifold->penetration = 0.0f;
1498
1499     separation = MathVector2DotProduct(refFaceNormal, incidentFace[1]) - refC;
1500
1501     if (separation <= 0.0f)
1502     {
1503         manifold->contacts[currentPoint] = incidentFace[1];
1504         manifold->penetration += -separation;
1505         currentPoint++;
1506
1507         // Calculate total penetration average
1508         manifold->penetration /= currentPoint;
1509     }
1510
1511     manifold->contactsCount = currentPoint;
1512 }
1513
1514 // Integrates physics forces into velocity
1515 static void IntegratePhysicsForces(PhysicsBody body)
1516 {
1517     if ((body == NULL) || (body->inverseMass == 0.0f) || !body->enabled) return;
1518
1519     body->velocity.x += (float)((body->force.x*body->inverseMass)*(deltaTime/2.0));
1520     body->velocity.y += (float)((body->force.y*body->inverseMass)*(deltaTime/2.0));
1521
1522     if (body->useGravity)
1523     {
1524         body->velocity.x += (float)(gravityForce.x*(deltaTime/1000/2.0));
1525         body->velocity.y += (float)(gravityForce.y*(deltaTime/1000/2.0));
1526     }
1527
1528     if (!body->freezeOrient) body->angularVelocity += (float)(body->torque*body->inverseInertia*(deltaTime/2.0));
1529 }
1530
1531 // Initializes physics manifolds to solve collisions
1532 static void InitializePhysicsManifolds(PhysicsManifold manifold)
1533 {
1534     PhysicsBody bodyA = manifold->bodyA;
1535     PhysicsBody bodyB = manifold->bodyB;
1536
1537     if ((bodyA == NULL) || (bodyB == NULL)) return;
1538
1539     // Calculate average restitution, static and dynamic friction
1540     manifold->restitution = sqrtf(bodyA->restitution*bodyB->restitution);
1541     manifold->staticFriction = sqrtf(bodyA->staticFriction*bodyB->staticFriction);
1542     manifold->dynamicFriction = sqrtf(bodyA->dynamicFriction*bodyB->dynamicFriction);
1543
1544     for (unsigned int i = 0; i < manifold->contactsCount; i++)
1545     {
1546         // Caculate radius from center of mass to contact
1547         Vector2 radiusA = MathVector2Subtract(manifold->contacts[i], bodyA->position);
1548         Vector2 radiusB = MathVector2Subtract(manifold->contacts[i], bodyB->position);
1549
1550         Vector2 crossA = MathVector2Product(radiusA, bodyA->angularVelocity);
1551         Vector2 crossB = MathVector2Product(radiusB, bodyB->angularVelocity);
1552
1553         Vector2 radiusV = { 0.0f, 0.0f };
1554         radiusV.x = bodyB->velocity.x + crossB.x - bodyA->velocity.x - crossA.x;
1555         radiusV.y = bodyB->velocity.y + crossB.y - bodyA->velocity.y - crossA.y;
1556
1557         // Determine if we should perform a resting collision or not;
1558         // The idea is if the only thing moving this object is gravity, then the collision should be performed without any restitution
1559         if (MathVector2SqrLen(radiusV) < (MathVector2SqrLen(CLITERAL(Vector2){ (float)(gravityForce.x*deltaTime/1000), (float)(gravityForce.y*deltaTime/1000) }) + PHYSAC_EPSILON)) manifold->restitution = 0;
1560     }
1561 }
1562
1563 // Integrates physics collisions impulses to solve collisions
1564 static void IntegratePhysicsImpulses(PhysicsManifold manifold)
1565 {
1566     PhysicsBody bodyA = manifold->bodyA;
1567     PhysicsBody bodyB = manifold->bodyB;
1568
1569     if ((bodyA == NULL) || (bodyB == NULL)) return;
1570
1571     // Early out and positional correct if both objects have infinite mass
1572     if (fabs(bodyA->inverseMass + bodyB->inverseMass) <= PHYSAC_EPSILON)
1573     {
1574         bodyA->velocity = PHYSAC_VECTOR_ZERO;
1575         bodyB->velocity = PHYSAC_VECTOR_ZERO;
1576         return;
1577     }
1578
1579     for (unsigned int i = 0; i < manifold->contactsCount; i++)
1580     {
1581         // Calculate radius from center of mass to contact
1582         Vector2 radiusA = MathVector2Subtract(manifold->contacts[i], bodyA->position);
1583         Vector2 radiusB = MathVector2Subtract(manifold->contacts[i], bodyB->position);
1584
1585         // Calculate relative velocity
1586         Vector2 radiusV = { 0.0f, 0.0f };
1587         radiusV.x = bodyB->velocity.x + MathVector2Product(radiusB, bodyB->angularVelocity).x - bodyA->velocity.x - MathVector2Product(radiusA, bodyA->angularVelocity).x;
1588         radiusV.y = bodyB->velocity.y + MathVector2Product(radiusB, bodyB->angularVelocity).y - bodyA->velocity.y - MathVector2Product(radiusA, bodyA->angularVelocity).y;
1589
1590         // Relative velocity along the normal
1591         float contactVelocity = MathVector2DotProduct(radiusV, manifold->normal);
1592
1593         // Do not resolve if velocities are separating
1594         if (contactVelocity > 0.0f) return;
1595
1596         float raCrossN = MathVector2CrossProduct(radiusA, manifold->normal);
1597         float rbCrossN = MathVector2CrossProduct(radiusB, manifold->normal);
1598
1599         float inverseMassSum = bodyA->inverseMass + bodyB->inverseMass + (raCrossN*raCrossN)*bodyA->inverseInertia + (rbCrossN*rbCrossN)*bodyB->inverseInertia;
1600
1601         // Calculate impulse scalar value
1602         float impulse = -(1.0f + manifold->restitution)*contactVelocity;
1603         impulse /= inverseMassSum;
1604         impulse /= (float)manifold->contactsCount;
1605
1606         // Apply impulse to each physics body
1607         Vector2 impulseV = { manifold->normal.x*impulse, manifold->normal.y*impulse };
1608
1609         if (bodyA->enabled)
1610         {
1611             bodyA->velocity.x += bodyA->inverseMass*(-impulseV.x);
1612             bodyA->velocity.y += bodyA->inverseMass*(-impulseV.y);
1613             if (!bodyA->freezeOrient) bodyA->angularVelocity += bodyA->inverseInertia*MathVector2CrossProduct(radiusA, CLITERAL(Vector2){ -impulseV.x, -impulseV.y });
1614         }
1615
1616         if (bodyB->enabled)
1617         {
1618             bodyB->velocity.x += bodyB->inverseMass*(impulseV.x);
1619             bodyB->velocity.y += bodyB->inverseMass*(impulseV.y);
1620             if (!bodyB->freezeOrient) bodyB->angularVelocity += bodyB->inverseInertia*MathVector2CrossProduct(radiusB, impulseV);
1621         }
1622
1623         // Apply friction impulse to each physics body
1624         radiusV.x = bodyB->velocity.x + MathVector2Product(radiusB, bodyB->angularVelocity).x - bodyA->velocity.x - MathVector2Product(radiusA, bodyA->angularVelocity).x;
1625         radiusV.y = bodyB->velocity.y + MathVector2Product(radiusB, bodyB->angularVelocity).y - bodyA->velocity.y - MathVector2Product(radiusA, bodyA->angularVelocity).y;
1626
1627         Vector2 tangent = { radiusV.x - (manifold->normal.x*MathVector2DotProduct(radiusV, manifold->normal)), radiusV.y - (manifold->normal.y*MathVector2DotProduct(radiusV, manifold->normal)) };
1628         MathVector2Normalize(&tangent);
1629
1630         // Calculate impulse tangent magnitude
1631         float impulseTangent = -MathVector2DotProduct(radiusV, tangent);
1632         impulseTangent /= inverseMassSum;
1633         impulseTangent /= (float)manifold->contactsCount;
1634
1635         float absImpulseTangent = (float)fabs(impulseTangent);
1636
1637         // Don't apply tiny friction impulses
1638         if (absImpulseTangent <= PHYSAC_EPSILON) return;
1639
1640         // Apply coulumb's law
1641         Vector2 tangentImpulse = { 0.0f, 0.0f };
1642         if (absImpulseTangent < impulse*manifold->staticFriction) tangentImpulse = CLITERAL(Vector2){ tangent.x*impulseTangent, tangent.y*impulseTangent };
1643         else tangentImpulse = CLITERAL(Vector2){ tangent.x*-impulse*manifold->dynamicFriction, tangent.y*-impulse*manifold->dynamicFriction };
1644
1645         // Apply friction impulse
1646         if (bodyA->enabled)
1647         {
1648             bodyA->velocity.x += bodyA->inverseMass*(-tangentImpulse.x);
1649             bodyA->velocity.y += bodyA->inverseMass*(-tangentImpulse.y);
1650
1651             if (!bodyA->freezeOrient) bodyA->angularVelocity += bodyA->inverseInertia*MathVector2CrossProduct(radiusA, CLITERAL(Vector2){ -tangentImpulse.x, -tangentImpulse.y });
1652         }
1653
1654         if (bodyB->enabled)
1655         {
1656             bodyB->velocity.x += bodyB->inverseMass*(tangentImpulse.x);
1657             bodyB->velocity.y += bodyB->inverseMass*(tangentImpulse.y);
1658
1659             if (!bodyB->freezeOrient) bodyB->angularVelocity += bodyB->inverseInertia*MathVector2CrossProduct(radiusB, tangentImpulse);
1660         }
1661     }
1662 }
1663
1664 // Integrates physics velocity into position and forces
1665 static void IntegratePhysicsVelocity(PhysicsBody body)
1666 {
1667     if ((body == NULL) ||!body->enabled) return;
1668
1669     body->position.x += (float)(body->velocity.x*deltaTime);
1670     body->position.y += (float)(body->velocity.y*deltaTime);
1671
1672     if (!body->freezeOrient) body->orient += (float)(body->angularVelocity*deltaTime);
1673     body->shape.transform = MathMatFromRadians(body->orient);
1674
1675     IntegratePhysicsForces(body);
1676 }
1677
1678 // Corrects physics bodies positions based on manifolds collision information
1679 static void CorrectPhysicsPositions(PhysicsManifold manifold)
1680 {
1681     PhysicsBody bodyA = manifold->bodyA;
1682     PhysicsBody bodyB = manifold->bodyB;
1683
1684     if ((bodyA == NULL) || (bodyB == NULL)) return;
1685
1686     Vector2 correction = { 0.0f, 0.0f };
1687     correction.x = (PHYSAC_MAX(manifold->penetration - PHYSAC_PENETRATION_ALLOWANCE, 0.0f)/(bodyA->inverseMass + bodyB->inverseMass))*manifold->normal.x*PHYSAC_PENETRATION_CORRECTION;
1688     correction.y = (PHYSAC_MAX(manifold->penetration - PHYSAC_PENETRATION_ALLOWANCE, 0.0f)/(bodyA->inverseMass + bodyB->inverseMass))*manifold->normal.y*PHYSAC_PENETRATION_CORRECTION;
1689
1690     if (bodyA->enabled)
1691     {
1692         bodyA->position.x -= correction.x*bodyA->inverseMass;
1693         bodyA->position.y -= correction.y*bodyA->inverseMass;
1694     }
1695
1696     if (bodyB->enabled)
1697     {
1698         bodyB->position.x += correction.x*bodyB->inverseMass;
1699         bodyB->position.y += correction.y*bodyB->inverseMass;
1700     }
1701 }
1702
1703 // Returns the extreme point along a direction within a polygon
1704 static Vector2 GetSupport(PhysicsShape shape, Vector2 dir)
1705 {
1706     float bestProjection = -PHYSAC_FLT_MAX;
1707     Vector2 bestVertex = { 0.0f, 0.0f };
1708     PhysicsVertexData data = shape.vertexData;
1709
1710     for (unsigned int i = 0; i < data.vertexCount; i++)
1711     {
1712         Vector2 vertex = data.positions[i];
1713         float projection = MathVector2DotProduct(vertex, dir);
1714
1715         if (projection > bestProjection)
1716         {
1717             bestVertex = vertex;
1718             bestProjection = projection;
1719         }
1720     }
1721
1722     return bestVertex;
1723 }
1724
1725 // Finds polygon shapes axis least penetration
1726 static float FindAxisLeastPenetration(int *faceIndex, PhysicsShape shapeA, PhysicsShape shapeB)
1727 {
1728     float bestDistance = -PHYSAC_FLT_MAX;
1729     int bestIndex = 0;
1730
1731     PhysicsVertexData dataA = shapeA.vertexData;
1732     //PhysicsVertexData dataB = shapeB.vertexData;
1733
1734     for (unsigned int i = 0; i < dataA.vertexCount; i++)
1735     {
1736         // Retrieve a face normal from A shape
1737         Vector2 normal = dataA.normals[i];
1738         Vector2 transNormal = MathMatVector2Product(shapeA.transform, normal);
1739
1740         // Transform face normal into B shape's model space
1741         Matrix2x2 buT = MathMatTranspose(shapeB.transform);
1742         normal = MathMatVector2Product(buT, transNormal);
1743
1744         // Retrieve support point from B shape along -n
1745         Vector2 support = GetSupport(shapeB, CLITERAL(Vector2){ -normal.x, -normal.y });
1746
1747         // Retrieve vertex on face from A shape, transform into B shape's model space
1748         Vector2 vertex = dataA.positions[i];
1749         vertex = MathMatVector2Product(shapeA.transform, vertex);
1750         vertex = MathVector2Add(vertex, shapeA.body->position);
1751         vertex = MathVector2Subtract(vertex, shapeB.body->position);
1752         vertex = MathMatVector2Product(buT, vertex);
1753
1754         // Compute penetration distance in B shape's model space
1755         float distance = MathVector2DotProduct(normal, MathVector2Subtract(support, vertex));
1756
1757         // Store greatest distance
1758         if (distance > bestDistance)
1759         {
1760             bestDistance = distance;
1761             bestIndex = i;
1762         }
1763     }
1764
1765     *faceIndex = bestIndex;
1766     return bestDistance;
1767 }
1768
1769 // Finds two polygon shapes incident face
1770 static void FindIncidentFace(Vector2 *v0, Vector2 *v1, PhysicsShape ref, PhysicsShape inc, int index)
1771 {
1772     PhysicsVertexData refData = ref.vertexData;
1773     PhysicsVertexData incData = inc.vertexData;
1774
1775     Vector2 referenceNormal = refData.normals[index];
1776
1777     // Calculate normal in incident's frame of reference
1778     referenceNormal = MathMatVector2Product(ref.transform, referenceNormal); // To world space
1779     referenceNormal = MathMatVector2Product(MathMatTranspose(inc.transform), referenceNormal); // To incident's model space
1780
1781     // Find most anti-normal face on polygon
1782     int incidentFace = 0;
1783     float minDot = PHYSAC_FLT_MAX;
1784
1785     for (unsigned int i = 0; i < incData.vertexCount; i++)
1786     {
1787         float dot = MathVector2DotProduct(referenceNormal, incData.normals[i]);
1788
1789         if (dot < minDot)
1790         {
1791             minDot = dot;
1792             incidentFace = i;
1793         }
1794     }
1795
1796     // Assign face vertices for incident face
1797     *v0 = MathMatVector2Product(inc.transform, incData.positions[incidentFace]);
1798     *v0 = MathVector2Add(*v0, inc.body->position);
1799     incidentFace = (((incidentFace + 1) < (int)incData.vertexCount) ? (incidentFace + 1) : 0);
1800     *v1 = MathMatVector2Product(inc.transform, incData.positions[incidentFace]);
1801     *v1 = MathVector2Add(*v1, inc.body->position);
1802 }
1803
1804 // Returns clipping value based on a normal and two faces
1805 static int MathVector2Clip(Vector2 normal, Vector2 *faceA, Vector2 *faceB, float clip)
1806 {
1807     int sp = 0;
1808     Vector2 out[2] = { *faceA, *faceB };
1809
1810     // Retrieve distances from each endpoint to the line
1811     float distanceA = MathVector2DotProduct(normal, *faceA) - clip;
1812     float distanceB = MathVector2DotProduct(normal, *faceB) - clip;
1813
1814     // If negative (behind plane)
1815     if (distanceA <= 0.0f) out[sp++] = *faceA;
1816     if (distanceB <= 0.0f) out[sp++] = *faceB;
1817
1818     // If the points are on different sides of the plane
1819     if ((distanceA*distanceB) < 0.0f)
1820     {
1821         // Push intersection point
1822         float alpha = distanceA/(distanceA - distanceB);
1823         out[sp] = *faceA;
1824         Vector2 delta = MathVector2Subtract(*faceB, *faceA);
1825         delta.x *= alpha;
1826         delta.y *= alpha;
1827         out[sp] = MathVector2Add(out[sp], delta);
1828         sp++;
1829     }
1830
1831     // Assign the new converted values
1832     *faceA = out[0];
1833     *faceB = out[1];
1834
1835     return sp;
1836 }
1837
1838 // Returns the barycenter of a triangle given by 3 points
1839 static Vector2 MathTriangleBarycenter(Vector2 v1, Vector2 v2, Vector2 v3)
1840 {
1841     Vector2 result = { 0.0f, 0.0f };
1842
1843     result.x = (v1.x + v2.x + v3.x)/3;
1844     result.y = (v1.y + v2.y + v3.y)/3;
1845
1846     return result;
1847 }
1848
1849 #if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
1850 // Initializes hi-resolution MONOTONIC timer
1851 static void InitTimer(void)
1852 {
1853 #if defined(_WIN32)
1854     QueryPerformanceFrequency((unsigned long long int *) &frequency);
1855 #endif
1856
1857 #if defined(__EMSCRIPTEN__) || defined(__linux__)
1858     struct timespec now;
1859     if (clock_gettime(CLOCK_MONOTONIC, &now) == 0) frequency = 1000000000;
1860 #endif
1861
1862 #if defined(__APPLE__)
1863     mach_timebase_info_data_t timebase;
1864     mach_timebase_info(&timebase);
1865     frequency = (timebase.denom*1e9)/timebase.numer;
1866 #endif
1867
1868     baseClockTicks = (double)GetClockTicks();      // Get MONOTONIC clock time offset
1869     startTime = GetCurrentTime();                  // Get current time in milliseconds
1870 }
1871
1872 // Get hi-res MONOTONIC time measure in clock ticks
1873 static unsigned long long int GetClockTicks(void)
1874 {
1875     unsigned long long int value = 0;
1876
1877 #if defined(_WIN32)
1878     QueryPerformanceCounter((unsigned long long int *) &value);
1879 #endif
1880
1881 #if defined(__linux__)
1882     struct timespec now;
1883     clock_gettime(CLOCK_MONOTONIC, &now);
1884     value = (unsigned long long int)now.tv_sec*(unsigned long long int)1000000000 + (unsigned long long int)now.tv_nsec;
1885 #endif
1886
1887 #if defined(__APPLE__)
1888     value = mach_absolute_time();
1889 #endif
1890
1891     return value;
1892 }
1893
1894 // Get current time in milliseconds
1895 static double GetCurrentTime(void)
1896 {
1897     return (double)(GetClockTicks() - baseClockTicks)/frequency*1000;
1898 }
1899 #endif // !PHYSAC_AVOID_TIMMING_SYSTEM
1900
1901
1902 // Returns the cross product of a vector and a value
1903 static inline Vector2 MathVector2Product(Vector2 vector, float value)
1904 {
1905     Vector2 result = { -value*vector.y, value*vector.x };
1906     return result;
1907 }
1908
1909 // Returns the cross product of two vectors
1910 static inline float MathVector2CrossProduct(Vector2 v1, Vector2 v2)
1911 {
1912     return (v1.x*v2.y - v1.y*v2.x);
1913 }
1914
1915 // Returns the len square root of a vector
1916 static inline float MathVector2SqrLen(Vector2 vector)
1917 {
1918     return (vector.x*vector.x + vector.y*vector.y);
1919 }
1920
1921 // Returns the dot product of two vectors
1922 static inline float MathVector2DotProduct(Vector2 v1, Vector2 v2)
1923 {
1924     return (v1.x*v2.x + v1.y*v2.y);
1925 }
1926
1927 // Returns the square root of distance between two vectors
1928 static inline float MathVector2SqrDistance(Vector2 v1, Vector2 v2)
1929 {
1930     Vector2 dir = MathVector2Subtract(v1, v2);
1931     return MathVector2DotProduct(dir, dir);
1932 }
1933
1934 // Returns the normalized values of a vector
1935 static void MathVector2Normalize(Vector2 *vector)
1936 {
1937     float length, ilength;
1938
1939     Vector2 aux = *vector;
1940     length = sqrtf(aux.x*aux.x + aux.y*aux.y);
1941
1942     if (length == 0) length = 1.0f;
1943
1944     ilength = 1.0f/length;
1945
1946     vector->x *= ilength;
1947     vector->y *= ilength;
1948 }
1949
1950 // Returns the sum of two given vectors
1951 static inline Vector2 MathVector2Add(Vector2 v1, Vector2 v2)
1952 {
1953     Vector2 result = { v1.x + v2.x, v1.y + v2.y };
1954     return result;
1955 }
1956
1957 // Returns the subtract of two given vectors
1958 static inline Vector2 MathVector2Subtract(Vector2 v1, Vector2 v2)
1959 {
1960     Vector2 result = { v1.x - v2.x, v1.y - v2.y };
1961     return result;
1962 }
1963
1964 // Creates a matrix 2x2 from a given radians value
1965 static Matrix2x2 MathMatFromRadians(float radians)
1966 {
1967     float cos = cosf(radians);
1968     float sin = sinf(radians);
1969
1970     Matrix2x2 result = { cos, -sin, sin, cos };
1971     return result;
1972 }
1973
1974 // Returns the transpose of a given matrix 2x2
1975 static inline Matrix2x2 MathMatTranspose(Matrix2x2 matrix)
1976 {
1977     Matrix2x2 result = { matrix.m00, matrix.m10, matrix.m01, matrix.m11 };
1978     return result;
1979 }
1980
1981 // Multiplies a vector by a matrix 2x2
1982 static inline Vector2 MathMatVector2Product(Matrix2x2 matrix, Vector2 vector)
1983 {
1984     Vector2 result = { matrix.m00*vector.x + matrix.m01*vector.y, matrix.m10*vector.x + matrix.m11*vector.y };
1985     return result;
1986 }
1987
1988 #endif  // PHYSAC_IMPLEMENTATION