|
|
@@ -1,8 +1,45 @@
|
|
|
/**********************************************************************************************
|
|
|
*
|
|
|
-* [physac] raylib physics module - Basic functions to apply physics to 2D objects
|
|
|
+* physac 1.0 - 2D Physics library for raylib (https://github.com/raysan5/raylib)
|
|
|
*
|
|
|
-* Copyright (c) 2016 Victor Fisac and Ramon Santamaria
|
|
|
+* // TODO: Description...
|
|
|
+*
|
|
|
+* CONFIGURATION:
|
|
|
+*
|
|
|
+* #define PHYSAC_IMPLEMENTATION
|
|
|
+* Generates the implementation of the library into the included file.
|
|
|
+* If not defined, the library is in header only mode and can be included in other headers
|
|
|
+* or source files without problems. But only ONE file should hold the implementation.
|
|
|
+*
|
|
|
+* #define PHYSAC_STATIC (defined by default)
|
|
|
+* The generated implementation will stay private inside implementation file and all
|
|
|
+* internal symbols and functions will only be visible inside that file.
|
|
|
+*
|
|
|
+* #define PHYSAC_STANDALONE
|
|
|
+* Avoid raylib.h header inclusion in this file. Data types defined on raylib are defined
|
|
|
+* internally in the library and input management and drawing functions must be provided by
|
|
|
+* the user (check library implementation for further details).
|
|
|
+*
|
|
|
+* #define PHYSAC_MALLOC()
|
|
|
+* #define PHYSAC_FREE()
|
|
|
+* You can define your own malloc/free implementation replacing stdlib.h malloc()/free() functions.
|
|
|
+* Otherwise it will include stdlib.h and use the C standard library malloc()/free() function.
|
|
|
+*
|
|
|
+* LIMITATIONS:
|
|
|
+*
|
|
|
+* // TODO.
|
|
|
+*
|
|
|
+* VERSIONS:
|
|
|
+*
|
|
|
+* 1.0 (09-Jun-2016) Module names review and converted to header-only.
|
|
|
+* 0.9 (23-Mar-2016) Complete module redesign, steps-based for better physics resolution.
|
|
|
+* 0.3 (13-Feb-2016) Reviewed to add PhysicObjects pool.
|
|
|
+* 0.2 (03-Jan-2016) Improved physics calculations.
|
|
|
+* 0.1 (30-Dec-2015) Initial release.
|
|
|
+*
|
|
|
+* LICENSE: zlib/libpng
|
|
|
+*
|
|
|
+* Copyright (c) 2016 Victor Fisac (main developer) and Ramon Santamaria
|
|
|
*
|
|
|
* This software is provided "as-is", without any express or implied warranty. In no event
|
|
|
* will the authors be held liable for any damages arising from the use of this software.
|
|
|
@@ -24,6 +61,21 @@
|
|
|
#ifndef PHYSAC_H
|
|
|
#define PHYSAC_H
|
|
|
|
|
|
+#if !defined(RAYGUI_STANDALONE)
|
|
|
+ #include "raylib.h"
|
|
|
+#endif
|
|
|
+
|
|
|
+#define PHYSAC_STATIC
|
|
|
+#ifdef PHYSAC_STATIC
|
|
|
+ #define PHYSACDEF static // Functions just visible to module including this file
|
|
|
+#else
|
|
|
+ #ifdef __cplusplus
|
|
|
+ #define PHYSACDEF extern "C" // Functions visible from other files (no name mangling of functions in C++)
|
|
|
+ #else
|
|
|
+ #define PHYSACDEF extern // Functions visible from other files
|
|
|
+ #endif
|
|
|
+#endif
|
|
|
+
|
|
|
//----------------------------------------------------------------------------------
|
|
|
// Defines and Macros
|
|
|
//----------------------------------------------------------------------------------
|
|
|
@@ -33,12 +85,28 @@
|
|
|
// Types and Structures Definition
|
|
|
// NOTE: Below types are required for PHYSAC_STANDALONE usage
|
|
|
//----------------------------------------------------------------------------------
|
|
|
+#if defined(PHYSAC_STANDALONE)
|
|
|
+ #ifndef __cplusplus
|
|
|
+ // Boolean type
|
|
|
+ #ifndef true
|
|
|
+ typedef enum { false, true } bool;
|
|
|
+ #endif
|
|
|
+ #endif
|
|
|
|
|
|
-// Vector2 type
|
|
|
-typedef struct Vector2 {
|
|
|
- float x;
|
|
|
- float y;
|
|
|
-} Vector2;
|
|
|
+ // Vector2 type
|
|
|
+ typedef struct Vector2 {
|
|
|
+ float x;
|
|
|
+ float y;
|
|
|
+ } Vector2;
|
|
|
+
|
|
|
+ // Rectangle type
|
|
|
+ typedef struct Rectangle {
|
|
|
+ int x;
|
|
|
+ int y;
|
|
|
+ int width;
|
|
|
+ int height;
|
|
|
+ } Rectangle;
|
|
|
+#endif
|
|
|
|
|
|
typedef enum { COLLIDER_CIRCLE, COLLIDER_RECTANGLE } ColliderType;
|
|
|
|
|
|
@@ -66,13 +134,13 @@ typedef struct Collider {
|
|
|
int radius; // Used for COLLIDER_CIRCLE
|
|
|
} Collider;
|
|
|
|
|
|
-typedef struct PhysicObjectData {
|
|
|
+typedef struct PhysicBodyData {
|
|
|
unsigned int id;
|
|
|
Transform transform;
|
|
|
Rigidbody rigidbody;
|
|
|
Collider collider;
|
|
|
bool enabled;
|
|
|
-} PhysicObjectData, *PhysicObject;
|
|
|
+} PhysicBodyData, *PhysicBody;
|
|
|
|
|
|
#ifdef __cplusplus
|
|
|
extern "C" { // Prevents name mangling of functions
|
|
|
@@ -81,20 +149,602 @@ extern "C" { // Prevents name mangling of functions
|
|
|
//----------------------------------------------------------------------------------
|
|
|
// Module Functions Declaration
|
|
|
//----------------------------------------------------------------------------------
|
|
|
-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
|
|
|
+PHYSACDEF void InitPhysics(Vector2 gravity); // Initializes pointers array (just pointers, fixed size)
|
|
|
+PHYSACDEF void UpdatePhysics(); // Update physic objects, calculating physic behaviours and collisions detection
|
|
|
+PHYSACDEF 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
|
|
|
+PHYSACDEF PhysicBody CreatePhysicBody(Vector2 position, float rotation, Vector2 scale); // Create a new physic body dinamically, initialize it and add to pool
|
|
|
+PHYSACDEF void DestroyPhysicBody(PhysicBody pbody); // Destroy a specific physic body 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
|
|
|
+PHYSACDEF void ApplyForce(PhysicBody pbody, Vector2 force); // Apply directional force to a physic body
|
|
|
+PHYSACDEF 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)
|
|
|
+PHYSACDEF Rectangle TransformToRectangle(Transform transform); // Convert Transform data type to Rectangle (position and scale)
|
|
|
|
|
|
#ifdef __cplusplus
|
|
|
}
|
|
|
#endif
|
|
|
|
|
|
#endif // PHYSAC_H
|
|
|
+
|
|
|
+
|
|
|
+/***********************************************************************************
|
|
|
+*
|
|
|
+* PHYSAC IMPLEMENTATION
|
|
|
+*
|
|
|
+************************************************************************************/
|
|
|
+
|
|
|
+#if defined(PHYSAC_IMPLEMENTATION)
|
|
|
+
|
|
|
+// Check if custom malloc/free functions defined, if not, using standard ones
|
|
|
+#if !defined(PHYSAC_MALLOC)
|
|
|
+ #include <stdlib.h> // Required for: malloc(), free()
|
|
|
+
|
|
|
+ #define PHYSAC_MALLOC(size) malloc(size)
|
|
|
+ #define PHYSAC_FREE(ptr) free(ptr)
|
|
|
+#endif
|
|
|
+
|
|
|
+#include <math.h> // Required for: cos(), sin(), abs(), fminf()
|
|
|
+
|
|
|
+//----------------------------------------------------------------------------------
|
|
|
+// Defines and Macros
|
|
|
+//----------------------------------------------------------------------------------
|
|
|
+#define MAX_PHYSIC_BODIES 256 // Maximum available physic bodies slots in bodies pool
|
|
|
+#define PHYSICS_STEPS 450 // Physics update steps number (divided calculations in steps per frame) to get more accurately collisions detections
|
|
|
+#define PHYSICS_ACCURACY 0.0001f // Velocity subtract operations round filter (friction)
|
|
|
+#define PHYSICS_ERRORPERCENT 0.001f // Collision resolve position fix
|
|
|
+
|
|
|
+//----------------------------------------------------------------------------------
|
|
|
+// Types and Structures Definition
|
|
|
+// NOTE: Below types are required for PHYSAC_STANDALONE usage
|
|
|
+//----------------------------------------------------------------------------------
|
|
|
+// ...
|
|
|
+
|
|
|
+//----------------------------------------------------------------------------------
|
|
|
+// Global Variables Definition
|
|
|
+//----------------------------------------------------------------------------------
|
|
|
+static PhysicBody physicBodies[MAX_PHYSIC_BODIES]; // Physic bodies pool
|
|
|
+static int physicBodiesCount; // Counts current enabled physic bodies
|
|
|
+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)
|
|
|
+PHYSACDEF void InitPhysics(Vector2 gravity)
|
|
|
+{
|
|
|
+ // Initialize physics variables
|
|
|
+ physicBodiesCount = 0;
|
|
|
+ gravityForce = gravity;
|
|
|
+}
|
|
|
+
|
|
|
+// Update physic objects, calculating physic behaviours and collisions detection
|
|
|
+PHYSACDEF void UpdatePhysics()
|
|
|
+{
|
|
|
+ // Reset all physic objects is grounded state
|
|
|
+ for (int i = 0; i < physicBodiesCount; i++) physicBodies[i]->rigidbody.isGrounded = false;
|
|
|
+
|
|
|
+ for (int steps = 0; steps < PHYSICS_STEPS; steps++)
|
|
|
+ {
|
|
|
+ for (int i = 0; i < physicBodiesCount; i++)
|
|
|
+ {
|
|
|
+ if (physicBodies[i]->enabled)
|
|
|
+ {
|
|
|
+ // Update physic behaviour
|
|
|
+ if (physicBodies[i]->rigidbody.enabled)
|
|
|
+ {
|
|
|
+ // Apply friction to acceleration in X axis
|
|
|
+ if (physicBodies[i]->rigidbody.acceleration.x > PHYSICS_ACCURACY) physicBodies[i]->rigidbody.acceleration.x -= physicBodies[i]->rigidbody.friction/PHYSICS_STEPS;
|
|
|
+ else if (physicBodies[i]->rigidbody.acceleration.x < PHYSICS_ACCURACY) physicBodies[i]->rigidbody.acceleration.x += physicBodies[i]->rigidbody.friction/PHYSICS_STEPS;
|
|
|
+ else physicBodies[i]->rigidbody.acceleration.x = 0.0f;
|
|
|
+
|
|
|
+ // Apply friction to acceleration in Y axis
|
|
|
+ if (physicBodies[i]->rigidbody.acceleration.y > PHYSICS_ACCURACY) physicBodies[i]->rigidbody.acceleration.y -= physicBodies[i]->rigidbody.friction/PHYSICS_STEPS;
|
|
|
+ else if (physicBodies[i]->rigidbody.acceleration.y < PHYSICS_ACCURACY) physicBodies[i]->rigidbody.acceleration.y += physicBodies[i]->rigidbody.friction/PHYSICS_STEPS;
|
|
|
+ else physicBodies[i]->rigidbody.acceleration.y = 0.0f;
|
|
|
+
|
|
|
+ // Apply friction to velocity in X axis
|
|
|
+ if (physicBodies[i]->rigidbody.velocity.x > PHYSICS_ACCURACY) physicBodies[i]->rigidbody.velocity.x -= physicBodies[i]->rigidbody.friction/PHYSICS_STEPS;
|
|
|
+ else if (physicBodies[i]->rigidbody.velocity.x < PHYSICS_ACCURACY) physicBodies[i]->rigidbody.velocity.x += physicBodies[i]->rigidbody.friction/PHYSICS_STEPS;
|
|
|
+ else physicBodies[i]->rigidbody.velocity.x = 0.0f;
|
|
|
+
|
|
|
+ // Apply friction to velocity in Y axis
|
|
|
+ if (physicBodies[i]->rigidbody.velocity.y > PHYSICS_ACCURACY) physicBodies[i]->rigidbody.velocity.y -= physicBodies[i]->rigidbody.friction/PHYSICS_STEPS;
|
|
|
+ else if (physicBodies[i]->rigidbody.velocity.y < PHYSICS_ACCURACY) physicBodies[i]->rigidbody.velocity.y += physicBodies[i]->rigidbody.friction/PHYSICS_STEPS;
|
|
|
+ else physicBodies[i]->rigidbody.velocity.y = 0.0f;
|
|
|
+
|
|
|
+ // Apply gravity to velocity
|
|
|
+ if (physicBodies[i]->rigidbody.applyGravity)
|
|
|
+ {
|
|
|
+ physicBodies[i]->rigidbody.velocity.x += gravityForce.x/PHYSICS_STEPS;
|
|
|
+ physicBodies[i]->rigidbody.velocity.y += gravityForce.y/PHYSICS_STEPS;
|
|
|
+ }
|
|
|
+
|
|
|
+ // Apply acceleration to velocity
|
|
|
+ physicBodies[i]->rigidbody.velocity.x += physicBodies[i]->rigidbody.acceleration.x/PHYSICS_STEPS;
|
|
|
+ physicBodies[i]->rigidbody.velocity.y += physicBodies[i]->rigidbody.acceleration.y/PHYSICS_STEPS;
|
|
|
+
|
|
|
+ // Apply velocity to position
|
|
|
+ physicBodies[i]->transform.position.x += physicBodies[i]->rigidbody.velocity.x/PHYSICS_STEPS;
|
|
|
+ physicBodies[i]->transform.position.y -= physicBodies[i]->rigidbody.velocity.y/PHYSICS_STEPS;
|
|
|
+ }
|
|
|
+
|
|
|
+ // Update collision detection
|
|
|
+ if (physicBodies[i]->collider.enabled)
|
|
|
+ {
|
|
|
+ // Update collider bounds
|
|
|
+ physicBodies[i]->collider.bounds = TransformToRectangle(physicBodies[i]->transform);
|
|
|
+
|
|
|
+ // Check collision with other colliders
|
|
|
+ for (int k = 0; k < physicBodiesCount; k++)
|
|
|
+ {
|
|
|
+ if (physicBodies[k]->collider.enabled && i != k)
|
|
|
+ {
|
|
|
+ // 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 (physicBodies[i]->collider.type)
|
|
|
+ {
|
|
|
+ case COLLIDER_RECTANGLE:
|
|
|
+ {
|
|
|
+ switch (physicBodies[k]->collider.type)
|
|
|
+ {
|
|
|
+ case COLLIDER_RECTANGLE:
|
|
|
+ {
|
|
|
+ // Check if colliders are overlapped
|
|
|
+ if (CheckCollisionRecs(physicBodies[i]->collider.bounds, physicBodies[k]->collider.bounds))
|
|
|
+ {
|
|
|
+ // Calculate direction vector from i to k
|
|
|
+ direction.x = (physicBodies[k]->transform.position.x + physicBodies[k]->transform.scale.x/2) - (physicBodies[i]->transform.position.x + physicBodies[i]->transform.scale.x/2);
|
|
|
+ direction.y = (physicBodies[k]->transform.position.y + physicBodies[k]->transform.scale.y/2) - (physicBodies[i]->transform.position.y + physicBodies[i]->transform.scale.y/2);
|
|
|
+
|
|
|
+ // Define overlapping and penetration attributes
|
|
|
+ Vector2 overlap;
|
|
|
+
|
|
|
+ // Calculate overlap on X axis
|
|
|
+ overlap.x = (physicBodies[i]->transform.scale.x + physicBodies[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 = (physicBodies[i]->transform.scale.y + physicBodies[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(physicBodies[k]->transform.position, physicBodies[k]->collider.radius, physicBodies[i]->collider.bounds))
|
|
|
+ {
|
|
|
+ // Calculate direction vector between circles
|
|
|
+ direction.x = physicBodies[k]->transform.position.x - physicBodies[i]->transform.position.x + physicBodies[i]->transform.scale.x/2;
|
|
|
+ direction.y = physicBodies[k]->transform.position.y - physicBodies[i]->transform.position.y + physicBodies[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 = physicBodies[i]->collider.bounds.x + physicBodies[i]->collider.bounds.width;
|
|
|
+ else closestPoint.x = physicBodies[i]->collider.bounds.x;
|
|
|
+
|
|
|
+ if (direction.y > 0.0f) closestPoint.y = physicBodies[i]->collider.bounds.y + physicBodies[i]->collider.bounds.height;
|
|
|
+ else closestPoint.y = physicBodies[i]->collider.bounds.y;
|
|
|
+
|
|
|
+ // Check if the closest point is inside the circle
|
|
|
+ if (CheckCollisionPointCircle(closestPoint, physicBodies[k]->transform.position, physicBodies[k]->collider.radius))
|
|
|
+ {
|
|
|
+ // Recalculate direction based on closest point position
|
|
|
+ direction.x = physicBodies[k]->transform.position.x - closestPoint.x;
|
|
|
+ direction.y = physicBodies[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 = physicBodies[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(physicBodies[i]->collider.bounds.y - physicBodies[k]->transform.position.y - physicBodies[k]->collider.radius);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ contactNormal = (Vector2){ 0.0f, 1.0f };
|
|
|
+ penetrationDepth = fabs(physicBodies[i]->collider.bounds.y - physicBodies[k]->transform.position.y + physicBodies[k]->collider.radius);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ // Calculate final contact normal
|
|
|
+ if (direction.x > 0.0f)
|
|
|
+ {
|
|
|
+ contactNormal = (Vector2){ 1.0f, 0.0f };
|
|
|
+ penetrationDepth = fabs(physicBodies[k]->transform.position.x + physicBodies[k]->collider.radius - physicBodies[i]->collider.bounds.x);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ contactNormal = (Vector2){ -1.0f, 0.0f };
|
|
|
+ penetrationDepth = fabs(physicBodies[i]->collider.bounds.x + physicBodies[i]->collider.bounds.width - physicBodies[k]->transform.position.x - physicBodies[k]->collider.radius);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ } break;
|
|
|
+ }
|
|
|
+ } break;
|
|
|
+ case COLLIDER_CIRCLE:
|
|
|
+ {
|
|
|
+ switch (physicBodies[k]->collider.type)
|
|
|
+ {
|
|
|
+ case COLLIDER_RECTANGLE:
|
|
|
+ {
|
|
|
+ if (CheckCollisionCircleRec(physicBodies[i]->transform.position, physicBodies[i]->collider.radius, physicBodies[k]->collider.bounds))
|
|
|
+ {
|
|
|
+ // Calculate direction vector between circles
|
|
|
+ direction.x = physicBodies[k]->transform.position.x + physicBodies[i]->transform.scale.x/2 - physicBodies[i]->transform.position.x;
|
|
|
+ direction.y = physicBodies[k]->transform.position.y + physicBodies[i]->transform.scale.y/2 - physicBodies[i]->transform.position.y;
|
|
|
+
|
|
|
+ // Calculate closest point on rectangle to circle
|
|
|
+ Vector2 closestPoint = { 0.0f, 0.0f };
|
|
|
+ if (direction.x > 0.0f) closestPoint.x = physicBodies[k]->collider.bounds.x + physicBodies[k]->collider.bounds.width;
|
|
|
+ else closestPoint.x = physicBodies[k]->collider.bounds.x;
|
|
|
+
|
|
|
+ if (direction.y > 0.0f) closestPoint.y = physicBodies[k]->collider.bounds.y + physicBodies[k]->collider.bounds.height;
|
|
|
+ else closestPoint.y = physicBodies[k]->collider.bounds.y;
|
|
|
+
|
|
|
+ // Check if the closest point is inside the circle
|
|
|
+ if (CheckCollisionPointCircle(closestPoint, physicBodies[i]->transform.position, physicBodies[i]->collider.radius))
|
|
|
+ {
|
|
|
+ // Recalculate direction based on closest point position
|
|
|
+ direction.x = physicBodies[i]->transform.position.x - closestPoint.x;
|
|
|
+ direction.y = physicBodies[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 = physicBodies[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(physicBodies[k]->collider.bounds.y - physicBodies[i]->transform.position.y - physicBodies[i]->collider.radius);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ contactNormal = (Vector2){ 0.0f, 1.0f };
|
|
|
+ penetrationDepth = fabs(physicBodies[k]->collider.bounds.y - physicBodies[i]->transform.position.y + physicBodies[i]->collider.radius);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ // Calculate final contact normal and penetration depth
|
|
|
+ if (direction.x > 0.0f)
|
|
|
+ {
|
|
|
+ contactNormal = (Vector2){ 1.0f, 0.0f };
|
|
|
+ penetrationDepth = fabs(physicBodies[i]->transform.position.x + physicBodies[i]->collider.radius - physicBodies[k]->collider.bounds.x);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ contactNormal = (Vector2){ -1.0f, 0.0f };
|
|
|
+ penetrationDepth = fabs(physicBodies[k]->collider.bounds.x + physicBodies[k]->collider.bounds.width - physicBodies[i]->transform.position.x - physicBodies[i]->collider.radius);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ } break;
|
|
|
+ case COLLIDER_CIRCLE:
|
|
|
+ {
|
|
|
+ // Check if colliders are overlapped
|
|
|
+ if (CheckCollisionCircles(physicBodies[i]->transform.position, physicBodies[i]->collider.radius, physicBodies[k]->transform.position, physicBodies[k]->collider.radius))
|
|
|
+ {
|
|
|
+ // Calculate direction vector between circles
|
|
|
+ direction.x = physicBodies[k]->transform.position.x - physicBodies[i]->transform.position.x;
|
|
|
+ direction.y = physicBodies[k]->transform.position.y - physicBodies[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 (physicBodies[i]->rigidbody.enabled)
|
|
|
+ {
|
|
|
+ if (contactNormal.y < 0.0f) physicBodies[i]->rigidbody.isGrounded = true;
|
|
|
+ }
|
|
|
+
|
|
|
+ // 2. Calculate collision impulse
|
|
|
+ // -------------------------------------------------------------------------------------------------------------------------------------
|
|
|
+
|
|
|
+ // Calculate relative velocity
|
|
|
+ Vector2 relVelocity = { 0.0f, 0.0f };
|
|
|
+ relVelocity.x = physicBodies[k]->rigidbody.velocity.x - physicBodies[i]->rigidbody.velocity.x;
|
|
|
+ relVelocity.y = physicBodies[k]->rigidbody.velocity.y - physicBodies[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(physicBodies[i]->rigidbody.bounciness, physicBodies[k]->rigidbody.bounciness);
|
|
|
+
|
|
|
+ // Calculate impulse scalar value
|
|
|
+ float j = -(1.0f + e)*velAlongNormal;
|
|
|
+ j /= 1.0f/physicBodies[i]->rigidbody.mass + 1.0f/physicBodies[k]->rigidbody.mass;
|
|
|
+
|
|
|
+ // Calculate final impulse vector
|
|
|
+ Vector2 impulse = { j*contactNormal.x, j*contactNormal.y };
|
|
|
+
|
|
|
+ // Calculate collision mass ration
|
|
|
+ float massSum = physicBodies[i]->rigidbody.mass + physicBodies[k]->rigidbody.mass;
|
|
|
+ float ratio = 0.0f;
|
|
|
+
|
|
|
+ // Apply impulse to current rigidbodies velocities if they are enabled
|
|
|
+ if (physicBodies[i]->rigidbody.enabled)
|
|
|
+ {
|
|
|
+ // Calculate inverted mass ration
|
|
|
+ ratio = physicBodies[i]->rigidbody.mass/massSum;
|
|
|
+
|
|
|
+ // Apply impulse direction to velocity
|
|
|
+ physicBodies[i]->rigidbody.velocity.x -= impulse.x*ratio*(1.0f+physicBodies[i]->rigidbody.bounciness);
|
|
|
+ physicBodies[i]->rigidbody.velocity.y -= impulse.y*ratio*(1.0f+physicBodies[i]->rigidbody.bounciness);
|
|
|
+ }
|
|
|
+
|
|
|
+ if (physicBodies[k]->rigidbody.enabled)
|
|
|
+ {
|
|
|
+ // Calculate inverted mass ration
|
|
|
+ ratio = physicBodies[k]->rigidbody.mass/massSum;
|
|
|
+
|
|
|
+ // Apply impulse direction to velocity
|
|
|
+ physicBodies[k]->rigidbody.velocity.x += impulse.x*ratio*(1.0f+physicBodies[i]->rigidbody.bounciness);
|
|
|
+ physicBodies[k]->rigidbody.velocity.y += impulse.y*ratio*(1.0f+physicBodies[i]->rigidbody.bounciness);
|
|
|
+ }
|
|
|
+
|
|
|
+ // 3. Correct colliders overlaping (transform position)
|
|
|
+ // ---------------------------------------------------------------------------------------------------------------------------------
|
|
|
+
|
|
|
+ // Calculate transform position penetration correction
|
|
|
+ Vector2 posCorrection;
|
|
|
+ posCorrection.x = penetrationDepth/((1.0f/physicBodies[i]->rigidbody.mass) + (1.0f/physicBodies[k]->rigidbody.mass))*PHYSICS_ERRORPERCENT*contactNormal.x;
|
|
|
+ posCorrection.y = penetrationDepth/((1.0f/physicBodies[i]->rigidbody.mass) + (1.0f/physicBodies[k]->rigidbody.mass))*PHYSICS_ERRORPERCENT*contactNormal.y;
|
|
|
+
|
|
|
+ // Fix transform positions
|
|
|
+ if (physicBodies[i]->rigidbody.enabled)
|
|
|
+ {
|
|
|
+ // Fix physic objects transform position
|
|
|
+ physicBodies[i]->transform.position.x -= 1.0f/physicBodies[i]->rigidbody.mass*posCorrection.x;
|
|
|
+ physicBodies[i]->transform.position.y += 1.0f/physicBodies[i]->rigidbody.mass*posCorrection.y;
|
|
|
+
|
|
|
+ // Update collider bounds
|
|
|
+ physicBodies[i]->collider.bounds = TransformToRectangle(physicBodies[i]->transform);
|
|
|
+
|
|
|
+ if (physicBodies[k]->rigidbody.enabled)
|
|
|
+ {
|
|
|
+ // Fix physic objects transform position
|
|
|
+ physicBodies[k]->transform.position.x += 1.0f/physicBodies[k]->rigidbody.mass*posCorrection.x;
|
|
|
+ physicBodies[k]->transform.position.y -= 1.0f/physicBodies[k]->rigidbody.mass*posCorrection.y;
|
|
|
+
|
|
|
+ // Update collider bounds
|
|
|
+ physicBodies[k]->collider.bounds = TransformToRectangle(physicBodies[k]->transform);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+// Unitialize all physic objects and empty the objects pool
|
|
|
+PHYSACDEF void ClosePhysics()
|
|
|
+{
|
|
|
+ // Free all dynamic memory allocations
|
|
|
+ for (int i = 0; i < physicBodiesCount; i++) PHYSAC_FREE(physicBodies[i]);
|
|
|
+
|
|
|
+ // Reset enabled physic objects count
|
|
|
+ physicBodiesCount = 0;
|
|
|
+}
|
|
|
+
|
|
|
+// Create a new physic body dinamically, initialize it and add to pool
|
|
|
+PHYSACDEF PhysicBody CreatePhysicBody(Vector2 position, float rotation, Vector2 scale)
|
|
|
+{
|
|
|
+ // Allocate dynamic memory
|
|
|
+ PhysicBody obj = (PhysicBody)PHYSAC_MALLOC(sizeof(PhysicBodyData));
|
|
|
+
|
|
|
+ // Initialize physic body values with generic values
|
|
|
+ obj->id = physicBodiesCount;
|
|
|
+ obj->enabled = true;
|
|
|
+
|
|
|
+ obj->transform = (Transform){ (Vector2){ position.x - scale.x/2, position.y - scale.y/2 }, rotation, scale };
|
|
|
+
|
|
|
+ obj->rigidbody.enabled = false;
|
|
|
+ obj->rigidbody.mass = 1.0f;
|
|
|
+ obj->rigidbody.acceleration = (Vector2){ 0.0f, 0.0f };
|
|
|
+ obj->rigidbody.velocity = (Vector2){ 0.0f, 0.0f };
|
|
|
+ obj->rigidbody.applyGravity = false;
|
|
|
+ obj->rigidbody.isGrounded = false;
|
|
|
+ obj->rigidbody.friction = 0.0f;
|
|
|
+ obj->rigidbody.bounciness = 0.0f;
|
|
|
+
|
|
|
+ obj->collider.enabled = true;
|
|
|
+ obj->collider.type = COLLIDER_RECTANGLE;
|
|
|
+ obj->collider.bounds = TransformToRectangle(obj->transform);
|
|
|
+ obj->collider.radius = 0.0f;
|
|
|
+
|
|
|
+ // Add new physic body to the pointers array
|
|
|
+ physicBodies[physicBodiesCount] = obj;
|
|
|
+
|
|
|
+ // Increase enabled physic bodies count
|
|
|
+ physicBodiesCount++;
|
|
|
+
|
|
|
+ return obj;
|
|
|
+}
|
|
|
+
|
|
|
+// Destroy a specific physic body and take it out of the list
|
|
|
+PHYSACDEF void DestroyPhysicBody(PhysicBody pbody)
|
|
|
+{
|
|
|
+ // Free dynamic memory allocation
|
|
|
+ PHYSAC_FREE(physicBodies[pbody->id]);
|
|
|
+
|
|
|
+ // Remove *obj from the pointers array
|
|
|
+ for (int i = pbody->id; i < physicBodiesCount; i++)
|
|
|
+ {
|
|
|
+ // Resort all the following pointers of the array
|
|
|
+ if ((i + 1) < physicBodiesCount)
|
|
|
+ {
|
|
|
+ physicBodies[i] = physicBodies[i + 1];
|
|
|
+ physicBodies[i]->id = physicBodies[i + 1]->id;
|
|
|
+ }
|
|
|
+ else PHYSAC_FREE(physicBodies[i]);
|
|
|
+ }
|
|
|
+
|
|
|
+ // Decrease enabled physic bodies count
|
|
|
+ physicBodiesCount--;
|
|
|
+}
|
|
|
+
|
|
|
+// Apply directional force to a physic body
|
|
|
+PHYSACDEF void ApplyForce(PhysicBody pbody, Vector2 force)
|
|
|
+{
|
|
|
+ if (pbody->rigidbody.enabled)
|
|
|
+ {
|
|
|
+ pbody->rigidbody.velocity.x += force.x/pbody->rigidbody.mass;
|
|
|
+ pbody->rigidbody.velocity.y += force.y/pbody->rigidbody.mass;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+// Apply radial force to all physic objects in range
|
|
|
+PHYSACDEF void ApplyForceAtPosition(Vector2 position, float force, float radius)
|
|
|
+{
|
|
|
+ for (int i = 0; i < physicBodiesCount; i++)
|
|
|
+ {
|
|
|
+ if (physicBodies[i]->rigidbody.enabled)
|
|
|
+ {
|
|
|
+ // Calculate direction and distance between force and physic body position
|
|
|
+ Vector2 distance = (Vector2){ physicBodies[i]->transform.position.x - position.x, physicBodies[i]->transform.position.y - position.y };
|
|
|
+
|
|
|
+ if (physicBodies[i]->collider.type == COLLIDER_RECTANGLE)
|
|
|
+ {
|
|
|
+ distance.x += physicBodies[i]->transform.scale.x/2;
|
|
|
+ distance.y += physicBodies[i]->transform.scale.y/2;
|
|
|
+ }
|
|
|
+
|
|
|
+ float distanceLength = Vector2Length(distance);
|
|
|
+
|
|
|
+ // Check if physic body is in force range
|
|
|
+ if (distanceLength <= radius)
|
|
|
+ {
|
|
|
+ // Normalize force direction
|
|
|
+ distance.x /= distanceLength;
|
|
|
+ distance.y /= -distanceLength;
|
|
|
+
|
|
|
+ // Calculate final force
|
|
|
+ Vector2 finalForce = { distance.x*force, distance.y*force };
|
|
|
+
|
|
|
+ // Apply force to the physic body
|
|
|
+ ApplyForce(physicBodies[i], finalForce);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+// Convert Transform data type to Rectangle (position and scale)
|
|
|
+PHYSACDEF Rectangle TransformToRectangle(Transform transform)
|
|
|
+{
|
|
|
+ return (Rectangle){transform.position.x, transform.position.y, transform.scale.x, transform.scale.y};
|
|
|
+}
|
|
|
+
|
|
|
+//----------------------------------------------------------------------------------
|
|
|
+// Module specific Functions Definition
|
|
|
+//----------------------------------------------------------------------------------
|
|
|
+
|
|
|
+// Returns the dot product of two Vector2
|
|
|
+static float Vector2DotProduct(Vector2 v1, Vector2 v2)
|
|
|
+{
|
|
|
+ float result;
|
|
|
+
|
|
|
+ result = v1.x*v2.x + v1.y*v2.y;
|
|
|
+
|
|
|
+ return result;
|
|
|
+}
|
|
|
+
|
|
|
+static float Vector2Length(Vector2 v)
|
|
|
+{
|
|
|
+ float result;
|
|
|
+
|
|
|
+ result = sqrt(v.x*v.x + v.y*v.y);
|
|
|
+
|
|
|
+ return result;
|
|
|
+}
|
|
|
+
|
|
|
+#endif // PHYSAC_IMPLEMENTATION
|
raysan5