824 lines
44 KiB
C
824 lines
44 KiB
C
/**********************************************************************************************
|
|
*
|
|
* physac 1.0 - 2D Physics library for raylib (https://github.com/raysan5/raylib)
|
|
*
|
|
* // 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.
|
|
*
|
|
* Permission is granted to anyone to use this software for any purpose, including commercial
|
|
* applications, and to alter it and redistribute it freely, subject to the following restrictions:
|
|
*
|
|
* 1. The origin of this software must not be misrepresented; you must not claim that you
|
|
* wrote the original software. If you use this software in a product, an acknowledgment
|
|
* in the product documentation would be appreciated but is not required.
|
|
*
|
|
* 2. Altered source versions must be plainly marked as such, and must not be misrepresented
|
|
* as being the original software.
|
|
*
|
|
* 3. This notice may not be removed or altered from any source distribution.
|
|
*
|
|
**********************************************************************************************/
|
|
|
|
#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
|
|
//----------------------------------------------------------------------------------
|
|
// ...
|
|
|
|
//----------------------------------------------------------------------------------
|
|
// 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;
|
|
|
|
// Rectangle type
|
|
typedef struct Rectangle {
|
|
int x;
|
|
int y;
|
|
int width;
|
|
int height;
|
|
} Rectangle;
|
|
#endif
|
|
|
|
typedef enum { COLLIDER_CIRCLE, COLLIDER_RECTANGLE } ColliderType;
|
|
|
|
typedef struct Transform {
|
|
Vector2 position;
|
|
float rotation; // Radians (not used)
|
|
Vector2 scale; // Just for rectangle physic objects, for circle physic objects use collider radius and keep scale as { 0, 0 }
|
|
} Transform;
|
|
|
|
typedef struct Rigidbody {
|
|
bool enabled; // Acts as kinematic state (collisions are calculated anyway)
|
|
float mass;
|
|
Vector2 acceleration;
|
|
Vector2 velocity;
|
|
bool applyGravity;
|
|
bool isGrounded;
|
|
float friction; // Normalized value
|
|
float bounciness;
|
|
} Rigidbody;
|
|
|
|
typedef struct Collider {
|
|
bool enabled;
|
|
ColliderType type;
|
|
Rectangle bounds; // Used for COLLIDER_RECTANGLE
|
|
int radius; // Used for COLLIDER_CIRCLE
|
|
} Collider;
|
|
|
|
typedef struct PhysicBodyData {
|
|
unsigned int id;
|
|
Transform transform;
|
|
Rigidbody rigidbody;
|
|
Collider collider;
|
|
bool enabled;
|
|
} PhysicBodyData, *PhysicBody;
|
|
|
|
//----------------------------------------------------------------------------------
|
|
// Module Functions Declaration
|
|
//----------------------------------------------------------------------------------
|
|
PHYSACDEF void InitPhysics(Vector2 gravity); // Initializes pointers array (just pointers, fixed size)
|
|
PHYSACDEF void* PhysicsThread(void *arg); // Physics calculations thread function
|
|
PHYSACDEF void ClosePhysics(); // Unitialize all physic objects and empty the objects pool
|
|
|
|
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
|
|
|
|
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
|
|
|
|
PHYSACDEF Rectangle TransformToRectangle(Transform transform); // Convert Transform data type to Rectangle (position and scale)
|
|
|
|
#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()
|
|
#include <stdint.h> // Required for typedef unsigned long long int uint64_t, used by hi-res timer
|
|
|
|
#ifndef PHYSAC_NO_THREADS
|
|
#include <pthread.h> // Required for: pthread_create()
|
|
#endif
|
|
|
|
#if defined(PLATFORM_DESKTOP)
|
|
// Functions required to query time on Windows
|
|
int __stdcall QueryPerformanceCounter(unsigned long long int *lpPerformanceCount);
|
|
int __stdcall QueryPerformanceFrequency(unsigned long long int *lpFrequency);
|
|
#elif defined(PLATFORM_ANDROID) || defined(PLATFORM_RPI)
|
|
#include <sys/time.h> // Required for: timespec
|
|
#include <time.h> // Required for: clock_gettime()
|
|
#endif
|
|
|
|
//----------------------------------------------------------------------------------
|
|
// Defines and Macros
|
|
//----------------------------------------------------------------------------------
|
|
#define MAX_PHYSIC_BODIES 256 // Maximum available physic bodies slots in bodies pool
|
|
#define PHYSICS_TIMESTEP 0.016666 // Physics fixed time step (1/fps)
|
|
#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 bool physicsThreadEnabled = false; // Physics calculations thread exit control
|
|
static uint64_t baseTime; // Base time measure for hi-res timer
|
|
static double currentTime, previousTime; // Used to track timmings
|
|
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 void UpdatePhysics(double deltaTime); // Update physic objects, calculating physic behaviours and collisions detection
|
|
static void InitTimer(void); // Initialize hi-resolution timer
|
|
static double GetCurrentTime(void); // Time measure returned are microseconds
|
|
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;
|
|
|
|
#ifndef PHYSAC_NO_THREADS // NOTE: if defined, user will need to create a thread for PhysicsThread function manually
|
|
// Create physics thread
|
|
pthread_t tid;
|
|
pthread_create(&tid, NULL, &PhysicsThread, NULL);
|
|
#endif
|
|
}
|
|
|
|
// Unitialize all physic objects and empty the objects pool
|
|
PHYSACDEF void ClosePhysics()
|
|
{
|
|
// Exit physics thread loop
|
|
physicsThreadEnabled = false;
|
|
|
|
// 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};
|
|
}
|
|
|
|
// Physics calculations thread function
|
|
PHYSACDEF void* PhysicsThread(void *arg)
|
|
{
|
|
// Initialize thread loop state
|
|
physicsThreadEnabled = true;
|
|
|
|
// Initialize hi-resolution timer
|
|
InitTimer();
|
|
|
|
// Physics update loop
|
|
while (physicsThreadEnabled)
|
|
{
|
|
currentTime = GetCurrentTime();
|
|
double deltaTime = (double)(currentTime - previousTime);
|
|
previousTime = currentTime;
|
|
|
|
// Delta time value needs to be inverse multiplied by physics time step value (1/target fps)
|
|
UpdatePhysics(deltaTime/PHYSICS_TIMESTEP);
|
|
}
|
|
|
|
return NULL;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------------
|
|
// Module specific Functions Definition
|
|
//----------------------------------------------------------------------------------
|
|
// Initialize hi-resolution timer
|
|
static void InitTimer(void)
|
|
{
|
|
#if defined(PLATFORM_ANDROID) || defined(PLATFORM_RPI)
|
|
struct timespec now;
|
|
|
|
if (clock_gettime(CLOCK_MONOTONIC, &now) == 0) // Success
|
|
{
|
|
baseTime = (uint64_t)now.tv_sec*1000000000LLU + (uint64_t)now.tv_nsec;
|
|
}
|
|
#endif
|
|
|
|
previousTime = GetCurrentTime(); // Get time as double
|
|
}
|
|
|
|
// Time measure returned are microseconds
|
|
static double GetCurrentTime(void)
|
|
{
|
|
double time;
|
|
|
|
#if defined(PLATFORM_DESKTOP)
|
|
unsigned long long int clockFrequency, currentTime;
|
|
|
|
QueryPerformanceFrequency(&clockFrequency);
|
|
QueryPerformanceCounter(¤tTime);
|
|
|
|
time = (double)((double)currentTime/(double)clockFrequency);
|
|
#endif
|
|
|
|
#if defined(PLATFORM_ANDROID) || defined(PLATFORM_RPI)
|
|
struct timespec ts;
|
|
clock_gettime(CLOCK_MONOTONIC, &ts);
|
|
uint64_t temp = (uint64_t)ts.tv_sec*1000000000LLU + (uint64_t)ts.tv_nsec;
|
|
|
|
time = (double)(temp - baseTime)*1e-9;
|
|
#endif
|
|
|
|
return time;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
|
|
// Update physic objects, calculating physic behaviours and collisions detection
|
|
static void UpdatePhysics(double deltaTime)
|
|
{
|
|
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*deltaTime;
|
|
else if (physicBodies[i]->rigidbody.acceleration.x < PHYSICS_ACCURACY) physicBodies[i]->rigidbody.acceleration.x += physicBodies[i]->rigidbody.friction*deltaTime;
|
|
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*deltaTime;
|
|
else if (physicBodies[i]->rigidbody.acceleration.y < PHYSICS_ACCURACY) physicBodies[i]->rigidbody.acceleration.y += physicBodies[i]->rigidbody.friction*deltaTime;
|
|
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*deltaTime;
|
|
else if (physicBodies[i]->rigidbody.velocity.x < PHYSICS_ACCURACY) physicBodies[i]->rigidbody.velocity.x += physicBodies[i]->rigidbody.friction*deltaTime;
|
|
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*deltaTime;
|
|
else if (physicBodies[i]->rigidbody.velocity.y < PHYSICS_ACCURACY) physicBodies[i]->rigidbody.velocity.y += physicBodies[i]->rigidbody.friction*deltaTime;
|
|
else physicBodies[i]->rigidbody.velocity.y = 0.0f;
|
|
|
|
// Apply gravity to velocity
|
|
if (physicBodies[i]->rigidbody.applyGravity)
|
|
{
|
|
physicBodies[i]->rigidbody.velocity.x += gravityForce.x*deltaTime;
|
|
physicBodies[i]->rigidbody.velocity.y += gravityForce.y*deltaTime;
|
|
}
|
|
|
|
// Apply acceleration to velocity
|
|
physicBodies[i]->rigidbody.velocity.x += physicBodies[i]->rigidbody.acceleration.x*deltaTime;
|
|
physicBodies[i]->rigidbody.velocity.y += physicBodies[i]->rigidbody.acceleration.y*deltaTime;
|
|
|
|
// Apply velocity to position
|
|
physicBodies[i]->transform.position.x += physicBodies[i]->rigidbody.velocity.x*deltaTime;
|
|
physicBodies[i]->transform.position.y -= physicBodies[i]->rigidbody.velocity.y*deltaTime;
|
|
}
|
|
|
|
// 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) physicBodies[i]->rigidbody.isGrounded = (contactNormal.y < 0.0f);
|
|
|
|
// 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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif // PHYSAC_IMPLEMENTATION
|