physac module redesign (2/3)

physac module base almost finished. All collisions are now resolved
properly and some force functions was added.

COLLIDER_CAPSULE removed for now because in 2D everything is composed by
rectangle and circle colliders...

The last step is move physics update loop into another thread and update
it in a fixed time step based on fps.

2 files changed, 371 insertions, 132 deletions

diff --git a/src/physac.c b/src/physac.c
index 13247117..718a06bb 100644
--- a/src/physac.c
+++ b/src/physac.c
@@ -36,10 +36,9 @@
 // Defines and Macros
 //----------------------------------------------------------------------------------
 #define MAX_PHYSIC_OBJECTS      256
-#define PHYSICS_GRAVITY         -9.81f/2
 #define PHYSICS_STEPS           450
-#define PHYSICS_ACCURACY        0.0001f     // Velocity subtract operations round filter (friction)
-#define PHYSICS_ERRORPERCENT    0.001f        // Collision resolve position fix
+#define PHYSICS_ACCURACY        0.0001f         // Velocity subtract operations round filter (friction)
+#define PHYSICS_ERRORPERCENT    0.001f          // Collision resolve position fix
 
 //----------------------------------------------------------------------------------
 // Types and Structures Definition
@@ -52,53 +51,70 @@
 //----------------------------------------------------------------------------------
 static PhysicObject *physicObjects[MAX_PHYSIC_OBJECTS];             // Physic objects pool
 static int physicObjectsCount;                                      // Counts current enabled physic objects
+static Vector2 gravityForce;                                        // Gravity force
 
 //----------------------------------------------------------------------------------
 // Module specific Functions Declaration
 //----------------------------------------------------------------------------------
 static float Vector2DotProduct(Vector2 v1, Vector2 v2);             // Returns the dot product of two Vector2
+static float Vector2Length(Vector2 v);                              // Returns the length of a Vector2
 
 //----------------------------------------------------------------------------------
 // Module Functions Definition
 //----------------------------------------------------------------------------------
 
 // Initializes pointers array (just pointers, fixed size)
