|
|
@ -1,8 +1,45 @@ |
|
|
|
/********************************************************************************************** |
|
|
|
* |
|
|
|
* p">[physac] raylib physics module - Basic functions to apply physics to 2D objects |
|
|
|
* n">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 64 // Physics update steps per frame for improved collision-detection |
|
|
|
#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 |