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.

src/physac.c

@@ -36,7 +36,6 @@
 // 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
@@ -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;
@@ -119,9 +135,6 @@ void UpdatePhysics()
                     for (int k = 0; k < physicObjectsCount; k++)
                     {
                         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)
@@ -130,17 +143,28 @@ void UpdatePhysics()
                             // 1. Calculate collision normal
                             // -------------------------------------------------------------------------------------------------------------------------------------
                             
-                                // Define collision ontact 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)
+                            {
+                                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
-                                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);
                                                 
                                                 // Define overlapping and penetration attributes
                                                 Vector2 overlap;
-                                float penetrationDepth = 0.0f;
 
                                                 // Calculate overlap on X axis
                                                 overlap.x = (physicObjects[i]->transform.scale.x + physicObjects[k]->transform.scale.x)/2 - abs(direction.x);
@@ -175,6 +199,169 @@ void UpdatePhysics()
                                                         }
                                                     }
                                                 }
+                                            }
+                                        } 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)
@@ -186,7 +373,9 @@ void UpdatePhysics()
                             // -------------------------------------------------------------------------------------------------------------------------------------
                             
                             // 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 };
+                            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);
@@ -198,7 +387,7 @@ void UpdatePhysics()
                                 float e = fminf(physicObjects[i]->rigidbody.bounciness, physicObjects[k]->rigidbody.bounciness);
                                 
                                 // Calculate impulse scalar value
-                                    float j = -(1.0f + e) * velAlongNormal;
+                                float j = -(1.0f + e)*velAlongNormal;
                                 j /= 1.0f/physicObjects[i]->rigidbody.mass + 1.0f/physicObjects[k]->rigidbody.mass;
                                 
                                 // Calculate final impulse vector
@@ -215,8 +404,8 @@ void UpdatePhysics()
                                     ratio = physicObjects[i]->rigidbody.mass/massSum;
                                     
                                     // Apply impulse direction to velocity
-                                        physicObjects[i]->rigidbody.velocity.x -= impulse.x*ratio;
+                                    physicObjects[i]->rigidbody.velocity.x -= impulse.x*ratio*(1.0f+physicObjects[i]->rigidbody.bounciness);
-                                        physicObjects[i]->rigidbody.velocity.y -= impulse.y*ratio;
+                                    physicObjects[i]->rigidbody.velocity.y -= impulse.y*ratio*(1.0f+physicObjects[i]->rigidbody.bounciness);
                                 }
                                 
                                 if (physicObjects[k]->rigidbody.enabled) 
@@ -225,8 +414,8 @@ void UpdatePhysics()
                                     ratio = physicObjects[k]->rigidbody.mass/massSum;
                                     
                                     // Apply impulse direction to velocity
-                                        physicObjects[k]->rigidbody.velocity.x += impulse.x*ratio;
+                                    physicObjects[k]->rigidbody.velocity.x += impulse.x*ratio*(1.0f+physicObjects[i]->rigidbody.bounciness);
-                                        physicObjects[k]->rigidbody.velocity.y += impulse.y*ratio;
+                                    physicObjects[k]->rigidbody.velocity.y += impulse.y*ratio*(1.0f+physicObjects[i]->rigidbody.bounciness);
                                 }
                                 
                                 // 3. Correct colliders overlaping (transform position)
@@ -264,7 +453,6 @@ void UpdatePhysics()
             }
         }
     }
-    }
 }
 
 // Unitialize all physic objects and empty the objects pool
@@ -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;
+}

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