-void InitPhysics()
+void InitPhysics(Vector2 gravity)
 {
     // Initialize physics variables
     physicObjectsCount = 0;
+    gravityForce = gravity;
 }
 
 // Update physic objects, calculating physic behaviours and collisions detection
 void UpdatePhysics()
 {
     // Reset all physic objects is grounded state
-    for(int i = 0; i < physicObjectsCount; i++)
+    for (int i = 0; i < physicObjectsCount; i++)
     {
-        if(physicObjects[i]->rigidbody.enabled) physicObjects[i]->rigidbody.isGrounded = false;
+        if (physicObjects[i]->rigidbody.enabled) physicObjects[i]->rigidbody.isGrounded = false;
     }
     
-    for(int steps = 0; steps < PHYSICS_STEPS; steps++)
+    for (int steps = 0; steps < PHYSICS_STEPS; steps++)
     {
-        for(int i = 0; i < physicObjectsCount; i++)
+        for (int i = 0; i < physicObjectsCount; i++)
         {
-            if(physicObjects[i]->enabled)
+            if (physicObjects[i]->enabled)
             {
                 // Update physic behaviour
-                if(physicObjects[i]->rigidbody.enabled)
+                if (physicObjects[i]->rigidbody.enabled)
                 {
                     // Apply friction to acceleration in X axis
                     if (physicObjects[i]->rigidbody.acceleration.x > PHYSICS_ACCURACY) physicObjects[i]->rigidbody.acceleration.x -= physicObjects[i]->rigidbody.friction/PHYSICS_STEPS;
                     else if (physicObjects[i]->rigidbody.acceleration.x < PHYSICS_ACCURACY) physicObjects[i]->rigidbody.acceleration.x += physicObjects[i]->rigidbody.friction/PHYSICS_STEPS;
                     else physicObjects[i]->rigidbody.acceleration.x = 0.0f;
                     
+                    // Apply friction to acceleration in Y axis
+                    if (physicObjects[i]->rigidbody.acceleration.y > PHYSICS_ACCURACY) physicObjects[i]->rigidbody.acceleration.y -= physicObjects[i]->rigidbody.friction/PHYSICS_STEPS;
+                    else if (physicObjects[i]->rigidbody.acceleration.y < PHYSICS_ACCURACY) physicObjects[i]->rigidbody.acceleration.y += physicObjects[i]->rigidbody.friction/PHYSICS_STEPS;
+                    else physicObjects[i]->rigidbody.acceleration.y = 0.0f;
+                    
                     // Apply friction to velocity in X axis
                     if (physicObjects[i]->rigidbody.velocity.x > PHYSICS_ACCURACY) physicObjects[i]->rigidbody.velocity.x -= physicObjects[i]->rigidbody.friction/PHYSICS_STEPS;
                     else if (physicObjects[i]->rigidbody.velocity.x < PHYSICS_ACCURACY) physicObjects[i]->rigidbody.velocity.x += physicObjects[i]->rigidbody.friction/PHYSICS_STEPS;
                     else physicObjects[i]->rigidbody.velocity.x = 0.0f;
                     
+                    // Apply friction to velocity in Y axis
+                    if (physicObjects[i]->rigidbody.velocity.y > PHYSICS_ACCURACY) physicObjects[i]->rigidbody.velocity.y -= physicObjects[i]->rigidbody.friction/PHYSICS_STEPS;
+                    else if (physicObjects[i]->rigidbody.velocity.y < PHYSICS_ACCURACY) physicObjects[i]->rigidbody.velocity.y += physicObjects[i]->rigidbody.friction/PHYSICS_STEPS;
+                    else physicObjects[i]->rigidbody.velocity.y = 0.0f;
+                    
                     // Apply gravity to velocity
-                    if (physicObjects[i]->rigidbody.applyGravity) physicObjects[i]->rigidbody.velocity.y += PHYSICS_GRAVITY/PHYSICS_STEPS;
+                    if (physicObjects[i]->rigidbody.applyGravity)
+                    {
+                        physicObjects[i]->rigidbody.velocity.x += gravityForce.x/PHYSICS_STEPS;
+                        physicObjects[i]->rigidbody.velocity.y += gravityForce.y/PHYSICS_STEPS;
+                    }
                     
                     // Apply acceleration to velocity
                     physicObjects[i]->rigidbody.velocity.x += physicObjects[i]->rigidbody.acceleration.x/PHYSICS_STEPS;
@@ -120,142 +136,314 @@ void UpdatePhysics()
                     {
                         if (physicObjects[k]->collider.enabled && i != k)
                         {
-                            // Check if colliders are overlapped
-                            if (CheckCollisionRecs(physicObjects[i]->collider.bounds, physicObjects[k]->collider.bounds))
+                            // Resolve physic collision
+                            // NOTE: collision resolve is generic for all directions and conditions (no axis separated cases behaviours)
+                            // and it is separated in rigidbody attributes resolve (velocity changes by impulse) and position correction (position overlap)
+                            
+                            // 1. Calculate collision normal
+                            // -------------------------------------------------------------------------------------------------------------------------------------
+                            
+                            // Define collision contact normal, direction and penetration depth
+                            Vector2 contactNormal = { 0.0f, 0.0f };
+                            Vector2 direction = { 0.0f, 0.0f };
+                            float penetrationDepth = 0.0f;
+                            
+                            switch(physicObjects[i]->collider.type)
                             {
-                                // Resolve physic collision
-                                // NOTE: collision resolve is generic for all directions and conditions (no axis separated cases behaviours)
-                                // and it is separated in rigidbody attributes resolve (velocity changes by impulse) and position correction (position overlap)
-                                
-                                // 1. Calculate collision normal
-                                // -------------------------------------------------------------------------------------------------------------------------------------
-                                
-                                // Define collision ontact normal
-                                Vector2 contactNormal = { 0.0f, 0.0f };
+                                case COLLIDER_RECTANGLE:
+                                {
+                                    switch(physicObjects[k]->collider.type)
+                                    {
+                                        case COLLIDER_RECTANGLE:
+                                        {
+                                            // Check if colliders are overlapped
+                                            if (CheckCollisionRecs(physicObjects[i]->collider.bounds, physicObjects[k]->collider.bounds))
+                                            {
+                                                // Calculate direction vector from i to k
+                                                direction.x = (physicObjects[k]->transform.position.x + physicObjects[k]->transform.scale.x/2) - (physicObjects[i]->transform.position.x + physicObjects[i]->transform.scale.x/2);
+                                                direction.y = (physicObjects[k]->transform.position.y + physicObjects[k]->transform.scale.y/2) - (physicObjects[i]->transform.position.y + physicObjects[i]->transform.scale.y/2);
+                                                
+                                                // Define overlapping and penetration attributes
+                                                Vector2 overlap;
+
+                                                // Calculate overlap on X axis
+                                                overlap.x = (physicObjects[i]->transform.scale.x + physicObjects[k]->transform.scale.x)/2 - abs(direction.x);
+                                                
+                                                // SAT test on X axis
+                                                if (overlap.x > 0.0f)
+                                                {
+                                                    // Calculate overlap on Y axis
+                                                    overlap.y = (physicObjects[i]->transform.scale.y + physicObjects[k]->transform.scale.y)/2 - abs(direction.y);
+                                                    
+                                                    // SAT test on Y axis
+                                                    if (overlap.y > 0.0f)
+                                                    {
+                                                        // Find out which axis is axis of least penetration
+                                                        if (overlap.y > overlap.x)
+                                                        {
+                                                            // Point towards k knowing that direction points from i to k
+                                                            if (direction.x < 0.0f) contactNormal = (Vector2){ -1.0f, 0.0f };
+                                                            else contactNormal = (Vector2){ 1.0f, 0.0f };
+                                                            
+                                                            // Update penetration depth for position correction
+                                                            penetrationDepth = overlap.x;
+                                                        }
+                                                        else
+                                                        {
+                                                            // Point towards k knowing that direction points from i to k
+                                                            if (direction.y < 0.0f) contactNormal = (Vector2){ 0.0f, 1.0f };
+                                                            else contactNormal = (Vector2){ 0.0f, -1.0f };
+                                                            
+                                                            // Update penetration depth for position correction
+                                                            penetrationDepth = overlap.y;
+                                                        }
+                                                    }
+                                                }
+                                            }
+                                        } break;
+                                        case COLLIDER_CIRCLE:
+                                        {
+                                            if (CheckCollisionCircleRec(physicObjects[k]->transform.position, physicObjects[k]->collider.radius, physicObjects[i]->collider.bounds))
+                                            {
+                                                // Calculate direction vector between circles
+                                                direction.x = physicObjects[k]->transform.position.x - physicObjects[i]->transform.position.x + physicObjects[i]->transform.scale.x/2;
+                                                direction.y = physicObjects[k]->transform.position.y - physicObjects[i]->transform.position.y + physicObjects[i]->transform.scale.y/2;
+                                                
+                                                // Calculate closest point on rectangle to circle
+                                                Vector2 closestPoint = { 0.0f, 0.0f };
+                                                if (direction.x > 0.0f) closestPoint.x = physicObjects[i]->collider.bounds.x + physicObjects[i]->collider.bounds.width;
+                                                else closestPoint.x = physicObjects[i]->collider.bounds.x;
+                                                
+                                                if (direction.y > 0.0f) closestPoint.y = physicObjects[i]->collider.bounds.y + physicObjects[i]->collider.bounds.height;
+                                                else closestPoint.y = physicObjects[i]->collider.bounds.y;
+                                                
+                                                // Check if the closest point is inside the circle
+                                                if (CheckCollisionPointCircle(closestPoint, physicObjects[k]->transform.position, physicObjects[k]->collider.radius))
+                                                {
+                                                    // Recalculate direction based on closest point position
+                                                    direction.x = physicObjects[k]->transform.position.x - closestPoint.x;
+                                                    direction.y = physicObjects[k]->transform.position.y - closestPoint.y;
+                                                    float distance = Vector2Length(direction);
+                                                    
+                                                    // Calculate final contact normal
+                                                    contactNormal.x = direction.x/distance;
+                                                    contactNormal.y = -direction.y/distance;
+                                                    
+                                                    // Calculate penetration depth
+                                                    penetrationDepth = physicObjects[k]->collider.radius - distance;
+                                                }
+                                                else
+                                                {
+                                                    if (abs(direction.y) < abs(direction.x))
+                                                    {
+                                                        // Calculate final contact normal
+                                                        if (direction.y > 0.0f)
+                                                        {
+                                                            contactNormal = (Vector2){ 0.0f, -1.0f };
+                                                            penetrationDepth = fabs(physicObjects[i]->collider.bounds.y - physicObjects[k]->transform.position.y - physicObjects[k]->collider.radius);
+                                                        }
+                                                        else 
+                                                        {
+                                                            contactNormal = (Vector2){ 0.0f, 1.0f };
+                                                            penetrationDepth = fabs(physicObjects[i]->collider.bounds.y - physicObjects[k]->transform.position.y + physicObjects[k]->collider.radius);
+                                                        }
+                                                    }
+                                                    else
+                                                    {
+                                                        // Calculate final contact normal
+                                                        if (direction.x > 0.0f)
+                                                        {
+                                                            contactNormal = (Vector2){ 1.0f, 0.0f };
+                                                            penetrationDepth = fabs(physicObjects[k]->transform.position.x + physicObjects[k]->collider.radius - physicObjects[i]->collider.bounds.x);
+                                                        }
+                                                        else 
+                                                        {
+                                                            contactNormal = (Vector2){ -1.0f, 0.0f };
+                                                            penetrationDepth = fabs(physicObjects[i]->collider.bounds.x + physicObjects[i]->collider.bounds.width - physicObjects[k]->transform.position.x - physicObjects[k]->collider.radius);
+                                                        }
+                                                    }
+                                                }
+                                            }
+                                        } break;
+                                    }
+                                } break;
+                                case COLLIDER_CIRCLE:
+                                {
+                                    switch(physicObjects[k]->collider.type)
+                                    {
+                                        case COLLIDER_RECTANGLE:
+                                        {
+                                            if (CheckCollisionCircleRec(physicObjects[i]->transform.position, physicObjects[i]->collider.radius, physicObjects[k]->collider.bounds))
+                                            {
+                                                // Calculate direction vector between circles
+                                                direction.x = physicObjects[k]->transform.position.x + physicObjects[i]->transform.scale.x/2 - physicObjects[i]->transform.position.x;
+                                                direction.y = physicObjects[k]->transform.position.y + physicObjects[i]->transform.scale.y/2 - physicObjects[i]->transform.position.y;
+                                                
+                                                // Calculate closest point on rectangle to circle
+                                                Vector2 closestPoint = { 0.0f, 0.0f };
+                                                if (direction.x > 0.0f) closestPoint.x = physicObjects[k]->collider.bounds.x + physicObjects[k]->collider.bounds.width;
+                                                else closestPoint.x = physicObjects[k]->collider.bounds.x;
+                                                
+                                                if (direction.y > 0.0f) closestPoint.y = physicObjects[k]->collider.bounds.y + physicObjects[k]->collider.bounds.height;
+                                                else closestPoint.y = physicObjects[k]->collider.bounds.y;
+                                                
+                                                // Check if the closest point is inside the circle
+                                                if (CheckCollisionPointCircle(closestPoint, physicObjects[i]->transform.position, physicObjects[i]->collider.radius))
+                                                {
+                                                    // Recalculate direction based on closest point position
+                                                    direction.x = physicObjects[i]->transform.position.x - closestPoint.x;
+                                                    direction.y = physicObjects[i]->transform.position.y - closestPoint.y;
+                                                    float distance = Vector2Length(direction);
+                                                    
+                                                    // Calculate final contact normal
+                                                    contactNormal.x = direction.x/distance;
+                                                    contactNormal.y = -direction.y/distance;
+                                                    
+                                                    // Calculate penetration depth
+                                                    penetrationDepth = physicObjects[k]->collider.radius - distance;
+                                                }
+                                                else
+                                                {
+                                                    if (abs(direction.y) < abs(direction.x))
+                                                    {
+                                                        // Calculate final contact normal
+                                                        if (direction.y > 0.0f)
+                                                        {
+                                                            contactNormal = (Vector2){ 0.0f, -1.0f };
+                                                            penetrationDepth = fabs(physicObjects[k]->collider.bounds.y - physicObjects[i]->transform.position.y - physicObjects[i]->collider.radius);
+                                                        }
+                                                        else 
+                                                        {
+                                                            contactNormal = (Vector2){ 0.0f, 1.0f };
+                                                            penetrationDepth = fabs(physicObjects[k]->collider.bounds.y - physicObjects[i]->transform.position.y + physicObjects[i]->collider.radius);
+                                                        }
+                                                    }
+                                                    else
+                                                    {
+                                                        // Calculate final contact normal and penetration depth
+                                                        if (direction.x > 0.0f)
+                                                        {
+                                                            contactNormal = (Vector2){ 1.0f, 0.0f };
+                                                            penetrationDepth = fabs(physicObjects[i]->transform.position.x + physicObjects[i]->collider.radius - physicObjects[k]->collider.bounds.x);
+                                                        }
+                                                        else 
+                                                        {
+                                                            contactNormal = (Vector2){ -1.0f, 0.0f };
+                                                            penetrationDepth = fabs(physicObjects[k]->collider.bounds.x + physicObjects[k]->collider.bounds.width - physicObjects[i]->transform.position.x - physicObjects[i]->collider.radius);
+                                                        }
+                                                    }
+                                                }
+                                            }
+                                        } break;
+                                        case COLLIDER_CIRCLE:
+                                        {
+                                            // Check if colliders are overlapped
+                                            if (CheckCollisionCircles(physicObjects[i]->transform.position, physicObjects[i]->collider.radius, physicObjects[k]->transform.position, physicObjects[k]->collider.radius))
+                                            {
+                                                // Calculate direction vector between circles
+                                                direction.x = physicObjects[k]->transform.position.x - physicObjects[i]->transform.position.x;
+                                                direction.y = physicObjects[k]->transform.position.y - physicObjects[i]->transform.position.y;
+                                                
+                                                // Calculate distance between circles
+                                                float distance = Vector2Length(direction);
+                                                
+                                                // Check if circles are not completely overlapped
+                                                if (distance != 0.0f)
+                                                {                                                    
+                                                    // Calculate contact normal direction (Y axis needs to be flipped)
+                                                    contactNormal.x = direction.x/distance;
+                                                    contactNormal.y = -direction.y/distance;
+                                                }
+                                                else contactNormal = (Vector2){ 1.0f, 0.0f };   // Choose random (but consistent) values
+                                            }
+                                        } break;
+                                        default: break;
+                                    }
+                                } break;
+                                default: break;
+                            }
+                            
+                            // Update rigidbody grounded state
+                            if (physicObjects[i]->rigidbody.enabled)
+                            {
+                                if (contactNormal.y < 0.0f) physicObjects[i]->rigidbody.isGrounded = true;
+                            }
+                            
+                            // 2. Calculate collision impulse
+                            // -------------------------------------------------------------------------------------------------------------------------------------
+                            
+                            // Calculate relative velocity
+                            Vector2 relVelocity = { 0.0f, 0.0f };
+                            relVelocity.x = physicObjects[k]->rigidbody.velocity.x - physicObjects[i]->rigidbody.velocity.x;
+                            relVelocity.y = physicObjects[k]->rigidbody.velocity.y - physicObjects[i]->rigidbody.velocity.y;
+
+                            // Calculate relative velocity in terms of the normal direction
+                            float velAlongNormal = Vector2DotProduct(relVelocity, contactNormal);
+                        
+                            // Dot not resolve if velocities are separating
+                            if (velAlongNormal <= 0.0f)
+                            {
+                                // Calculate minimum bounciness value from both objects
+                                float e = fminf(physicObjects[i]->rigidbody.bounciness, physicObjects[k]->rigidbody.bounciness);
                                 
-                                // Calculate direction vector from i to k
-                                Vector2 direction;
-                                direction.x = (physicObjects[k]->transform.position.x + physicObjects[k]->transform.scale.x/2) - (physicObjects[i]->transform.position.x + physicObjects[i]->transform.scale.x/2);
-                                direction.y = (physicObjects[k]->transform.position.y + physicObjects[k]->transform.scale.y/2) - (physicObjects[i]->transform.position.y + physicObjects[i]->transform.scale.y/2);
+                                // Calculate impulse scalar value
+                                float j = -(1.0f + e)*velAlongNormal;
+                                j /= 1.0f/physicObjects[i]->rigidbody.mass + 1.0f/physicObjects[k]->rigidbody.mass;
                                 
-                                // Define overlapping and penetration attributes
-                                Vector2 overlap;
-                                float penetrationDepth = 0.0f;
+                                // Calculate final impulse vector
+                                Vector2 impulse = { j*contactNormal.x, j*contactNormal.y };
                                 
-                                // Calculate overlap on X axis
-                                overlap.x = (physicObjects[i]->transform.scale.x + physicObjects[k]->transform.scale.x)/2 - abs(direction.x);
+                                // Calculate collision mass ration
+                                float massSum = physicObjects[i]->rigidbody.mass + physicObjects[k]->rigidbody.mass;
+                                float ratio = 0.0f;
                                 
-                                // SAT test on X axis
-                                if (overlap.x > 0.0f)
+                                // Apply impulse to current rigidbodies velocities if they are enabled
+                                if (physicObjects[i]->rigidbody.enabled) 
                                 {
-                                    // Calculate overlap on Y axis
-                                    overlap.y = (physicObjects[i]->transform.scale.y + physicObjects[k]->transform.scale.y)/2 - abs(direction.y);
+                                    // Calculate inverted mass ration
+                                    ratio = physicObjects[i]->rigidbody.mass/massSum;
                                     
-                                    // SAT test on Y axis
-                                    if (overlap.y > 0.0f)
-                                    {
-                                        // Find out which axis is axis of least penetration
-                                        if (overlap.y > overlap.x)
-                                        {
-                                            // Point towards k knowing that direction points from i to k
-                                            if (direction.x < 0.0f) contactNormal = (Vector2){ -1.0f, 0.0f };
-                                            else contactNormal = (Vector2){ 1.0f, 0.0f };
-                                            
-                                            // Update penetration depth for position correction
-                                            penetrationDepth = overlap.x;
-                                        }
-                                        else
-                                        {
-                                            // Point towards k knowing that direction points from i to k
-                                            if (direction.y < 0.0f) contactNormal = (Vector2){ 0.0f, 1.0f };
-                                            else contactNormal = (Vector2){ 0.0f, -1.0f };
-                                            
-                                            // Update penetration depth for position correction
-                                            penetrationDepth = overlap.y;
-                                        }
-                                    }
+                                    // Apply impulse direction to velocity
+                                    physicObjects[i]->rigidbody.velocity.x -= impulse.x*ratio*(1.0f+physicObjects[i]->rigidbody.bounciness);
+                                    physicObjects[i]->rigidbody.velocity.y -= impulse.y*ratio*(1.0f+physicObjects[i]->rigidbody.bounciness);
                                 }
                                 
-                                // Update rigidbody grounded state
-                                if (physicObjects[i]->rigidbody.enabled)
+                                if (physicObjects[k]->rigidbody.enabled) 
                                 {
-                                    if (contactNormal.y < 0.0f) physicObjects[i]->rigidbody.isGrounded = true;
+                                    // Calculate inverted mass ration
+                                    ratio = physicObjects[k]->rigidbody.mass/massSum;
+                                    
+                                    // Apply impulse direction to velocity
+                                    physicObjects[k]->rigidbody.velocity.x += impulse.x*ratio*(1.0f+physicObjects[i]->rigidbody.bounciness);
+                                    physicObjects[k]->rigidbody.velocity.y += impulse.y*ratio*(1.0f+physicObjects[i]->rigidbody.bounciness);
                                 }
                                 
-                                // 2. Calculate collision impulse
-                                // -------------------------------------------------------------------------------------------------------------------------------------
+                                // 3. Correct colliders overlaping (transform position)
+                                // ---------------------------------------------------------------------------------------------------------------------------------
                                 
-                                // Calculate relative velocity
-                                Vector2 relVelocity = { physicObjects[k]->rigidbody.velocity.x - physicObjects[i]->rigidbody.velocity.x, physicObjects[k]->rigidbody.velocity.y - physicObjects[i]->rigidbody.velocity.y };
-
-                                // Calculate relative velocity in terms of the normal direction
-                                float velAlongNormal = Vector2DotProduct(relVelocity, contactNormal);
-                            
-                                // Dot not resolve if velocities are separating
-                                if (velAlongNormal <= 0.0f)
-                                {
-                                    // Calculate minimum bounciness value from both objects
-                                    float e = fminf(physicObjects[i]->rigidbody.bounciness, physicObjects[k]->rigidbody.bounciness);
-                                    
-                                    // Calculate impulse scalar value
-                                    float j = -(1.0f + e) * velAlongNormal;
-                                    j /= 1.0f/physicObjects[i]->rigidbody.mass + 1.0f/physicObjects[k]->rigidbody.mass;
-                                    
-                                    // Calculate final impulse vector
-                                    Vector2 impulse = { j*contactNormal.x, j*contactNormal.y };
-                                    
-                                    // Calculate collision mass ration
-                                    float massSum = physicObjects[i]->rigidbody.mass + physicObjects[k]->rigidbody.mass;
-                                    float ratio = 0.0f;
+                                // Calculate transform position penetration correction
+                                Vector2 posCorrection;
+                                posCorrection.x = penetrationDepth/((1.0f/physicObjects[i]->rigidbody.mass) + (1.0f/physicObjects[k]->rigidbody.mass))*PHYSICS_ERRORPERCENT*contactNormal.x;
+                                posCorrection.y = penetrationDepth/((1.0f/physicObjects[i]->rigidbody.mass) + (1.0f/physicObjects[k]->rigidbody.mass))*PHYSICS_ERRORPERCENT*contactNormal.y;
+                                
+                                // Fix transform positions
+                                if (physicObjects[i]->rigidbody.enabled)
+                                {                                        
+                                    // Fix physic objects transform position
+                                    physicObjects[i]->transform.position.x -= 1.0f/physicObjects[i]->rigidbody.mass*posCorrection.x;
+                                    physicObjects[i]->transform.position.y += 1.0f/physicObjects[i]->rigidbody.mass*posCorrection.y;
                                     
-                                    // Apply impulse to current rigidbodies velocities if they are enabled
-                                    if (physicObjects[i]->rigidbody.enabled) 
-                                    {
-                                        // Calculate inverted mass ration
-                                        ratio = physicObjects[i]->rigidbody.mass/massSum;
-                                        
-                                        // Apply impulse direction to velocity
-                                        physicObjects[i]->rigidbody.velocity.x -= impulse.x*ratio;
-                                        physicObjects[i]->rigidbody.velocity.y -= impulse.y*ratio;
-                                    }
+                                    // Update collider bounds
+                                    physicObjects[i]->collider.bounds = TransformToRectangle(physicObjects[i]->transform);
                                     
-                                    if (physicObjects[k]->rigidbody.enabled) 
+                                    if (physicObjects[k]->rigidbody.enabled)
                                     {
-                                        // Calculate inverted mass ration
-                                        ratio = physicObjects[k]->rigidbody.mass/massSum;
-                                        
-                                        // Apply impulse direction to velocity
-                                        physicObjects[k]->rigidbody.velocity.x += impulse.x*ratio;
-                                        physicObjects[k]->rigidbody.velocity.y += impulse.y*ratio;
-                                    }
-                                    
-                                    // 3. Correct colliders overlaping (transform position)
-                                    // ---------------------------------------------------------------------------------------------------------------------------------
-                                    
-                                    // Calculate transform position penetration correction
-                                    Vector2 posCorrection;
-                                    posCorrection.x = penetrationDepth/((1.0f/physicObjects[i]->rigidbody.mass) + (1.0f/physicObjects[k]->rigidbody.mass))*PHYSICS_ERRORPERCENT*contactNormal.x;
-                                    posCorrection.y = penetrationDepth/((1.0f/physicObjects[i]->rigidbody.mass) + (1.0f/physicObjects[k]->rigidbody.mass))*PHYSICS_ERRORPERCENT*contactNormal.y;
-                                    
-                                    // Fix transform positions
-                                    if (physicObjects[i]->rigidbody.enabled)
-                                    {                                        
                                         // Fix physic objects transform position
-                                        physicObjects[i]->transform.position.x -= 1.0f/physicObjects[i]->rigidbody.mass*posCorrection.x;
-                                        physicObjects[i]->transform.position.y += 1.0f/physicObjects[i]->rigidbody.mass*posCorrection.y;
+                                        physicObjects[k]->transform.position.x += 1.0f/physicObjects[k]->rigidbody.mass*posCorrection.x;
+                                        physicObjects[k]->transform.position.y -= 1.0f/physicObjects[k]->rigidbody.mass*posCorrection.y;
                                         
                                         // Update collider bounds
-                                        physicObjects[i]->collider.bounds = TransformToRectangle(physicObjects[i]->transform);
-                                        
-                                        if (physicObjects[k]->rigidbody.enabled)
-                                        {
-                                            // Fix physic objects transform position
-                                            physicObjects[k]->transform.position.x += 1.0f/physicObjects[k]->rigidbody.mass*posCorrection.x;
-                                            physicObjects[k]->transform.position.y -= 1.0f/physicObjects[k]->rigidbody.mass*posCorrection.y;
-                                            
-                                            // Update collider bounds
-                                            physicObjects[k]->collider.bounds = TransformToRectangle(physicObjects[k]->transform);
-                                        }
+                                        physicObjects[k]->collider.bounds = TransformToRectangle(physicObjects[k]->transform);
                                     }
                                 }
                             }
@@ -298,7 +486,7 @@ PhysicObject *CreatePhysicObject(Vector2 position, float rotation, Vector2 scale
     obj->rigidbody.friction = 0.0f;
     obj->rigidbody.bounciness = 0.0f;
     
-    obj->collider.enabled = false;
+    obj->collider.enabled = true;
     obj->collider.type = COLLIDER_RECTANGLE;
     obj->collider.bounds = TransformToRectangle(obj->transform);
     obj->collider.radius = 0.0f;
@@ -334,6 +522,45 @@ void DestroyPhysicObject(PhysicObject *pObj)
     physicObjectsCount--;
 }
 
+// Apply directional force to a physic object
+void ApplyForce(PhysicObject *pObj, Vector2 force)
+{
+    if (pObj->rigidbody.enabled)
+    {
+        pObj->rigidbody.velocity.x += force.x/pObj->rigidbody.mass;
+        pObj->rigidbody.velocity.y += force.y/pObj->rigidbody.mass;
+    }
+}
+
+// Apply radial force to all physic objects in range
+void ApplyForceAtPosition(Vector2 position, float force, float radius)
+{
+    for(int i = 0; i < physicObjectsCount; i++)
+    {
+        // Calculate direction and distance between force and physic object pposition
+        Vector2 distance = (Vector2){ physicObjects[i]->transform.position.x - position.x, physicObjects[i]->transform.position.y - position.y };
+
+        if(physicObjects[i]->collider.type == COLLIDER_RECTANGLE)
+        {
+            distance.x += physicObjects[i]->transform.scale.x/2;
+            distance.y += physicObjects[i]->transform.scale.y/2;
+        }
+        
+        float distanceLength = Vector2Length(distance);
+        
+        // Check if physic object is in force range
+        if(distanceLength <= radius)
+        {
+            // Normalize force direction
+            distance.x /= distanceLength;
+            distance.y /= -distanceLength;
+            
+            // Apply force to the physic object
+            ApplyForce(physicObjects[i], (Vector2){ distance.x*force, distance.y*force });
+        }
+    }
+}
+
 // Convert Transform data type to Rectangle (position and scale)
 Rectangle TransformToRectangle(Transform transform)
 {
@@ -369,3 +596,12 @@ static float Vector2DotProduct(Vector2 v1, Vector2 v2)
 
     return result;
 }
+
+static float Vector2Length(Vector2 v)
+{
+    float result;
+    
+    result = sqrt(v.x*v.x + v.y*v.y);
+    
+    return result;
+}
diff --git a/src/physac.h b/src/physac.h
index fc5502c4..852a7e4a 100644
--- a/src/physac.h
+++ b/src/physac.h
@@ -40,7 +40,7 @@ typedef struct Vector2 {
     float y;
 } Vector2;
 
-typedef enum { COLLIDER_CIRCLE, COLLIDER_RECTANGLE, COLLIDER_CAPSULE } ColliderType;
+typedef enum { COLLIDER_CIRCLE, COLLIDER_RECTANGLE } ColliderType;
 
 typedef struct Transform {
     Vector2 position;
@@ -56,7 +56,7 @@ typedef struct Rigidbody {
     bool applyGravity;
     bool isGrounded;
     float friction;         // Normalized value
-    float bounciness;       // Normalized value
+    float bounciness;
 } Rigidbody;
 
 typedef struct Collider {
@@ -81,13 +81,16 @@ extern "C" {            // Prevents name mangling of functions
 //----------------------------------------------------------------------------------
 // Module Functions Declaration
 //----------------------------------------------------------------------------------
-void InitPhysics();                                                                     // Initializes pointers array (just pointers, fixed size)
+void InitPhysics(Vector2 gravity);                                                      // Initializes pointers array (just pointers, fixed size)
 void UpdatePhysics();                                                                   // Update physic objects, calculating physic behaviours and collisions detection
 void ClosePhysics();                                                                    // Unitialize all physic objects and empty the objects pool
 
 PhysicObject *CreatePhysicObject(Vector2 position, float rotation, Vector2 scale);      // Create a new physic object dinamically, initialize it and add to pool
 void DestroyPhysicObject(PhysicObject *pObj);                                           // Destroy a specific physic object and take it out of the list
 
+void ApplyForce(PhysicObject *pObj, Vector2 force);                                     // Apply directional force to a physic object
+void ApplyForceAtPosition(Vector2 position, float force, float radius);                 // Apply radial force to all physic objects in range
+
 Rectangle TransformToRectangle(Transform transform);                                    // Convert Transform data type to Rectangle (position and scale)
 void DrawPhysicObjectInfo(PhysicObject *pObj, Vector2 position, int fontSize);          // Draw physic object information at screen position