diff --git a/examples/physics_basic_rigidbody.c b/examples/physics_basic_rigidbody.c deleted file mode 100644 index 87316a981..000000000 --- a/examples/physics_basic_rigidbody.c +++ /dev/null @@ -1,135 +0,0 @@ -/******************************************************************************************* -* -* raylib [physac] example - Basic rigidbody -* -* This example has been created using raylib 1.5 (www.raylib.com) -* raylib is licensed under an unmodified zlib/libpng license (View raylib.h for details) -* -* NOTE: -* Physac requires multi-threading, when InitPhysics() a second thread is created to manage -* physics calculations. To accomplish that, physac uses pthread Win32 library that can be -* found inside raylib/src/external/pthread directory. -* -* Add pthread library when compiling physac example: -* gcc -o $(NAME_PART).exe $(FILE_NAME) $(RAYLIB_DIR)\raylib_icon -L../src/external/pthread/lib \ -* -I../src -I../src/external/pthread/include -lraylib -lglfw3 -lopengl32 -lgdi32 -lpthreadGC2 -std=c99 -Wall -* -* Note that pthreadGC2.dll must be also copied to project directory! -* -* Copyright (c) 2016 Victor Fisac and Ramon Santamaria (@raysan5) -* -********************************************************************************************/ - -#include "raylib.h" - -#define PHYSAC_IMPLEMENTATION -#include "physac.h" - -#define MOVE_VELOCITY 5 -#define JUMP_VELOCITY 30 - -int main() -{ - // Initialization - //-------------------------------------------------------------------------------------- - int screenWidth = 800; - int screenHeight = 450; - - InitWindow(screenWidth, screenHeight, "raylib [physac] example - basic rigidbody"); - InitPhysics((Vector2){ 0.0f, -9.81f/2 }); // Initialize physics module - - // Debug variables - bool isDebug = false; - - // Create rectangle physic object - PhysicBody rectangle = CreatePhysicBody((Vector2){ screenWidth*0.25f, screenHeight/2 }, 0.0f, (Vector2){ 75, 50 }); - rectangle->rigidbody.enabled = true; // Enable physic object rigidbody behaviour - rectangle->rigidbody.applyGravity = true; - rectangle->rigidbody.friction = 0.1f; - rectangle->rigidbody.bounciness = 6.0f; - - // Create square physic object - PhysicBody square = CreatePhysicBody((Vector2){ screenWidth*0.75f, screenHeight/2 }, 0.0f, (Vector2){ 50, 50 }); - square->rigidbody.enabled = true; // Enable physic object rigidbody behaviour - square->rigidbody.applyGravity = true; - square->rigidbody.friction = 0.1f; - - // Create walls physic objects - PhysicBody floor = CreatePhysicBody((Vector2){ screenWidth/2, screenHeight*0.95f }, 0.0f, (Vector2){ screenWidth*0.9f, 100 }); - PhysicBody leftWall = CreatePhysicBody((Vector2){ 0.0f, screenHeight/2 }, 0.0f, (Vector2){ screenWidth*0.1f, screenHeight }); - PhysicBody rightWall = CreatePhysicBody((Vector2){ screenWidth, screenHeight/2 }, 0.0f, (Vector2){ screenWidth*0.1f, screenHeight }); - PhysicBody roof = CreatePhysicBody((Vector2){ screenWidth/2, screenHeight*0.05f }, 0.0f, (Vector2){ screenWidth*0.9f, 100 }); - - // Create pplatform physic object - PhysicBody platform = CreatePhysicBody((Vector2){ screenWidth/2, screenHeight*0.7f }, 0.0f, (Vector2){ screenWidth*0.25f, 20 }); - - SetTargetFPS(60); - //-------------------------------------------------------------------------------------- - - // Main game loop - while (!WindowShouldClose()) // Detect window close button or ESC key - { - // Update - //---------------------------------------------------------------------------------- - // Check rectangle movement inputs - if (IsKeyPressed('W')) rectangle->rigidbody.velocity.y = JUMP_VELOCITY; - if (IsKeyDown('A')) rectangle->rigidbody.velocity.x = -MOVE_VELOCITY; - else if (IsKeyDown('D')) rectangle->rigidbody.velocity.x = MOVE_VELOCITY; - - // Check square movement inputs - if (IsKeyDown(KEY_UP) && square->rigidbody.isGrounded) square->rigidbody.velocity.y = JUMP_VELOCITY; - if (IsKeyDown(KEY_LEFT)) square->rigidbody.velocity.x = -MOVE_VELOCITY; - else if (IsKeyDown(KEY_RIGHT)) square->rigidbody.velocity.x = MOVE_VELOCITY; - - // Check debug switch input - if (IsKeyPressed('P')) isDebug = !isDebug; - //---------------------------------------------------------------------------------- - - // Draw - //---------------------------------------------------------------------------------- - BeginDrawing(); - - ClearBackground(RAYWHITE); - - // Draw floor, roof and walls rectangles - DrawRectangleRec(TransformToRectangle(floor->transform), DARKGRAY); // Convert transform values to rectangle data type variable - DrawRectangleRec(TransformToRectangle(leftWall->transform), DARKGRAY); - DrawRectangleRec(TransformToRectangle(rightWall->transform), DARKGRAY); - DrawRectangleRec(TransformToRectangle(roof->transform), DARKGRAY); - - // Draw middle platform rectangle - DrawRectangleRec(TransformToRectangle(platform->transform), DARKGRAY); - - // Draw physic objects - DrawRectangleRec(TransformToRectangle(rectangle->transform), RED); - DrawRectangleRec(TransformToRectangle(square->transform), BLUE); - - // Draw collider lines if debug is enabled - if (isDebug) - { - DrawRectangleLines(floor->collider.bounds.x, floor->collider.bounds.y, floor->collider.bounds.width, floor->collider.bounds.height, GREEN); - DrawRectangleLines(leftWall->collider.bounds.x, leftWall->collider.bounds.y, leftWall->collider.bounds.width, leftWall->collider.bounds.height, GREEN); - DrawRectangleLines(rightWall->collider.bounds.x, rightWall->collider.bounds.y, rightWall->collider.bounds.width, rightWall->collider.bounds.height, GREEN); - DrawRectangleLines(roof->collider.bounds.x, roof->collider.bounds.y, roof->collider.bounds.width, roof->collider.bounds.height, GREEN); - DrawRectangleLines(platform->collider.bounds.x, platform->collider.bounds.y, platform->collider.bounds.width, platform->collider.bounds.height, GREEN); - DrawRectangleLines(rectangle->collider.bounds.x, rectangle->collider.bounds.y, rectangle->collider.bounds.width, rectangle->collider.bounds.height, GREEN); - DrawRectangleLines(square->collider.bounds.x, square->collider.bounds.y, square->collider.bounds.width, square->collider.bounds.height, GREEN); - } - - // Draw help message - DrawText("Use WASD to move rectangle and ARROWS to move square", screenWidth/2 - MeasureText("Use WASD to move rectangle and ARROWS to move square", 20)/2, screenHeight*0.075f, 20, LIGHTGRAY); - - DrawFPS(10, 10); - - EndDrawing(); - //---------------------------------------------------------------------------------- - } - - // De-Initialization - //-------------------------------------------------------------------------------------- - ClosePhysics(); // Unitialize physics (including all loaded objects) - CloseWindow(); // Close window and OpenGL context - //-------------------------------------------------------------------------------------- - - return 0; -} \ No newline at end of file diff --git a/examples/physics_basic_rigidbody.png b/examples/physics_basic_rigidbody.png deleted file mode 100644 index 52f265acf..000000000 Binary files a/examples/physics_basic_rigidbody.png and /dev/null differ diff --git a/examples/physics_demo.c b/examples/physics_demo.c new file mode 100644 index 000000000..bed7c94d3 --- /dev/null +++ b/examples/physics_demo.c @@ -0,0 +1,122 @@ +/******************************************************************************************* +* +* Physac - Physics demo +* +* NOTE: Physac requires multi-threading, when InitPhysics() a second thread is created to manage physics calculations. +* The file pthreadGC2.dll is required to run the program; you can find it in 'src\external' +* +* Copyright (c) 2016 Victor Fisac +* +********************************************************************************************/ + +#include "raylib.h" + +#define PHYSAC_IMPLEMENTATION +#include "..\src\physac.h" + +int main() +{ + // Initialization + //-------------------------------------------------------------------------------------- + int screenWidth = 800; + int screenHeight = 450; + + SetConfigFlags(FLAG_MSAA_4X_HINT); + InitWindow(screenWidth, screenHeight, "Physac [raylib] - Physics demo"); + SetTargetFPS(60); + + // Physac logo drawing position + int logoX = screenWidth - MeasureText("Physac", 30) - 10; + int logoY = 15; + + // Initialize physics and default physics bodies + InitPhysics(); + + // Create floor rectangle physics body + PhysicsBody floor = CreatePhysicsBodyRectangle((Vector2){ screenWidth/2, screenHeight }, 500, 100, 10); + floor->enabled = false; // Disable body state to convert it to static (no dynamics, but collisions) + + // Create obstacle circle physics body + PhysicsBody circle = CreatePhysicsBodyCircle((Vector2){ screenWidth/2, screenHeight/2 }, 45, 10); + circle->enabled = false; // Disable body state to convert it to static (no dynamics, but collisions) + //-------------------------------------------------------------------------------------- + + // Main game loop + while (!WindowShouldClose()) // Detect window close button or ESC key + { + // Update + //---------------------------------------------------------------------------------- + if (IsKeyPressed('R')) // Reset physics input + { + ResetPhysics(); + + floor = CreatePhysicsBodyRectangle((Vector2){ screenWidth/2, screenHeight }, 500, 100, 10); + floor->enabled = false; + + circle = CreatePhysicsBodyCircle((Vector2){ screenWidth/2, screenHeight/2 }, 45, 10); + circle->enabled = false; + } + + // Physics body creation inputs + if (IsMouseButtonPressed(MOUSE_LEFT_BUTTON)) CreatePhysicsBodyPolygon(GetMousePosition(), GetRandomValue(20, 80), GetRandomValue(3, 8), 10); + else if (IsMouseButtonPressed(MOUSE_RIGHT_BUTTON)) CreatePhysicsBodyCircle(GetMousePosition(), GetRandomValue(10, 45), 10); + + // Destroy falling physics bodies + int bodiesCount = GetPhysicsBodiesCount(); + for (int i = bodiesCount - 1; i >= 0; i--) + { + PhysicsBody body = GetPhysicsBody(i); + if (body != NULL && (body->position.y > screenHeight*2)) DestroyPhysicsBody(body); + } + //---------------------------------------------------------------------------------- + + // Draw + //---------------------------------------------------------------------------------- + BeginDrawing(); + + ClearBackground(BLACK); + + DrawFPS(screenWidth - 90, screenHeight - 30); + + // Draw created physics bodies + bodiesCount = GetPhysicsBodiesCount(); + for (int i = 0; i < bodiesCount; i++) + { + PhysicsBody body = GetPhysicsBody(i); + + if (body != NULL) + { + int vertexCount = GetPhysicsShapeVerticesCount(i); + for (int j = 0; j < vertexCount; j++) + { + // Get physics bodies shape vertices to draw lines + // Note: GetPhysicsShapeVertex() already calculates rotation transformations + Vector2 vertexA = GetPhysicsShapeVertex(body, j); + + int jj = (((j + 1) < vertexCount) ? (j + 1) : 0); // Get next vertex or first to close the shape + Vector2 vertexB = GetPhysicsShapeVertex(body, jj); + + DrawLineV(vertexA, vertexB, GREEN); // Draw a line between two vertex positions + } + } + } + + DrawText("Left mouse button to create a polygon", 10, 10, 10, WHITE); + DrawText("Right mouse button to create a circle", 10, 25, 10, WHITE); + DrawText("Press 'R' to reset example", 10, 40, 10, WHITE); + + DrawText("Physac", logoX, logoY, 30, WHITE); + DrawText("Powered by", logoX + 50, logoY - 7, 10, WHITE); + + EndDrawing(); + //---------------------------------------------------------------------------------- + } + + // De-Initialization + //-------------------------------------------------------------------------------------- + ClosePhysics(); // Unitialize physics + CloseWindow(); // Close window and OpenGL context + //-------------------------------------------------------------------------------------- + + return 0; +} diff --git a/examples/physics_demo.png b/examples/physics_demo.png new file mode 100644 index 000000000..12dc7e724 Binary files /dev/null and b/examples/physics_demo.png differ diff --git a/examples/physics_forces.c b/examples/physics_forces.c deleted file mode 100644 index e45cb44cc..000000000 --- a/examples/physics_forces.c +++ /dev/null @@ -1,193 +0,0 @@ -/******************************************************************************************* -* -* raylib [physac] example - Forces -* -* This example has been created using raylib 1.5 (www.raylib.com) -* raylib is licensed under an unmodified zlib/libpng license (View raylib.h for details) -* -* NOTE: -* Physac requires multi-threading, when InitPhysics() a second thread is created to manage -* physics calculations. To accomplish that, physac uses pthread Win32 library that can be -* found inside raylib/src/external/pthread directory. -* -* Add pthread library when compiling physac example: -* gcc -o $(NAME_PART).exe $(FILE_NAME) $(RAYLIB_DIR)\raylib_icon -L../src/external/pthread/lib \ -* -I../src -I../src/external/pthread/include -lraylib -lglfw3 -lopengl32 -lgdi32 -lpthreadGC2 -std=c99 -Wall -* -* Note that pthreadGC2.dll must be also copied to project directory! -* -* Copyright (c) 2016 Victor Fisac and Ramon Santamaria (@raysan5) -* -********************************************************************************************/ - -#include "raylib.h" - -#define PHYSAC_IMPLEMENTATION -#include "physac.h" - -#define FORCE_AMOUNT 5.0f -#define FORCE_RADIUS 150 -#define LINE_LENGTH 75 -#define TRIANGLE_LENGTH 12 - -int main() -{ - // Initialization - //-------------------------------------------------------------------------------------- - int screenWidth = 800; - int screenHeight = 450; - - InitWindow(screenWidth, screenHeight, "raylib [physac] example - forces"); - InitPhysics((Vector2){ 0.0f, -9.81f/2 }); // Initialize physics module - - // Global variables - Vector2 mousePosition; - bool isDebug = false; - - // Create rectangle physic objects - PhysicBody rectangles[3]; - for (int i = 0; i < 3; i++) - { - rectangles[i] = CreatePhysicBody((Vector2){ screenWidth/4*(i+1), (((i % 2) == 0) ? (screenHeight/3) : (screenHeight/1.5f)) }, 0.0f, (Vector2){ 50, 50 }); - rectangles[i]->rigidbody.enabled = true; // Enable physic object rigidbody behaviour - rectangles[i]->rigidbody.friction = 0.1f; - } - - // Create circles physic objects - // NOTE: when creating circle physic objects, transform.scale must be { 0, 0 } and object radius must be defined in collider.radius and use this value to draw the circle. - PhysicBody circles[3]; - for (int i = 0; i < 3; i++) - { - circles[i] = CreatePhysicBody((Vector2){ screenWidth/4*(i+1), (((i % 2) == 0) ? (screenHeight/1.5f) : (screenHeight/4)) }, 0.0f, (Vector2){ 0, 0 }); - circles[i]->rigidbody.enabled = true; // Enable physic object rigidbody behaviour - circles[i]->rigidbody.friction = 0.1f; - circles[i]->collider.type = COLLIDER_CIRCLE; - circles[i]->collider.radius = 25; - } - - // Create walls physic objects - PhysicBody leftWall = CreatePhysicBody((Vector2){ -25, screenHeight/2 }, 0.0f, (Vector2){ 50, screenHeight }); - PhysicBody rightWall = CreatePhysicBody((Vector2){ screenWidth + 25, screenHeight/2 }, 0.0f, (Vector2){ 50, screenHeight }); - PhysicBody topWall = CreatePhysicBody((Vector2){ screenWidth/2, -25 }, 0.0f, (Vector2){ screenWidth, 50 }); - PhysicBody bottomWall = CreatePhysicBody((Vector2){ screenWidth/2, screenHeight + 25 }, 0.0f, (Vector2){ screenWidth, 50 }); - - SetTargetFPS(60); - //-------------------------------------------------------------------------------------- - - // Main game loop - while (!WindowShouldClose()) // Detect window close button or ESC key - { - // Update - //---------------------------------------------------------------------------------- - - // Update mouse position value - mousePosition = GetMousePosition(); - - // Check force input - if (IsMouseButtonPressed(MOUSE_LEFT_BUTTON)) ApplyForceAtPosition(mousePosition, FORCE_AMOUNT, FORCE_RADIUS); - - // Check reset input - if (IsKeyPressed('R')) - { - // Reset rectangle physic objects positions - for (int i = 0; i < 3; i++) - { - rectangles[i]->transform.position = (Vector2){ screenWidth/4*(i+1) - rectangles[i]->transform.scale.x/2, (((i % 2) == 0) ? (screenHeight/3) : (screenHeight/1.5f)) - rectangles[i]->transform.scale.y/2 }; - rectangles[i]->rigidbody.velocity =(Vector2){ 0.0f, 0.0f }; - } - - // Reset circles physic objects positions - for (int i = 0; i < 3; i++) - { - circles[i]->transform.position = (Vector2){ screenWidth/4*(i+1), (((i % 2) == 0) ? (screenHeight/1.5f) : (screenHeight/4)) }; - circles[i]->rigidbody.velocity =(Vector2){ 0.0f, 0.0f }; - } - } - - // Check debug switch input - if (IsKeyPressed('P')) isDebug = !isDebug; - //---------------------------------------------------------------------------------- - - // Draw - //---------------------------------------------------------------------------------- - BeginDrawing(); - - ClearBackground(RAYWHITE); - - // Draw rectangles - for (int i = 0; i < 3; i++) - { - // Convert transform values to rectangle data type variable - DrawRectangleRec(TransformToRectangle(rectangles[i]->transform), RED); - if (isDebug) DrawRectangleLines(rectangles[i]->collider.bounds.x, rectangles[i]->collider.bounds.y, rectangles[i]->collider.bounds.width, rectangles[i]->collider.bounds.height, GREEN); - - // Draw force radius - DrawCircleLines(mousePosition.x, mousePosition.y, FORCE_RADIUS, BLACK); - - // Draw direction lines - if (CheckCollisionPointCircle((Vector2){ rectangles[i]->transform.position.x + rectangles[i]->transform.scale.x/2, rectangles[i]->transform.position.y + rectangles[i]->transform.scale.y/2 }, mousePosition, FORCE_RADIUS)) - { - Vector2 direction = { rectangles[i]->transform.position.x + rectangles[i]->transform.scale.x/2 - mousePosition.x, rectangles[i]->transform.position.y + rectangles[i]->transform.scale.y/2 - mousePosition.y }; - float angle = atan2l(direction.y, direction.x); - - // Calculate arrow start and end positions - Vector2 startPosition = { rectangles[i]->transform.position.x + rectangles[i]->transform.scale.x/2, rectangles[i]->transform.position.y + rectangles[i]->transform.scale.y/2 }; - Vector2 endPosition = { rectangles[i]->transform.position.x + rectangles[i]->transform.scale.x/2 + (cos(angle)*LINE_LENGTH), rectangles[i]->transform.position.y + rectangles[i]->transform.scale.y/2 + (sin(angle)*LINE_LENGTH) }; - - // Draw arrow line - DrawLineV(startPosition, endPosition, BLACK); - - // Draw arrow triangle - DrawTriangleLines((Vector2){ endPosition.x - cos(angle + 90*DEG2RAD)*LINE_LENGTH/TRIANGLE_LENGTH, endPosition.y - sin(angle + 90*DEG2RAD)*LINE_LENGTH/TRIANGLE_LENGTH }, - (Vector2){ endPosition.x + cos(angle + 90*DEG2RAD)*LINE_LENGTH/TRIANGLE_LENGTH, endPosition.y + sin(angle + 90*DEG2RAD)*LINE_LENGTH/TRIANGLE_LENGTH }, - (Vector2){ endPosition.x + cos(angle)*LINE_LENGTH/TRIANGLE_LENGTH*2, endPosition.y + sin(angle)*LINE_LENGTH/TRIANGLE_LENGTH*2 }, BLACK); - } - } - - // Draw circles - for (int i = 0; i < 3; i++) - { - DrawCircleV(circles[i]->transform.position, circles[i]->collider.radius, BLUE); - if (isDebug) DrawCircleLines(circles[i]->transform.position.x, circles[i]->transform.position.y, circles[i]->collider.radius, GREEN); - - // Draw force radius - DrawCircleLines(mousePosition.x, mousePosition.y, FORCE_RADIUS, BLACK); - - // Draw direction lines - if (CheckCollisionPointCircle((Vector2){ circles[i]->transform.position.x, circles[i]->transform.position.y }, mousePosition, FORCE_RADIUS)) - { - Vector2 direction = { circles[i]->transform.position.x - mousePosition.x, circles[i]->transform.position.y - mousePosition.y }; - float angle = atan2l(direction.y, direction.x); - - // Calculate arrow start and end positions - Vector2 startPosition = { circles[i]->transform.position.x, circles[i]->transform.position.y }; - Vector2 endPosition = { circles[i]->transform.position.x + (cos(angle)*LINE_LENGTH), circles[i]->transform.position.y + (sin(angle)*LINE_LENGTH) }; - - // Draw arrow line - DrawLineV(startPosition, endPosition, BLACK); - - // Draw arrow triangle - DrawTriangleLines((Vector2){ endPosition.x - cos(angle + 90*DEG2RAD)*LINE_LENGTH/TRIANGLE_LENGTH, endPosition.y - sin(angle + 90*DEG2RAD)*LINE_LENGTH/TRIANGLE_LENGTH }, - (Vector2){ endPosition.x + cos(angle + 90*DEG2RAD)*LINE_LENGTH/TRIANGLE_LENGTH, endPosition.y + sin(angle + 90*DEG2RAD)*LINE_LENGTH/TRIANGLE_LENGTH }, - (Vector2){ endPosition.x + cos(angle)*LINE_LENGTH/TRIANGLE_LENGTH*2, endPosition.y + sin(angle)*LINE_LENGTH/TRIANGLE_LENGTH*2 }, BLACK); - } - } - - // Draw help messages - DrawText("Use LEFT MOUSE BUTTON to apply a force", screenWidth/2 - MeasureText("Use LEFT MOUSE BUTTON to apply a force", 20)/2, screenHeight*0.075f, 20, LIGHTGRAY); - DrawText("Use R to reset objects position", screenWidth/2 - MeasureText("Use R to reset objects position", 20)/2, screenHeight*0.875f, 20, GRAY); - - DrawFPS(10, 10); - - EndDrawing(); - //---------------------------------------------------------------------------------- - } - - // De-Initialization - //-------------------------------------------------------------------------------------- - ClosePhysics(); // Unitialize physics module - CloseWindow(); // Close window and OpenGL context - //-------------------------------------------------------------------------------------- - - return 0; -} \ No newline at end of file diff --git a/examples/physics_forces.png b/examples/physics_forces.png deleted file mode 100644 index 832bdbd9e..000000000 Binary files a/examples/physics_forces.png and /dev/null differ diff --git a/examples/physics_friction.c b/examples/physics_friction.c new file mode 100644 index 000000000..28d3c4b8c --- /dev/null +++ b/examples/physics_friction.c @@ -0,0 +1,136 @@ +/******************************************************************************************* +* +* Physac - Physics friction +* +* NOTE: Physac requires multi-threading, when InitPhysics() a second thread is created to manage physics calculations. +* The file pthreadGC2.dll is required to run the program; you can find it in 'src\external' +* +* Copyright (c) 2016 Victor Fisac +* +********************************************************************************************/ + +#include "raylib.h" + +#define PHYSAC_IMPLEMENTATION +#include "..\src\physac.h" + +int main() +{ + // Initialization + //-------------------------------------------------------------------------------------- + int screenWidth = 800; + int screenHeight = 450; + + SetConfigFlags(FLAG_MSAA_4X_HINT); + InitWindow(screenWidth, screenHeight, "Physac [raylib] - Physics friction"); + SetTargetFPS(60); + + // Physac logo drawing position + int logoX = screenWidth - MeasureText("Physac", 30) - 10; + int logoY = 15; + + // Initialize physics and default physics bodies + InitPhysics(); + + // Create floor rectangle physics body + PhysicsBody floor = CreatePhysicsBodyRectangle((Vector2){ screenWidth/2, screenHeight }, screenWidth, 100, 10); + floor->enabled = false; // Disable body state to convert it to static (no dynamics, but collisions) + PhysicsBody wall = CreatePhysicsBodyRectangle((Vector2){ screenWidth/2, screenHeight*0.8f }, 10, 80, 10); + wall->enabled = false; // Disable body state to convert it to static (no dynamics, but collisions) + + // Create left ramp physics body + PhysicsBody rectLeft = CreatePhysicsBodyRectangle((Vector2){ 25, screenHeight - 5 }, 250, 250, 10); + rectLeft->enabled = false; // Disable body state to convert it to static (no dynamics, but collisions) + SetPhysicsBodyRotation(rectLeft, 30*DEG2RAD); + + // Create right ramp physics body + PhysicsBody rectRight = CreatePhysicsBodyRectangle((Vector2){ screenWidth - 25, screenHeight - 5 }, 250, 250, 10); + rectRight->enabled = false; // Disable body state to convert it to static (no dynamics, but collisions) + SetPhysicsBodyRotation(rectRight, 330*DEG2RAD); + + // Create dynamic physics bodies + PhysicsBody bodyA = CreatePhysicsBodyRectangle((Vector2){ 35, screenHeight*0.6f }, 40, 40, 10); + bodyA->staticFriction = 0.1f; + bodyA->dynamicFriction = 0.1f; + SetPhysicsBodyRotation(bodyA, 30*DEG2RAD); + + PhysicsBody bodyB = CreatePhysicsBodyRectangle((Vector2){ screenWidth - 35, screenHeight*0.6f }, 40, 40, 10); + bodyB->staticFriction = 1; + bodyB->dynamicFriction = 1; + SetPhysicsBodyRotation(bodyB, 330*DEG2RAD); + //-------------------------------------------------------------------------------------- + + // Main game loop + while (!WindowShouldClose()) // Detect window close button or ESC key + { + // Update + //---------------------------------------------------------------------------------- + if (IsKeyPressed('R')) // Reset physics input + { + // Reset dynamic physics bodies position, velocity and rotation + bodyA->position = (Vector2){ 35, screenHeight*0.6f }; + bodyA->velocity = (Vector2){ 0, 0 }; + bodyA->angularVelocity = 0; + SetPhysicsBodyRotation(bodyA, 30*DEG2RAD); + + bodyB->position = (Vector2){ screenWidth - 35, screenHeight*0.6f }; + bodyB->velocity = (Vector2){ 0, 0 }; + bodyB->angularVelocity = 0; + SetPhysicsBodyRotation(bodyB, 330*DEG2RAD); + } + //---------------------------------------------------------------------------------- + + // Draw + //---------------------------------------------------------------------------------- + BeginDrawing(); + + ClearBackground(BLACK); + + DrawFPS(screenWidth - 90, screenHeight - 30); + + // Draw created physics bodies + int bodiesCount = GetPhysicsBodiesCount(); + for (int i = 0; i < bodiesCount; i++) + { + PhysicsBody body = GetPhysicsBody(i); + + if (body != NULL) + { + int vertexCount = GetPhysicsShapeVerticesCount(i); + for (int j = 0; j < vertexCount; j++) + { + // Get physics bodies shape vertices to draw lines + // Note: GetPhysicsShapeVertex() already calculates rotation transformations + Vector2 vertexA = GetPhysicsShapeVertex(body, j); + + int jj = (((j + 1) < vertexCount) ? (j + 1) : 0); // Get next vertex or first to close the shape + Vector2 vertexB = GetPhysicsShapeVertex(body, jj); + + DrawLineV(vertexA, vertexB, GREEN); // Draw a line between two vertex positions + } + } + } + + DrawRectangle(0, screenHeight - 49, screenWidth, 49, BLACK); + + DrawText("Friction amount", (screenWidth - MeasureText("Friction amount", 30))/2, 75, 30, WHITE); + DrawText("0.1", bodyA->position.x - MeasureText("0.1", 20)/2, bodyA->position.y - 7, 20, WHITE); + DrawText("1", bodyB->position.x - MeasureText("1", 20)/2, bodyB->position.y - 7, 20, WHITE); + + DrawText("Press 'R' to reset example", 10, 10, 10, WHITE); + + DrawText("Physac", logoX, logoY, 30, WHITE); + DrawText("Powered by", logoX + 50, logoY - 7, 10, WHITE); + + EndDrawing(); + //---------------------------------------------------------------------------------- + } + + // De-Initialization + //-------------------------------------------------------------------------------------- + ClosePhysics(); // Unitialize physics + CloseWindow(); // Close window and OpenGL context + //-------------------------------------------------------------------------------------- + + return 0; +} diff --git a/examples/physics_friction.png b/examples/physics_friction.png new file mode 100644 index 000000000..e791ec2b9 Binary files /dev/null and b/examples/physics_friction.png differ diff --git a/examples/physics_movement.c b/examples/physics_movement.c new file mode 100644 index 000000000..ca18f3dfc --- /dev/null +++ b/examples/physics_movement.c @@ -0,0 +1,122 @@ +/******************************************************************************************* +* +* Physac - Physics movement +* +* NOTE: Physac requires multi-threading, when InitPhysics() a second thread is created to manage physics calculations. +* The file pthreadGC2.dll is required to run the program; you can find it in 'src\external' +* +* Copyright (c) 2016 Victor Fisac +* +********************************************************************************************/ + +#include "raylib.h" + +#define PHYSAC_IMPLEMENTATION +#include "..\src\physac.h" + +#define VELOCITY 0.5f + +int main() +{ + // Initialization + //-------------------------------------------------------------------------------------- + int screenWidth = 800; + int screenHeight = 450; + + SetConfigFlags(FLAG_MSAA_4X_HINT); + InitWindow(screenWidth, screenHeight, "Physac [raylib] - Physics movement"); + SetTargetFPS(60); + + // Physac logo drawing position + int logoX = screenWidth - MeasureText("Physac", 30) - 10; + int logoY = 15; + + // Initialize physics and default physics bodies + InitPhysics(); + + // Create floor and walls rectangle physics body + PhysicsBody floor = CreatePhysicsBodyRectangle((Vector2){ screenWidth/2, screenHeight }, screenWidth, 100, 10); + PhysicsBody platformLeft = CreatePhysicsBodyRectangle((Vector2){ screenWidth*0.25f, screenHeight*0.6f }, screenWidth*0.25f, 10, 10); + PhysicsBody platformRight = CreatePhysicsBodyRectangle((Vector2){ screenWidth*0.75f, screenHeight*0.6f }, screenWidth*0.25f, 10, 10); + PhysicsBody wallLeft = CreatePhysicsBodyRectangle((Vector2){ -5, screenHeight/2 }, 10, screenHeight, 10); + PhysicsBody wallRight = CreatePhysicsBodyRectangle((Vector2){ screenWidth + 5, screenHeight/2 }, 10, screenHeight, 10); + + // Disable dynamics to floor and walls physics bodies + floor->enabled = false; + platformLeft->enabled = false; + platformRight->enabled = false; + wallLeft->enabled = false; + wallRight->enabled = false; + + // Create movement physics body + PhysicsBody body = CreatePhysicsBodyRectangle((Vector2){ screenWidth/2, screenHeight/2 }, 50, 50, 1); + body->freezeOrient = true; // Constrain body rotation to avoid little collision torque amounts + //-------------------------------------------------------------------------------------- + + // Main game loop + while (!WindowShouldClose()) // Detect window close button or ESC key + { + // Update + //---------------------------------------------------------------------------------- + if (IsKeyPressed('R')) // Reset physics input + { + // Reset movement physics body position, velocity and rotation + body->position = (Vector2){ screenWidth/2, screenHeight/2 }; + body->velocity = (Vector2){ 0, 0 }; + SetPhysicsBodyRotation(body, 0); + } + + // Horizontal movement input + if (IsKeyDown(KEY_RIGHT)) body->velocity.x = VELOCITY; + else if (IsKeyDown(KEY_LEFT)) body->velocity.x = -VELOCITY; + + // Vertical movement input checking if player physics body is grounded + if (IsKeyDown(KEY_UP) && body->isGrounded) body->velocity.y = -VELOCITY*4; + //---------------------------------------------------------------------------------- + + // Draw + //---------------------------------------------------------------------------------- + BeginDrawing(); + + ClearBackground(BLACK); + + DrawFPS(screenWidth - 90, screenHeight - 30); + + // Draw created physics bodies + int bodiesCount = GetPhysicsBodiesCount(); + for (int i = 0; i < bodiesCount; i++) + { + PhysicsBody body = GetPhysicsBody(i); + + int vertexCount = GetPhysicsShapeVerticesCount(i); + for (int j = 0; j < vertexCount; j++) + { + // Get physics bodies shape vertices to draw lines + // Note: GetPhysicsShapeVertex() already calculates rotation transformations + Vector2 vertexA = GetPhysicsShapeVertex(body, j); + + int jj = (((j + 1) < vertexCount) ? (j + 1) : 0); // Get next vertex or first to close the shape + Vector2 vertexB = GetPhysicsShapeVertex(body, jj); + + DrawLineV(vertexA, vertexB, GREEN); // Draw a line between two vertex positions + } + } + + DrawText("Use 'ARROWS' to move player", 10, 10, 10, WHITE); + DrawText("Press 'R' to reset example", 10, 30, 10, WHITE); + + DrawText("Physac", logoX, logoY, 30, WHITE); + DrawText("Powered by", logoX + 50, logoY - 7, 10, WHITE); + + EndDrawing(); + //---------------------------------------------------------------------------------- + } + + // De-Initialization + //-------------------------------------------------------------------------------------- + ClosePhysics(); // Unitialize physics + CloseWindow(); // Close window and OpenGL context + //-------------------------------------------------------------------------------------- + + return 0; +} diff --git a/examples/physics_movement.png b/examples/physics_movement.png new file mode 100644 index 000000000..a88a7d79f Binary files /dev/null and b/examples/physics_movement.png differ diff --git a/examples/physics_restitution.c b/examples/physics_restitution.c new file mode 100644 index 000000000..3543db69c --- /dev/null +++ b/examples/physics_restitution.c @@ -0,0 +1,115 @@ +/******************************************************************************************* +* +* Physac - Physics restitution +* +* NOTE: Physac requires multi-threading, when InitPhysics() a second thread is created to manage physics calculations. +* The file pthreadGC2.dll is required to run the program; you can find it in 'src\external' +* +* Copyright (c) 2016 Victor Fisac +* +********************************************************************************************/ + +#include "raylib.h" + +#define PHYSAC_IMPLEMENTATION +#include "..\src\physac.h" + +int main() +{ + // Initialization + //-------------------------------------------------------------------------------------- + int screenWidth = 800; + int screenHeight = 450; + + SetConfigFlags(FLAG_MSAA_4X_HINT); + InitWindow(screenWidth, screenHeight, "Physac [raylib] - Physics restitution"); + SetTargetFPS(60); + + // Physac logo drawing position + int logoX = screenWidth - MeasureText("Physac", 30) - 10; + int logoY = 15; + + // Initialize physics and default physics bodies + InitPhysics(); + + // Create floor rectangle physics body + PhysicsBody floor = CreatePhysicsBodyRectangle((Vector2){ screenWidth/2, screenHeight }, screenWidth, 100, 10); + floor->enabled = false; // Disable body state to convert it to static (no dynamics, but collisions) + floor->restitution = 1; + + // Create circles physics body + PhysicsBody circleA = CreatePhysicsBodyCircle((Vector2){ screenWidth*0.25f, screenHeight/2 }, 30, 10); + circleA->restitution = 0; + PhysicsBody circleB = CreatePhysicsBodyCircle((Vector2){ screenWidth*0.5f, screenHeight/2 }, 30, 10); + circleB->restitution = 0.5f; + PhysicsBody circleC = CreatePhysicsBodyCircle((Vector2){ screenWidth*0.75f, screenHeight/2 }, 30, 10); + circleC->restitution = 1; + //-------------------------------------------------------------------------------------- + + // Main game loop + while (!WindowShouldClose()) // Detect window close button or ESC key + { + // Update + //---------------------------------------------------------------------------------- + if (IsKeyPressed('R')) // Reset physics input + { + // Reset circles physics bodies position and velocity + circleA->position = (Vector2){ screenWidth*0.25f, screenHeight/2 }; + circleA->velocity = (Vector2){ 0, 0 }; + circleB->position = (Vector2){ screenWidth*0.5f, screenHeight/2 }; + circleB->velocity = (Vector2){ 0, 0 }; + circleC->position = (Vector2){ screenWidth*0.75f, screenHeight/2 }; + circleC->velocity = (Vector2){ 0, 0 }; + } + //---------------------------------------------------------------------------------- + + // Draw + //---------------------------------------------------------------------------------- + BeginDrawing(); + + ClearBackground(BLACK); + + DrawFPS(screenWidth - 90, screenHeight - 30); + + // Draw created physics bodies + int bodiesCount = GetPhysicsBodiesCount(); + for (int i = 0; i < bodiesCount; i++) + { + PhysicsBody body = GetPhysicsBody(i); + + int vertexCount = GetPhysicsShapeVerticesCount(i); + for (int j = 0; j < vertexCount; j++) + { + // Get physics bodies shape vertices to draw lines + // Note: GetPhysicsShapeVertex() already calculates rotation transformations + Vector2 vertexA = GetPhysicsShapeVertex(body, j); + + int jj = (((j + 1) < vertexCount) ? (j + 1) : 0); // Get next vertex or first to close the shape + Vector2 vertexB = GetPhysicsShapeVertex(body, jj); + + DrawLineV(vertexA, vertexB, GREEN); // Draw a line between two vertex positions + } + } + + DrawText("Restitution amount", (screenWidth - MeasureText("Restitution amount", 30))/2, 75, 30, WHITE); + DrawText("0", circleA->position.x - MeasureText("0", 20)/2, circleA->position.y - 7, 20, WHITE); + DrawText("0.5", circleB->position.x - MeasureText("0.5", 20)/2, circleB->position.y - 7, 20, WHITE); + DrawText("1", circleC->position.x - MeasureText("1", 20)/2, circleC->position.y - 7, 20, WHITE); + + DrawText("Press 'R' to reset example", 10, 10, 10, WHITE); + + DrawText("Physac", logoX, logoY, 30, WHITE); + DrawText("Powered by", logoX + 50, logoY - 7, 10, WHITE); + + EndDrawing(); + //---------------------------------------------------------------------------------- + } + + // De-Initialization + //-------------------------------------------------------------------------------------- + ClosePhysics(); // Unitialize physics + CloseWindow(); // Close window and OpenGL context + //-------------------------------------------------------------------------------------- + + return 0; +} diff --git a/examples/physics_restitution.png b/examples/physics_restitution.png new file mode 100644 index 000000000..8ec4b3f32 Binary files /dev/null and b/examples/physics_restitution.png differ diff --git a/examples/physics_shatter.c b/examples/physics_shatter.c new file mode 100644 index 000000000..2cb9d1952 --- /dev/null +++ b/examples/physics_shatter.c @@ -0,0 +1,107 @@ +/******************************************************************************************* +* +* Physac - Body shatter +* +* NOTE: Physac requires multi-threading, when InitPhysics() a second thread is created to manage physics calculations. +* The file pthreadGC2.dll is required to run the program; you can find it in 'src\external' +* +* Copyright (c) 2016 Victor Fisac +* +********************************************************************************************/ + +#include "raylib.h" + +#define PHYSAC_IMPLEMENTATION +#include "..\src\physac.h" + +int main() +{ + // Initialization + //-------------------------------------------------------------------------------------- + int screenWidth = 800; + int screenHeight = 450; + + SetConfigFlags(FLAG_MSAA_4X_HINT); + InitWindow(screenWidth, screenHeight, "Physac [raylib] - Body shatter"); + SetTargetFPS(60); + + // Physac logo drawing position + int logoX = screenWidth - MeasureText("Physac", 30) - 10; + int logoY = 15; + + // Initialize physics and default physics bodies + InitPhysics(); + SetPhysicsGravity(0, 0); + + // Create random polygon physics body to shatter + PhysicsBody body = CreatePhysicsBodyPolygon((Vector2){ screenWidth/2, screenHeight/2 }, GetRandomValue(80, 200), GetRandomValue(3, 8), 10); + //-------------------------------------------------------------------------------------- + + // Main game loop + while (!WindowShouldClose()) // Detect window close button or ESC key + { + // Update + //---------------------------------------------------------------------------------- + if (IsKeyPressed('R')) // Reset physics input + { + ResetPhysics(); + + // Create random polygon physics body to shatter + body = CreatePhysicsBodyPolygon((Vector2){ screenWidth/2, screenHeight/2 }, GetRandomValue(80, 200), GetRandomValue(3, 8), 10); + } + + if (IsMouseButtonPressed(MOUSE_LEFT_BUTTON)) // Physics shatter input + { + // Note: some values need to be stored in variables due to asynchronous changes during main thread + int count = GetPhysicsBodiesCount(); + for (int i = count - 1; i >= 0; i--) + { + PhysicsBody currentBody = GetPhysicsBody(i); + if (currentBody != NULL) PhysicsShatter(currentBody, GetMousePosition(), 10/currentBody->inverseMass); + } + } + //---------------------------------------------------------------------------------- + + // Draw + //---------------------------------------------------------------------------------- + BeginDrawing(); + + ClearBackground(BLACK); + + // Draw created physics bodies + int bodiesCount = GetPhysicsBodiesCount(); + for (int i = 0; i < bodiesCount; i++) + { + PhysicsBody currentBody = GetPhysicsBody(i); + + int vertexCount = GetPhysicsShapeVerticesCount(i); + for (int j = 0; j < vertexCount; j++) + { + // Get physics bodies shape vertices to draw lines + // Note: GetPhysicsShapeVertex() already calculates rotation transformations + Vector2 vertexA = GetPhysicsShapeVertex(currentBody, j); + + int jj = (((j + 1) < vertexCount) ? (j + 1) : 0); // Get next vertex or first to close the shape + Vector2 vertexB = GetPhysicsShapeVertex(currentBody, jj); + + DrawLineV(vertexA, vertexB, GREEN); // Draw a line between two vertex positions + } + } + + DrawText("Left mouse button in polygon area to shatter body\nPress 'R' to reset example", 10, 10, 10, WHITE); + + DrawText("Physac", logoX, logoY, 30, WHITE); + DrawText("Powered by", logoX + 50, logoY - 7, 10, WHITE); + + EndDrawing(); + //---------------------------------------------------------------------------------- + } + + // De-Initialization + //-------------------------------------------------------------------------------------- + ClosePhysics(); // Unitialize physics + CloseWindow(); // Close window and OpenGL context + //-------------------------------------------------------------------------------------- + + return 0; +} diff --git a/examples/physics_shatter.png b/examples/physics_shatter.png new file mode 100644 index 000000000..68f9a1b7d Binary files /dev/null and b/examples/physics_shatter.png differ diff --git a/src/physac.h b/src/physac.h index dd4c41269..e807ffa6a 100644 --- a/src/physac.h +++ b/src/physac.h @@ -1,8 +1,11 @@ /********************************************************************************************** * -* physac 1.0 - 2D Physics library for raylib (https://github.com/raysan5/raylib) +* Physac - 2D Physics library for videogames * -* // TODO: Description... +* Description: Physac is a small 2D physics engine written in pure C. The engine uses a fixed time-step thread loop +* to simluate physics. A physics step contains the following phases: get collision information, apply dynamics, +* collision solving and position correction. It uses a very simple struct for physic bodies with a position vector +* to be used in any 3D rendering API. * * CONFIGURATION: * @@ -24,30 +27,21 @@ * internally in the library and input management and drawing functions must be provided by * the user (check library implementation for further details). * +* #define PHYSAC_DEBUG +* Traces log messages when creating and destroying physics bodies and detects errors in physics +* calculations and reference exceptions; it is useful for debug purposes +* * #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: * -* - There is a limit of 256 physic objects. -* - Physics behaviour can be unexpected using bounciness or friction values out of 0.0f - 1.0f range. -* - The module is limited to 2D axis oriented physics. -* - Physics colliders must be rectangle or circle shapes (there is not a custom polygon collider type). -* -* VERSIONS: -* -* 1.0 (14-Jun-2016) New module defines and fixed some delta time calculation bugs. -* 0.9 (09-Jun-2016) Module names review and converted to header-only. -* 0.8 (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. +* VERY THANKS TO: +* - Ramón Santamaria (@raysan5) * * LICENSE: zlib/libpng * -* Copyright (c) 2016 Victor Fisac (main developer) and Ramon Santamaria +* Copyright (c) 2016 Victor Fisac * * 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. @@ -66,18 +60,18 @@ * **********************************************************************************************/ -#ifndef PHYSAC_H +#if !defined(PHYSAC_H) #define PHYSAC_H -#if !defined(RAYGUI_STANDALONE) - #include "raylib.h" -#endif - #define PHYSAC_STATIC -#ifdef PHYSAC_STATIC +// #define PHYSAC_NO_THREADS +// #define PHYSAC_STANDALONE +// #define PHYSAC_DEBUG + +#if defined(PHYSAC_STATIC) #define PHYSACDEF static // Functions just visible to module including this file #else - #ifdef __cplusplus + #if defined(__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 @@ -87,87 +81,141 @@ //---------------------------------------------------------------------------------- // Defines and Macros //---------------------------------------------------------------------------------- -// ... +#define PHYSAC_MAX_BODIES 64 +#define PHYSAC_MAX_MANIFOLDS 4096 +#define PHYSAC_MAX_VERTICES 24 +#define PHYSAC_CIRCLE_VERTICES 24 + +#define PHYSAC_DESIRED_DELTATIME 1.0/60.0 +#define PHYSAC_MAX_TIMESTEP 0.02 +#define PHYSAC_COLLISION_ITERATIONS 100 +#define PHYSAC_PENETRATION_ALLOWANCE 0.05f +#define PHYSAC_PENETRATION_CORRECTION 0.4f + +#define PHYSAC_PI 3.14159265358979323846 +#define PHYSAC_DEG2RAD (PHYSAC_PI/180.0f) + +#define PHYSAC_MALLOC(size) malloc(size) +#define PHYSAC_FREE(ptr) free(ptr) //---------------------------------------------------------------------------------- // 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; + // Boolean type + #if !defined(_STDBOOL_H) + typedef enum { false, true } bool; + #define _STDBOOL_H + #endif #endif -typedef enum { COLLIDER_CIRCLE, COLLIDER_RECTANGLE } ColliderType; +typedef enum PhysicsShapeType { PHYSICS_CIRCLE, PHYSICS_POLYGON } PhysicsShapeType; -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; +// Previously defined to be used in PhysicsShape struct as circular dependencies +typedef struct PhysicsBodyData *PhysicsBody; -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; +// Mat2 type (used for polygon shape rotation matrix) +typedef struct Mat2 +{ + float m00; + float m01; + float m10; + float m11; +} Mat2; -typedef struct Collider { - bool enabled; - ColliderType type; - Rectangle bounds; // Used for COLLIDER_RECTANGLE - int radius; // Used for COLLIDER_CIRCLE -} Collider; +typedef struct PolygonData { + unsigned int vertexCount; // Current used vertex and normals count + Vector2 vertices[PHYSAC_MAX_VERTICES]; // Polygon vertex positions vectors + Vector2 normals[PHYSAC_MAX_VERTICES]; // Polygon vertex normals vectors + Mat2 transform; // Vertices transform matrix 2x2 +} PolygonData; -typedef struct PhysicBodyData { - unsigned int id; - Transform transform; - Rigidbody rigidbody; - Collider collider; - bool enabled; -} PhysicBodyData, *PhysicBody; +typedef struct PhysicsShape { + PhysicsShapeType type; // Physics shape type (circle or polygon) + PhysicsBody body; // Shape physics body reference + float radius; // Circle shape radius (used for circle shapes) + PolygonData vertexData; // Polygon shape vertices position and normals data (just used for polygon shapes) +} PhysicsShape; + +typedef struct PhysicsBodyData { + unsigned int id; // Reference unique identifier + bool enabled; // Enabled dynamics state (collisions are calculated anyway) + Vector2 position; // Physics body shape pivot + Vector2 velocity; // Current linear velocity applied to position + Vector2 force; // Current linear force (reset to 0 every step) + float angularVelocity; // Current angular velocity applied to orient + float torque; // Current angular force (reset to 0 every step) + float orient; // Rotation in radians + float inertia; // Moment of inertia + float inverseInertia; // Inverse value of inertia + float mass; // Physics body mass + float inverseMass; // Inverse value of mass + float staticFriction; // Friction when the body has not movement (0 to 1) + float dynamicFriction; // Friction when the body has movement (0 to 1) + float restitution; // Restitution coefficient of the body (0 to 1) + bool useGravity; // Apply gravity force to dynamics + bool isGrounded; // Physics grounded on other body state + bool freezeOrient; // Physics rotation constraint + PhysicsShape shape; // Physics body shape information (type, radius, vertices, normals) +} PhysicsBodyData; + +typedef struct PhysicsManifoldData { + unsigned int id; // Reference unique identifier + PhysicsBody bodyA; // Manifold first physics body reference + PhysicsBody bodyB; // Manifold second physics body reference + float penetration; // Depth of penetration from collision + Vector2 normal; // Normal direction vector from 'a' to 'b' + Vector2 contacts[2]; // Points of contact during collision + unsigned int contactsCount; // Current collision number of contacts + float restitution; // Mixed restitution during collision + float dynamicFriction; // Mixed dynamic friction during collision + float staticFriction; // Mixed static friction during collision +} PhysicsManifoldData, *PhysicsManifold; + +#if defined(__cplusplus) +extern "C" { // Prevents name mangling of functions +#endif + +//---------------------------------------------------------------------------------- +// Global Variables Definition +//---------------------------------------------------------------------------------- +//... //---------------------------------------------------------------------------------- // 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 void InitPhysics(void); // Initializes physics values, pointers and creates physics loop thread +PHYSACDEF bool IsPhysicsEnabled(void); // Returns true if physics thread is currently enabled +PHYSACDEF void SetPhysicsGravity(float x, float y); // Sets physics global gravity force +PHYSACDEF PhysicsBody CreatePhysicsBodyCircle(Vector2 pos, float radius, float density); // Creates a new circle physics body with generic parameters +PHYSACDEF PhysicsBody CreatePhysicsBodyRectangle(Vector2 pos, float width, float height, float density); // Creates a new rectangle physics body with generic parameters +PHYSACDEF PhysicsBody CreatePhysicsBodyPolygon(Vector2 pos, float radius, int sides, float density); // Creates a new polygon physics body with generic parameters +PHYSACDEF void PhysicsAddForce(PhysicsBody body, Vector2 force); // Adds a force to a physics body +PHYSACDEF void PhysicsAddTorque(PhysicsBody body, float amount); // Adds an angular force to a physics body +PHYSACDEF void PhysicsShatter(PhysicsBody body, Vector2 position, float force); // Shatters a polygon shape physics body to little physics bodies with explosion force +PHYSACDEF int GetPhysicsBodiesCount(void); // Returns the current amount of created physics bodies +PHYSACDEF PhysicsBody GetPhysicsBody(int index); // Returns a physics body of the bodies pool at a specific index +PHYSACDEF int GetPhysicsShapeType(int index); // Returns the physics body shape type (PHYSICS_CIRCLE or PHYSICS_POLYGON) +PHYSACDEF int GetPhysicsShapeVerticesCount(int index); // Returns the amount of vertices of a physics body shape +PHYSACDEF Vector2 GetPhysicsShapeVertex(PhysicsBody body, int vertex); // Returns transformed position of a body shape (body position + vertex transformed position) +PHYSACDEF void SetPhysicsBodyRotation(PhysicsBody body, float radians); // Sets physics body shape transform based on radians parameter +PHYSACDEF void DestroyPhysicsBody(PhysicsBody body); // Unitializes and destroy a physics body +PHYSACDEF void ResetPhysics(void); // Destroys created physics bodies and manifolds and resets global values +PHYSACDEF void ClosePhysics(void); // Unitializes physics pointers and closes physics loop thread -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) +#if defined(__cplusplus) +} +#endif #endif // PHYSAC_H - /*********************************************************************************** * * PHYSAC IMPLEMENTATION @@ -176,657 +224,1854 @@ PHYSACDEF Rectangle TransformToRectangle(Transform transform); #if defined(PHYSAC_IMPLEMENTATION) -// Check if custom malloc/free functions defined, if not, using standard ones -#if !defined(PHYSAC_MALLOC) - #include // Required for: malloc(), free() - - #define PHYSAC_MALLOC(size) malloc(size) - #define PHYSAC_FREE(ptr) free(ptr) +#if !defined(PHYSAC_NO_THREADS) + #include // Required for: pthread_t, pthread_create() #endif -#include // Required for: cos(), sin(), abs(), fminf() -#include // Required for typedef unsigned long long int uint64_t, used by hi-res timer - -#ifndef PHYSAC_NO_THREADS - #include // Required for: pthread_create() +#if defined(PHYSAC_DEBUG) + #include // Required for: printf() #endif -#if defined(PLATFORM_DESKTOP) +#include // Required for: malloc(), free(), srand(), rand() +#include // Required for: cosf(), sinf(), fabs(), sqrtf() + +#if defined(_WIN32) // 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 // Required for: timespec - #include // Required for: clock_gettime() +#elif defined(__linux) + #include // Required for: timespec + #include // 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 +#define min(a,b) (((a)<(b))?(a):(b)) +#define max(a,b) (((a)>(b))?(a):(b)) +#define PHYSAC_FLT_MAX 3.402823466e+38f +#define PHYSAC_EPSILON 0.000001f //---------------------------------------------------------------------------------- // 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 +#if !defined(PHYSAC_NO_THREADS) + static pthread_t physicsThreadId; // Physics thread id +#endif +static unsigned int usedMemory = 0; // Total allocated dynamic memory +static bool physicsThreadEnabled = false; // Physics thread enabled state +static double currentTime = 0; // Current time in milliseconds +#if defined(PLATFORM_ANDROID) || defined(PLATFORM_RPI) + static double baseTime = 0; // Android and RPI platforms base time +#endif +static double startTime = 0; // Start time in milliseconds +static double deltaTime = 0; // Delta time used for physics steps +static double accumulator = 0; // Physics time step delta time accumulator +static unsigned int stepsCount = 0; // Total physics steps processed +static Vector2 gravityForce = { 0, 9.81f/1000 }; // Physics world gravity force +static PhysicsBody bodies[PHYSAC_MAX_BODIES]; // Physics bodies pointers array +static unsigned int physicsBodiesCount = 0; // Physics world current bodies counter +static PhysicsManifold contacts[PHYSAC_MAX_MANIFOLDS]; // Physics bodies pointers array +static unsigned int physicsManifoldsCount = 0; // Physics world current manifolds counter //---------------------------------------------------------------------------------- -// Module specific Functions Declaration +// Module Internal 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 +static PolygonData CreateRandomPolygon(float radius, int sides); // Creates a random polygon shape with max vertex distance from polygon pivot +static PolygonData CreateRectanglePolygon(Vector2 pos, Vector2 size); // Creates a rectangle polygon shape based on a min and max positions +static void *PhysicsLoop(void *arg); // Physics loop thread function +static void PhysicsStep(void); // Physics steps calculations (dynamics, collisions and position corrections) +static PhysicsManifold CreatePhysicsManifold(PhysicsBody a, PhysicsBody b); // Creates a new physics manifold to solve collision +static void DestroyPhysicsManifold(PhysicsManifold manifold); // Unitializes and destroys a physics manifold +static void SolvePhysicsManifold(PhysicsManifold manifold); // Solves a created physics manifold between two physics bodies +static void SolveCircleToCircle(PhysicsManifold manifold); // Solves collision between two circle shape physics bodies +static void SolveCircleToPolygon(PhysicsManifold manifold); // Solves collision between a circle to a polygon shape physics bodies +static void SolvePolygonToCircle(PhysicsManifold manifold); // Solves collision between a polygon to a circle shape physics bodies +static void SolvePolygonToPolygon(PhysicsManifold manifold); // Solves collision between two polygons shape physics bodies +static void IntegratePhysicsForces(PhysicsBody body); // Integrates physics forces into velocity +static void InitializePhysicsManifolds(PhysicsManifold manifold); // Initializes physics manifolds to solve collisions +static void IntegratePhysicsImpulses(PhysicsManifold manifold); // Integrates physics collisions impulses to solve collisions +static void IntegratePhysicsVelocity(PhysicsBody body); // Integrates physics velocity into position and forces +static void CorrectPhysicsPositions(PhysicsManifold manifold); // Corrects physics bodies positions based on manifolds collision information +static float FindAxisLeastPenetration(int *faceIndex, PhysicsShape shapeA, PhysicsShape shapeB); // Finds polygon shapes axis least penetration +static void FindIncidentFace(Vector2 *v0, Vector2 *v1, PhysicsShape ref, PhysicsShape inc, int index); // Finds two polygon shapes incident face +static int Clip(Vector2 normal, float clip, Vector2 *faceA, Vector2 *faceB); // Calculates clipping based on a normal and two faces +static bool BiasGreaterThan(float valueA, float valueB); // Check if values are between bias range +static Vector2 TriangleBarycenter(Vector2 v1, Vector2 v2, Vector2 v3); // Returns the barycenter of a triangle given by 3 points +static void InitTimer(void); // Initializes hi-resolution timer +static double GetCurrentTime(void); // Get current time in milliseconds +static int GetRandomNumber(int min, int max); // Returns a random number between min and max (both included) + +static void MathClamp(double *value, double min, double max); // Clamp a value in a range +static Vector2 MathCross(float value, Vector2 vector); // Returns the cross product of a vector and a value +static float MathCrossVector2(Vector2 v1, Vector2 v2); // Returns the cross product of two vectors +static float MathLenSqr(Vector2 vector); // Returns the len square root of a vector +static float MathDot(Vector2 v1, Vector2 v2); // Returns the dot product of two vectors +static inline float DistSqr(Vector2 v1, Vector2 v2); // Returns the square root of distance between two vectors +static void MathNormalize(Vector2 *vector); // Returns the normalized values of a vector +static Vector2 Vector2Add(Vector2 v1, Vector2 v2); // Returns the sum of two given vectors +static Vector2 Vector2Subtract(Vector2 v1, Vector2 v2); // Returns the subtract of two given vectors + +static Mat2 Mat2Radians(float radians); // Creates a matrix 2x2 from a given radians value +static void Mat2Set(Mat2 *matrix, float radians); // Set values from radians to a created matrix 2x2 +static Mat2 Mat2Transpose(Mat2 matrix); // Returns the transpose of a given matrix 2x2 +static Vector2 Mat2MultiplyVector2(Mat2 matrix, Vector2 vector); // Multiplies a vector by a matrix 2x2 //---------------------------------------------------------------------------------- // Module Functions Definition //---------------------------------------------------------------------------------- - -// Initializes pointers array (just pointers, fixed size) -PHYSACDEF void InitPhysics(Vector2 gravity) +// Initializes physics values, pointers and creates physics loop thread +PHYSACDEF void InitPhysics(void) { - // 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); + #if defined(PHYSAC_DEBUG) + printf("[PHYSAC] physics module initialized successfully\n"); + #endif + + #if !defined(PHYSAC_NO_THREADS) + // NOTE: if defined, user will need to create a thread for PhysicsThread function manually + // Create physics thread using POSIXS thread libraries + pthread_create(&physicsThreadId, NULL, &PhysicsLoop, NULL); #endif } -// Unitialize all physic objects and empty the objects pool -PHYSACDEF void ClosePhysics() +// Returns true if physics thread is currently enabled +PHYSACDEF bool IsPhysicsEnabled(void) { - // Exit physics thread loop + return physicsThreadEnabled; +} + +// Sets physics global gravity force +PHYSACDEF void SetPhysicsGravity(float x, float y) +{ + gravityForce.x = x; + gravityForce.y = y; +} + +// Creates a new circle physics body with generic parameters +PHYSACDEF PhysicsBody CreatePhysicsBodyCircle(Vector2 pos, float radius, float density) +{ + PhysicsBody newBody = CreatePhysicsBodyPolygon(pos, radius, PHYSAC_CIRCLE_VERTICES, density); + return newBody; + + /*PhysicsBody newBody = (PhysicsBody)PHYSAC_MALLOC(sizeof(PhysicsBodyData)); + usedMemory += sizeof(PhysicsBodyData); + + int newId = -1; + for (int i = 0; i < PHYSAC_MAX_BODIES; i++) + { + int currentId = i; + + // Check if current id already exist in other physics body + for (int k = 0; k < physicsBodiesCount; k++) + { + if (bodies[k]->id == currentId) + { + currentId++; + break; + } + } + + // If it is not used, use it as new physics body id + if (currentId == i) + { + newId = i; + break; + } + } + + if (newId != -1) + { + // Initialize new body with generic values + newBody->id = newId; + newBody->enabled = true; + newBody->position = pos; + newBody->velocity = (Vector2){ 0 }; + newBody->force = (Vector2){ 0 }; + newBody->angularVelocity = 0; + newBody->torque = 0; + newBody->orient = 0; + newBody->mass = PHYSAC_PI*radius*radius*density; + newBody->inverseMass = ((newBody->mass != 0.0f) ? 1.0f/newBody->mass : 0.0f); + newBody->inertia = newBody->mass*radius*radius; + newBody->inverseInertia = ((newBody->inertia != 0.0f) ? 1.0f/newBody->inertia : 0.0f); + newBody->staticFriction = 0; + newBody->dynamicFriction = 0; + newBody->restitution = 0; + newBody->useGravity = true; + newBody->freezeOrient = false; + newBody->shape.type = PHYSICS_CIRCLE; + newBody->shape.body = newBody; + newBody->shape.radius = radius; + + // Add new body to bodies pointers array and update bodies count + bodies[physicsBodiesCount] = newBody; + physicsBodiesCount++; + + #if defined(PHYSAC_DEBUG) + printf("[PHYSAC] created circle physics body id %i\n", newBody->id); + #endif + } + #if defined(PHYSAC_DEBUG) + else printf("[PHYSAC] new physics body creation failed because there is any available id to use\n"); + #endif + + return newBody;*/ +} + +// Creates a new rectangle physics body with generic parameters +PHYSACDEF PhysicsBody CreatePhysicsBodyRectangle(Vector2 pos, float width, float height, float density) +{ + PhysicsBody newBody = (PhysicsBody)PHYSAC_MALLOC(sizeof(PhysicsBodyData)); + usedMemory += sizeof(PhysicsBodyData); + + int newId = -1; + for (int i = 0; i < PHYSAC_MAX_BODIES; i++) + { + int currentId = i; + + // Check if current id already exist in other physics body + for (int k = 0; k < physicsBodiesCount; k++) + { + if (bodies[k]->id == currentId) + { + currentId++; + break; + } + } + + // If it is not used, use it as new physics body id + if (currentId == i) + { + newId = i; + break; + } + } + + if (newId != -1) + { + // Initialize new body with generic values + newBody->id = newId; + newBody->enabled = true; + newBody->position = pos; + newBody->velocity = (Vector2){ 0 }; + newBody->force = (Vector2){ 0 }; + newBody->angularVelocity = 0; + newBody->torque = 0; + newBody->orient = 0; + newBody->shape.type = PHYSICS_POLYGON; + newBody->shape.body = newBody; + newBody->shape.vertexData = CreateRectanglePolygon(pos, (Vector2){ width, height }); + + // Calculate centroid and moment of inertia + Vector2 center = { 0 }; + float area = 0; + float inertia = 0; + const float k = 1.0f/3.0f; + + for (int i = 0; i < newBody->shape.vertexData.vertexCount; i++) + { + // Triangle vertices, third vertex implied as (0, 0) + Vector2 p1 = newBody->shape.vertexData.vertices[i]; + int nextIndex = (((i + 1) < newBody->shape.vertexData.vertexCount) ? (i + 1) : 0); + Vector2 p2 = newBody->shape.vertexData.vertices[nextIndex]; + + float D = MathCrossVector2(p1, p2); + float triangleArea = D/2; + + area += triangleArea; + + // Use area to weight the centroid average, not just vertex position + center.x += triangleArea*k*(p1.x + p2.x); + center.y += triangleArea*k*(p1.y + p2.y); + + float intx2 = p1.x*p1.x + p2.x*p1.x + p2.x*p2.x; + float inty2 = p1.y*p1.y + p2.y*p1.y + p2.y*p2.y; + inertia += (0.25f*k*D)*(intx2 + inty2); + } + + center.x *= 1.0f/area; + center.y *= 1.0f/area; + + // Translate vertices to centroid (make the centroid (0, 0) for the polygon in model space) + // Note: this is not really necessary + for (int i = 0; i < newBody->shape.vertexData.vertexCount; i++) + { + newBody->shape.vertexData.vertices[i].x -= center.x; + newBody->shape.vertexData.vertices[i].y -= center.y; + } + + newBody->mass = density*area; + newBody->inverseMass = ((newBody->mass != 0.0f) ? 1.0f/newBody->mass : 0.0f); + newBody->inertia = density*inertia; + newBody->inverseInertia = ((newBody->inertia != 0.0f) ? 1.0f/newBody->inertia : 0.0f); + newBody->staticFriction = 0.4f; + newBody->dynamicFriction = 0.2f; + newBody->restitution = 0; + newBody->useGravity = true; + newBody->isGrounded = false; + newBody->freezeOrient = false; + + // Add new body to bodies pointers array and update bodies count + bodies[physicsBodiesCount] = newBody; + physicsBodiesCount++; + + #if defined(PHYSAC_DEBUG) + printf("[PHYSAC] created polygon physics body id %i\n", newBody->id); + #endif + } + #if defined(PHYSAC_DEBUG) + else printf("[PHYSAC] new physics body creation failed because there is any available id to use\n"); + #endif + + return newBody; +} + +// Creates a new polygon physics body with generic parameters +PHYSACDEF PhysicsBody CreatePhysicsBodyPolygon(Vector2 pos, float radius, int sides, float density) +{ + PhysicsBody newBody = (PhysicsBody)PHYSAC_MALLOC(sizeof(PhysicsBodyData)); + usedMemory += sizeof(PhysicsBodyData); + + int newId = -1; + for (int i = 0; i < PHYSAC_MAX_BODIES; i++) + { + int currentId = i; + + // Check if current id already exist in other physics body + for (int k = 0; k < physicsBodiesCount; k++) + { + if (bodies[k]->id == currentId) + { + currentId++; + break; + } + } + + // If it is not used, use it as new physics body id + if (currentId == i) + { + newId = i; + break; + } + } + + if (newId != -1) + { + // Initialize new body with generic values + newBody->id = newId; + newBody->enabled = true; + newBody->position = pos; + newBody->velocity = (Vector2){ 0 }; + newBody->force = (Vector2){ 0 }; + newBody->angularVelocity = 0; + newBody->torque = 0; + newBody->orient = 0; + newBody->shape.type = PHYSICS_POLYGON; + newBody->shape.body = newBody; + newBody->shape.vertexData = CreateRandomPolygon(radius, sides); + + // Calculate centroid and moment of inertia + Vector2 center = { 0 }; + float area = 0; + float inertia = 0; + const float alpha = 1.0f/3.0f; + + for (int i = 0; i < newBody->shape.vertexData.vertexCount; i++) + { + // Triangle vertices, third vertex implied as (0, 0) + Vector2 position1 = newBody->shape.vertexData.vertices[i]; + int nextIndex = (((i + 1) < newBody->shape.vertexData.vertexCount) ? (i + 1) : 0); + Vector2 position2 = newBody->shape.vertexData.vertices[nextIndex]; + + float cross = MathCrossVector2(position1, position2); + float triangleArea = cross/2; + + area += triangleArea; + + // Use area to weight the centroid average, not just vertex position + center.x += triangleArea*alpha*(position1.x + position2.x); + center.y += triangleArea*alpha*(position1.y + position2.y); + + float intx2 = position1.x*position1.x + position2.x*position1.x + position2.x*position2.x; + float inty2 = position1.y*position1.y + position2.y*position1.y + position2.y*position2.y; + inertia += (0.25f*alpha*cross)*(intx2 + inty2); + } + + center.x *= 1.0f/area; + center.y *= 1.0f/area; + + // Translate vertices to centroid (make the centroid (0, 0) for the polygon in model space) + // Note: this is not really necessary + for (int i = 0; i < newBody->shape.vertexData.vertexCount; i++) + { + newBody->shape.vertexData.vertices[i].x -= center.x; + newBody->shape.vertexData.vertices[i].y -= center.y; + } + + newBody->mass = density*area; + newBody->inverseMass = ((newBody->mass != 0.0f) ? 1.0f/newBody->mass : 0.0f); + newBody->inertia = density*inertia; + newBody->inverseInertia = ((newBody->inertia != 0.0f) ? 1.0f/newBody->inertia : 0.0f); + newBody->staticFriction = 0.4f; + newBody->dynamicFriction = 0.2f; + newBody->restitution = 0; + newBody->useGravity = true; + newBody->isGrounded = false; + newBody->freezeOrient = false; + + // Add new body to bodies pointers array and update bodies count + bodies[physicsBodiesCount] = newBody; + physicsBodiesCount++; + + #if defined(PHYSAC_DEBUG) + printf("[PHYSAC] created polygon physics body id %i\n", newBody->id); + #endif + } + #if defined(PHYSAC_DEBUG) + else printf("[PHYSAC] new physics body creation failed because there is any available id to use\n"); + #endif + + return newBody; +} + +// Adds a force to a physics body +PHYSACDEF void PhysicsAddForce(PhysicsBody body, Vector2 force) +{ + if (body != NULL) body->force = Vector2Add(body->force, force); +} + +// Adds an angular force to a physics body +PHYSACDEF void PhysicsAddTorque(PhysicsBody body, float amount) +{ + if (body != NULL) body->torque += amount; +} + +// Shatters a polygon shape physics body to little physics bodies with explosion force +PHYSACDEF void PhysicsShatter(PhysicsBody body, Vector2 position, float force) +{ + if (body != NULL) + { + if (body->shape.type == PHYSICS_POLYGON) + { + PolygonData vertexData = body->shape.vertexData; + bool collision = false; + + for (int i = 0; i < vertexData.vertexCount; i++) + { + Vector2 positionA = body->position; + Vector2 positionB = Mat2MultiplyVector2(vertexData.transform, Vector2Add(body->position, vertexData.vertices[i])); + int nextIndex = (((i + 1) < vertexData.vertexCount) ? (i + 1) : 0); + Vector2 positionC = Mat2MultiplyVector2(vertexData.transform, Vector2Add(body->position, vertexData.vertices[nextIndex])); + + // Check collision between each triangle + float alpha = ((positionB.y - positionC.y)*(position.x - positionC.x) + (positionC.x - positionB.x)*(position.y - positionC.y))/ + ((positionB.y - positionC.y)*(positionA.x - positionC.x) + (positionC.x - positionB.x)*(positionA.y - positionC.y)); + + float beta = ((positionC.y - positionA.y)*(position.x - positionC.x) + (positionA.x - positionC.x)*(position.y - positionC.y))/ + ((positionB.y - positionC.y)*(positionA.x - positionC.x) + (positionC.x - positionB.x)*(positionA.y - positionC.y)); + + float gamma = 1.0f - alpha - beta; + + if ((alpha > 0) && (beta > 0) & (gamma > 0)) + { + collision = true; + break; + } + } + + if (collision) + { + int count = vertexData.vertexCount; + Vector2 bodyPos = body->position; + Vector2 vertices[count]; + Mat2 trans = vertexData.transform; + for (int i = 0; i < count; i++) vertices[i] = vertexData.vertices[i]; + + // Destroy shattered physics body + DestroyPhysicsBody(body); + + for (int i = 0; i < count; i++) + { + int nextIndex = (((i + 1) < count) ? (i + 1) : 0); + Vector2 center = TriangleBarycenter(vertices[i], vertices[nextIndex], (Vector2){ 0, 0 }); + center = Vector2Add(bodyPos, center); + Vector2 offset = Vector2Subtract(center, bodyPos); + + PhysicsBody newBody = CreatePhysicsBodyPolygon(center, 10, 3, 10); // Create polygon physics body with relevant values + + PolygonData newData = { 0 }; + newData.vertexCount = 3; + newData.transform = trans; + + newData.vertices[0] = Vector2Subtract(vertices[i], offset); + newData.vertices[1] = Vector2Subtract(vertices[nextIndex], offset); + newData.vertices[2] = Vector2Subtract(position, center); + + // Separate vertices to avoid unnecessary physics collisions + newData.vertices[0].x *= 0.95f; + newData.vertices[0].y *= 0.95f; + newData.vertices[1].x *= 0.95f; + newData.vertices[1].y *= 0.95f; + newData.vertices[2].x *= 0.95f; + newData.vertices[2].y *= 0.95f; + + // Calculate polygon faces normals + for (int j = 0; j < newData.vertexCount; j++) + { + int nextVertex = (((j + 1) < newData.vertexCount) ? (j + 1) : 0); + Vector2 face = Vector2Subtract(newData.vertices[nextVertex], newData.vertices[j]); + + newData.normals[j] = (Vector2){ face.y, -face.x }; + MathNormalize(&newData.normals[j]); + } + + // Apply computed vertex data to new physics body shape + newBody->shape.vertexData = newData; + + // Calculate centroid and moment of inertia + center = (Vector2){ 0 }; + float area = 0; + float inertia = 0; + const float k = 1.0f/3.0f; + + for (int j = 0; j < newBody->shape.vertexData.vertexCount; j++) + { + // Triangle vertices, third vertex implied as (0, 0) + Vector2 p1 = newBody->shape.vertexData.vertices[j]; + int nextVertex = (((j + 1) < newBody->shape.vertexData.vertexCount) ? (j + 1) : 0); + Vector2 p2 = newBody->shape.vertexData.vertices[nextVertex]; + + float D = MathCrossVector2(p1, p2); + float triangleArea = D/2; + + area += triangleArea; + + // Use area to weight the centroid average, not just vertex position + center.x += triangleArea*k*(p1.x + p2.x); + center.y += triangleArea*k*(p1.y + p2.y); + + float intx2 = p1.x*p1.x + p2.x*p1.x + p2.x*p2.x; + float inty2 = p1.y*p1.y + p2.y*p1.y + p2.y*p2.y; + inertia += (0.25f*k*D)*(intx2 + inty2); + } + + center.x *= 1.0f/area; + center.y *= 1.0f/area; + + newBody->mass = area; + newBody->inverseMass = ((newBody->mass != 0.0f) ? 1.0f/newBody->mass : 0.0f); + newBody->inertia = inertia; + newBody->inverseInertia = ((newBody->inertia != 0.0f) ? 1.0f/newBody->inertia : 0.0f); + + // Calculate explosion force direction + Vector2 pointA = newBody->position; + Vector2 pointB = Vector2Subtract(newData.vertices[1], newData.vertices[0]); + pointB.x /= 2; + pointB.y /= 2; + Vector2 forceDirection = Vector2Subtract(Vector2Add(pointA, Vector2Add(newData.vertices[0], pointB)), newBody->position); + MathNormalize(&forceDirection); + forceDirection.x *= force; + forceDirection.y *= force; + + // Apply force to new physics body + PhysicsAddForce(newBody, forceDirection); + } + } + } + } + #if defined(PHYSAC_DEBUG) + else printf("[PHYSAC] error when trying to shatter a null reference physics body"); + #endif +} + +// Returns the current amount of created physics bodies +PHYSACDEF int GetPhysicsBodiesCount(void) +{ + return physicsBodiesCount; +} + +// Returns a physics body of the bodies pool at a specific index +PHYSACDEF PhysicsBody GetPhysicsBody(int index) +{ + if (index < physicsBodiesCount) + { + PhysicsBody body = bodies[index]; + if (body != NULL) return body; + else + { + #if defined(PHYSAC_DEBUG) + printf("[PHYSAC] error when trying to get a null reference physics body"); + #endif + + return NULL; + } + } + #if defined(PHYSAC_DEBUG) + else + { + printf("[PHYSAC] physics body index is out of bounds"); + return NULL; + } + #endif +} + +// Returns the physics body shape type (PHYSICS_CIRCLE or PHYSICS_POLYGON) +PHYSACDEF int GetPhysicsShapeType(int index) +{ + if (index < physicsBodiesCount) + { + PhysicsBody body = bodies[index]; + if (body != NULL) return body->shape.type; + #if defined(PHYSAC_DEBUG) + else + { + printf("[PHYSAC] error when trying to get a null reference physics body"); + return -1; + } + #endif + } + #if defined(PHYSAC_DEBUG) + else + { + printf("[PHYSAC] physics body index is out of bounds"); + return -1; + } + #endif +} + +// Returns the amount of vertices of a physics body shape +PHYSACDEF int GetPhysicsShapeVerticesCount(int index) +{ + if (index < physicsBodiesCount) + { + PhysicsBody body = bodies[index]; + if (body != NULL) + { + switch (body->shape.type) + { + case PHYSICS_CIRCLE: return PHYSAC_CIRCLE_VERTICES; break; + case PHYSICS_POLYGON: return body->shape.vertexData.vertexCount; break; + default: break; + } + } + #if defined(PHYSAC_DEBUG) + else + { + printf("[PHYSAC] error when trying to get a null reference physics body"); + return 0; + } + #endif + } + #if defined(PHYSAC_DEBUG) + else + { + printf("[PHYSAC] physics body index is out of bounds"); + return 0; + } + #endif +} + +// Returns transformed position of a body shape (body position + vertex transformed position) +PHYSACDEF Vector2 GetPhysicsShapeVertex(PhysicsBody body, int vertex) +{ + Vector2 position = { 0 }; + + if (body != NULL) + { + switch (body->shape.type) + { + case PHYSICS_CIRCLE: + { + position.x = body->position.x + cosf(360/PHYSAC_CIRCLE_VERTICES*vertex*PHYSAC_DEG2RAD)*body->shape.radius; + position.y = body->position.y + sinf(360/PHYSAC_CIRCLE_VERTICES*vertex*PHYSAC_DEG2RAD)*body->shape.radius; + } break; + case PHYSICS_POLYGON: + { + PolygonData vertexData = body->shape.vertexData; + position = Vector2Add(body->position, Mat2MultiplyVector2(vertexData.transform, vertexData.vertices[vertex])); + } break; + default: break; + } + } + #if defined(PHYSAC_DEBUG) + else printf("[PHYSAC] error when trying to get a null reference physics body"); + #endif + + return position; +} + +// Sets physics body shape transform based on radians parameter +PHYSACDEF void SetPhysicsBodyRotation(PhysicsBody body, float radians) +{ + if (body != NULL) + { + body->orient = radians; + + if (body->shape.type == PHYSICS_POLYGON) body->shape.vertexData.transform = Mat2Radians(radians); + } +} + +// Unitializes and destroys a physics body +PHYSACDEF void DestroyPhysicsBody(PhysicsBody body) +{ + if (body != NULL) + { + int id = body->id; + int index = -1; + + for (int i = 0; i < physicsBodiesCount; i++) + { + if (bodies[i]->id == id) + { + index = i; + break; + } + } + + #if defined(PHYSAC_DEBUG) + if (index == -1) printf("[PHYSAC] cannot find body id %i in pointers array\n", id); + #endif + + // Free body allocated memory + PHYSAC_FREE(bodies[index]); + usedMemory -= sizeof(PhysicsBodyData); + bodies[index] = NULL; + + // Reorder physics bodies pointers array and its catched index + for (int i = index; i < physicsBodiesCount; i++) + { + if ((i + 1) < physicsBodiesCount) bodies[i] = bodies[i + 1]; + } + + // Update physics bodies count + physicsBodiesCount--; + + #if defined(PHYSAC_DEBUG) + printf("[PHYSAC] destroyed physics body id %i\n", id); + #endif + } + #if defined(PHYSAC_DEBUG) + else printf("[PHYSAC] error trying to destroy a null referenced body\n"); + #endif +} + +// Destroys created physics bodies and manifolds and resets global values +PHYSACDEF void ResetPhysics(void) +{ + // Unitialize physics bodies dynamic memory allocations + for (int i = physicsBodiesCount - 1; i >= 0; i--) + { + PhysicsBody body = bodies[i]; + + if (body != NULL) + { + PHYSAC_FREE(body); + body = NULL; + usedMemory -= sizeof(PhysicsBodyData); + } + } + + physicsBodiesCount = 0; + + // Unitialize physics manifolds dynamic memory allocations + for (int i = physicsManifoldsCount - 1; i >= 0; i--) + { + PhysicsManifold manifold = contacts[i]; + + if (manifold != NULL) + { + PHYSAC_FREE(manifold); + manifold = NULL; + usedMemory -= sizeof(PhysicsManifoldData); + } + } + + physicsManifoldsCount = 0; + + #if defined(PHYSAC_DEBUG) + printf("[PHYSAC] physics module reset successfully\n"); + #endif +} + +// Unitializes physics pointers and exits physics loop thread +PHYSACDEF void ClosePhysics(void) +{ + // Exit physics loop thread 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; + + #if !defined(PHYSAC_NO_THREADS) + pthread_join(physicsThreadId, NULL); + #endif } -// Create a new physic body dinamically, initialize it and add to pool -PHYSACDEF PhysicBody CreatePhysicBody(Vector2 position, float rotation, Vector2 scale) +//---------------------------------------------------------------------------------- +// Module Internal Functions Definition +//---------------------------------------------------------------------------------- +// Creates a random polygon shape with max vertex distance from polygon pivot +static PolygonData CreateRandomPolygon(float radius, int sides) { - // 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; -} + PolygonData data = { 0 }; + data.vertexCount = sides; -// 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++) + float orient = GetRandomNumber(0, 360); + data.transform = Mat2Radians(orient*PHYSAC_DEG2RAD); + + // Calculate polygon vertices positions + for (int i = 0; i < data.vertexCount; 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]); + data.vertices[i].x = cosf(360/sides*i*PHYSAC_DEG2RAD)*radius; + data.vertices[i].y = sinf(360/sides*i*PHYSAC_DEG2RAD)*radius; } - - // Decrease enabled physic bodies count - physicBodiesCount--; -} -// Apply directional force to a physic body -PHYSACDEF void ApplyForce(PhysicBody pbody, Vector2 force) -{ - if (pbody->rigidbody.enabled) + // Calculate polygon faces normals + for (int i = 0; i < data.vertexCount; i++) { - pbody->rigidbody.velocity.x += force.x/pbody->rigidbody.mass; - pbody->rigidbody.velocity.y += force.y/pbody->rigidbody.mass; + int nextIndex = (((i + 1) < sides) ? (i + 1) : 0); + Vector2 face = Vector2Subtract(data.vertices[nextIndex], data.vertices[i]); + + data.normals[i] = (Vector2){ face.y, -face.x }; + MathNormalize(&data.normals[i]); } + + return data; } -// Apply radial force to all physic objects in range -PHYSACDEF void ApplyForceAtPosition(Vector2 position, float force, float radius) +// Creates a rectangle polygon shape based on a min and max positions +static PolygonData CreateRectanglePolygon(Vector2 pos, Vector2 size) { - for (int i = 0; i < physicBodiesCount; i++) + PolygonData data = { 0 }; + + data.vertexCount = 4; + data.transform = Mat2Radians(0); + + // Calculate polygon vertices positions + data.vertices[0] = (Vector2){ pos.x + size.x/2, pos.y - size.y/2 }; + data.vertices[1] = (Vector2){ pos.x + size.x/2, pos.y + size.y/2 }; + data.vertices[2] = (Vector2){ pos.x - size.x/2, pos.y + size.y/2 }; + data.vertices[3] = (Vector2){ pos.x - size.x/2, pos.y - size.y/2 }; + + // Calculate polygon faces normals + for (int i = 0; i < data.vertexCount; 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 }; + int nextIndex = (((i + 1) < data.vertexCount) ? (i + 1) : 0); + Vector2 face = Vector2Subtract(data.vertices[nextIndex], data.vertices[i]); - 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); - } - } + data.normals[i] = (Vector2){ face.y, -face.x }; + MathNormalize(&data.normals[i]); } + + return data; } -// Convert Transform data type to Rectangle (position and scale) -PHYSACDEF Rectangle TransformToRectangle(Transform transform) +// Physics loop thread function +static void *PhysicsLoop(void *arg) { - return (Rectangle){transform.position.x, transform.position.y, transform.scale.x, transform.scale.y}; -} + #if defined(PHYSAC_DEBUG) + printf("[PHYSAC] physics thread created with successfully\n"); + #endif -// Physics calculations thread function -PHYSACDEF void* PhysicsThread(void *arg) -{ - // Initialize thread loop state + // Initialize physics loop thread values physicsThreadEnabled = true; - - // Initialize hi-resolution timer - InitTimer(); - - // Physics update loop - while (physicsThreadEnabled) - { - currentTime = GetCurrentTime(); - double deltaTime = (double)(currentTime - previousTime); - previousTime = currentTime; + accumulator = 0; - // Delta time value needs to be inverse multiplied by physics time step value (1/target fps) - UpdatePhysics(deltaTime/PHYSICS_TIMESTEP); + // Initialize high resolution timer + InitTimer(); + + // Physics update loop + while (physicsThreadEnabled) + { + // Calculate current time + currentTime = GetCurrentTime(); + + // Calculate current delta time + deltaTime = currentTime - startTime; + + // Store the time elapsed since the last frame began + accumulator += deltaTime; + + // Clamp accumulator to max time step to avoid bad performance + MathClamp(&accumulator, 0, PHYSAC_MAX_TIMESTEP); + + // Fixed time stepping loop + while (accumulator >= PHYSAC_DESIRED_DELTATIME) + { + PhysicsStep(); + accumulator -= deltaTime; + } + + // Record the starting of this frame + startTime = currentTime; } - + + // Unitialize physics manifolds dynamic memory allocations + for (int i = physicsManifoldsCount - 1; i >= 0; i--) DestroyPhysicsManifold(contacts[i]); + + // Unitialize physics bodies dynamic memory allocations + for (int i = physicsBodiesCount - 1; i >= 0; i--) DestroyPhysicsBody(bodies[i]); + + #if defined(PHYSAC_DEBUG) + if (physicsBodiesCount > 0 || usedMemory != 0) printf("[PHYSAC] physics module closed with %i still allocated bodies [MEMORY: %i bytes]\n", physicsBodiesCount, usedMemory); + else if (physicsManifoldsCount > 0 || usedMemory != 0) printf("[PHYSAC] physics module closed with %i still allocated manifolds [MEMORY: %i bytes]\n", physicsManifoldsCount, usedMemory); + else printf("[PHYSAC] physics module closed successfully\n"); + #endif + return NULL; } -//---------------------------------------------------------------------------------- -// Module specific Functions Definition -//---------------------------------------------------------------------------------- -// Initialize hi-resolution timer -static void InitTimer(void) +// Physics steps calculations (dynamics, collisions and position corrections) +static void PhysicsStep(void) { -#if defined(PLATFORM_ANDROID) || defined(PLATFORM_RPI) - struct timespec now; + stepsCount++; - if (clock_gettime(CLOCK_MONOTONIC, &now) == 0) // Success + // Clear previous generated collisions information + for (int i = physicsManifoldsCount - 1; i >= 0; i--) { - baseTime = (uint64_t)now.tv_sec*1000000000LLU + (uint64_t)now.tv_nsec; + PhysicsManifold manifold = contacts[i]; + if (manifold != NULL) DestroyPhysicsManifold(manifold); } -#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++) + // Generate new collision information + for (int i = 0; i < physicsBodiesCount; i++) { - if (physicBodies[i]->enabled) + PhysicsBody bodyA = bodies[i]; + + if (bodyA != NULL) { - // Update physic behaviour - if (physicBodies[i]->rigidbody.enabled) + for (int j = i + 1; j < physicsBodiesCount; j++) { - // 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) + PhysicsBody bodyB = bodies[j]; + + if (bodyB != NULL) { - 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) + if ((bodyA->inverseMass == 0) && (bodyB->inverseMass == 0)) continue; + + PhysicsManifold manifold = NULL; + if (bodyA->shape.type == PHYSICS_POLYGON && bodyB->shape.type == PHYSICS_CIRCLE) manifold = CreatePhysicsManifold(bodyB, bodyA); + else manifold = CreatePhysicsManifold(bodyA, bodyB); + SolvePhysicsManifold(manifold); + + if (manifold->contactsCount > 0) { - // 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); - } - } - } + // Create a new manifold with same information as previously solved manifold and add it to the manifolds pool last slot + PhysicsManifold newManifold = CreatePhysicsManifold(bodyA, bodyB); + newManifold->penetration = manifold->penetration; + newManifold->normal = manifold->normal; + newManifold->contacts[0] = manifold->contacts[0]; + newManifold->contacts[1] = manifold->contacts[1]; + newManifold->contactsCount = manifold->contactsCount; + newManifold->restitution = manifold->restitution; + newManifold->dynamicFriction = manifold->dynamicFriction; + newManifold->staticFriction = manifold->staticFriction; } } } } } + + // Integrate forces to physics bodies + for (int i = 0; i < physicsBodiesCount; i++) + { + PhysicsBody body = bodies[i]; + if (body != NULL) IntegratePhysicsForces(body); + } + + // Initialize physics manifolds to solve collisions + for (int i = 0; i < physicsManifoldsCount; i++) + { + PhysicsManifold manifold = contacts[i]; + if (manifold != NULL) InitializePhysicsManifolds(manifold); + } + + // Integrate physics collisions impulses to solve collisions + for (int i = 0; i < PHYSAC_COLLISION_ITERATIONS; i++) + { + for (int j = 0; j < physicsManifoldsCount; j++) + { + PhysicsManifold manifold = contacts[i]; + if (manifold != NULL) IntegratePhysicsImpulses(manifold); + } + } + + // Integrate velocity to physics bodies + for (int i = 0; i < physicsBodiesCount; i++) + { + PhysicsBody body = bodies[i]; + if (body != NULL) IntegratePhysicsVelocity(body); + } + + // Correct physics bodies positions based on manifolds collision information + for (int i = 0; i < physicsManifoldsCount; i++) + { + PhysicsManifold manifold = contacts[i]; + if (manifold != NULL) CorrectPhysicsPositions(manifold); + } + + // Clear physics bodies forces + for (int i = 0; i < physicsBodiesCount; i++) + { + PhysicsBody body = bodies[i]; + if (body != NULL) + { + body->force = (Vector2){ 0 }; + body->torque = 0; + } + } } -#endif // PHYSAC_IMPLEMENTATION \ No newline at end of file +// Creates a new physics manifold to solve collision +static PhysicsManifold CreatePhysicsManifold(PhysicsBody a, PhysicsBody b) +{ + PhysicsManifold newManifold = (PhysicsManifold)PHYSAC_MALLOC(sizeof(PhysicsManifoldData)); + usedMemory += sizeof(PhysicsManifoldData); + + int newId = -1; + for (int i = 0; i < PHYSAC_MAX_MANIFOLDS; i++) + { + int currentId = i; + + // Check if current id already exist in other physics body + for (int k = 0; k < physicsManifoldsCount; k++) + { + if (contacts[k]->id == currentId) + { + currentId++; + break; + } + } + + // If it is not used, use it as new physics body id + if (currentId == i) + { + newId = i; + break; + } + } + + if (newId != -1) + { + // Initialize new manifold with generic values + newManifold->id = newId; + newManifold->bodyA = a; + newManifold->bodyB = b; + newManifold->penetration = 0; + newManifold->normal = (Vector2){ 0 }; + newManifold->contacts[0] = (Vector2){ 0 }; + newManifold->contacts[1] = (Vector2){ 0 }; + newManifold->contactsCount = 0; + newManifold->restitution = 0; + newManifold->dynamicFriction = 0; + newManifold->staticFriction = 0; + + // Add new body to bodies pointers array and update bodies count + contacts[physicsManifoldsCount] = newManifold; + physicsManifoldsCount++; + } + #if defined(PHYSAC_DEBUG) + else printf("[PHYSAC] new physics manifold creation failed because there is any available id to use\n"); + #endif + + return newManifold; +} + +// Unitializes and destroys a physics manifold +static void DestroyPhysicsManifold(PhysicsManifold manifold) +{ + if (manifold != NULL) + { + int id = manifold->id; + int index = -1; + + for (int i = 0; i < physicsManifoldsCount; i++) + { + if (contacts[i]->id == id) + { + index = i; + break; + } + } + + #if defined(PHYSAC_DEBUG) + if (index == -1) printf("[PHYSAC] cannot find manifold id %i in pointers array\n", id); + #endif + + // Free manifold allocated memory + PHYSAC_FREE(contacts[index]); + usedMemory -= sizeof(PhysicsManifoldData); + contacts[index] = NULL; + + // Reorder physics manifolds pointers array and its catched index + for (int i = index; i < physicsManifoldsCount; i++) + { + if ((i + 1) < physicsManifoldsCount) contacts[i] = contacts[i + 1]; + } + + // Update physics manifolds count + physicsManifoldsCount--; + } + #if defined(PHYSAC_DEBUG) + else printf("[PHYSAC] error trying to destroy a null referenced manifold\n"); + #endif +} + +// Solves a created physics manifold between two physics bodies +static void SolvePhysicsManifold(PhysicsManifold manifold) +{ + switch (manifold->bodyA->shape.type) + { + case PHYSICS_CIRCLE: + { + switch (manifold->bodyB->shape.type) + { + case PHYSICS_CIRCLE: SolveCircleToCircle(manifold); break; + case PHYSICS_POLYGON: SolveCircleToPolygon(manifold); break; + default: break; + } + } break; + case PHYSICS_POLYGON: + { + switch (manifold->bodyB->shape.type) + { + case PHYSICS_CIRCLE: SolvePolygonToCircle(manifold); break; + case PHYSICS_POLYGON: SolvePolygonToPolygon(manifold); break; + default: break; + } + } break; + default: break; + } + + // Update physics body grounded state if normal direction is downside + manifold->bodyB->isGrounded = (manifold->normal.y < 0); +} + +// Solves collision between two circle shape physics bodies +static void SolveCircleToCircle(PhysicsManifold manifold) +{ + PhysicsBody bodyA = manifold->bodyA; + PhysicsBody bodyB = manifold->bodyB; + + // Calculate translational vector, which is normal + Vector2 normal = Vector2Subtract(bodyB->position, bodyA->position); + + float distSqr = MathLenSqr(normal); + float radius = bodyA->shape.radius + bodyB->shape.radius; + + // Check if circles are not in contact + if (distSqr >= radius*radius) + { + manifold->contactsCount = 0; + return; + } + + float distance = sqrtf(distSqr); + manifold->contactsCount = 1; + + if (distance == 0) + { + manifold->penetration = bodyA->shape.radius; + manifold->normal = (Vector2){ 1, 0 }; + manifold->contacts[0] = bodyA->position; + } + else + { + manifold->penetration = radius - distance; + manifold->normal = (Vector2){ normal.x/distance, normal.y/distance }; // Faster than using MathNormalize() due to sqrt is already performed + manifold->contacts[0] = (Vector2){ manifold->normal.x*bodyA->shape.radius + bodyA->position.x, manifold->normal.y*bodyA->shape.radius + bodyA->position.y }; + } + + // Update physics body grounded state if normal direction is down + if (manifold->normal.y < 0) bodyA->isGrounded = true; +} + +// Solves collision between a circle to a polygon shape physics bodies +static void SolveCircleToPolygon(PhysicsManifold manifold) +{ + PhysicsBody bodyA = manifold->bodyA; + PhysicsBody bodyB = manifold->bodyB; + + manifold->contactsCount = 0; + + // Transform circle center to polygon transform space + Vector2 center = bodyA->position; + center = Mat2MultiplyVector2(Mat2Transpose(bodyB->shape.vertexData.transform), Vector2Subtract(center, bodyB->position)); + + // Find edge with minimum penetration + // It is the same concept as using support points in SolvePolygonToPolygon + float separation = -PHYSAC_FLT_MAX; + int faceNormal = 0; + PolygonData vertexData = bodyB->shape.vertexData; + + for (int i = 0; i < vertexData.vertexCount; i++) + { + float currentSeparation = MathDot(vertexData.normals[i], Vector2Subtract(center, vertexData.vertices[i])); + + if (currentSeparation > bodyA->shape.radius) return; + + if (currentSeparation > separation) + { + separation = currentSeparation; + faceNormal = i; + } + } + + // Grab face's vertices + Vector2 v1 = vertexData.vertices[faceNormal]; + int nextIndex = (((faceNormal + 1) < vertexData.vertexCount) ? (faceNormal + 1) : 0); + Vector2 v2 = vertexData.vertices[nextIndex]; + + // Check to see if center is within polygon + if (separation < PHYSAC_EPSILON) + { + manifold->contactsCount = 1; + Vector2 normal = Mat2MultiplyVector2(vertexData.transform, vertexData.normals[faceNormal]); + manifold->normal = (Vector2){ -normal.x, -normal.y }; + manifold->contacts[0] = (Vector2){ manifold->normal.x*bodyA->shape.radius + bodyA->position.x, manifold->normal.y*bodyA->shape.radius + bodyA->position.y }; + manifold->penetration = bodyA->shape.radius; + return; + } + + // Determine which voronoi region of the edge center of circle lies within + float dot1 = MathDot(Vector2Subtract(center, v1), Vector2Subtract(v2, v1)); + float dot2 = MathDot(Vector2Subtract(center, v2), Vector2Subtract(v1, v2)); + manifold->penetration = bodyA->shape.radius - separation; + + if (dot1 <= 0) // Closest to v1 + { + if (DistSqr(center, v1) > bodyA->shape.radius*bodyA->shape.radius) return; + + manifold->contactsCount = 1; + Vector2 normal = Vector2Subtract(v1, center); + normal = Mat2MultiplyVector2(vertexData.transform, normal); + MathNormalize(&normal); + manifold->normal = normal; + v1 = Mat2MultiplyVector2(vertexData.transform, v1); + v1 = Vector2Add(v1, bodyB->position); + manifold->contacts[0] = v1; + } + else if (dot2 <= 0) // Closest to v2 + { + if (DistSqr(center, v2) > bodyA->shape.radius*bodyA->shape.radius) return; + + manifold->contactsCount = 1; + Vector2 normal = Vector2Subtract(v2, center); + v2 = Mat2MultiplyVector2(vertexData.transform, v2); + v2 = Vector2Add(v2, bodyB->position); + manifold->contacts[0] = v2; + normal = Mat2MultiplyVector2(vertexData.transform, normal); + MathNormalize(&normal); + manifold->normal = normal; + } + else // Closest to face + { + Vector2 normal = vertexData.normals[faceNormal]; + + if (MathDot(Vector2Subtract(center, v1), normal) > bodyA->shape.radius) return; + + normal = Mat2MultiplyVector2(vertexData.transform, normal); + manifold->normal = (Vector2){ -normal.x, -normal.y }; + manifold->contacts[0] = (Vector2){ manifold->normal.x*bodyA->shape.radius + bodyA->position.x, manifold->normal.y*bodyA->shape.radius + bodyA->position.y }; + manifold->contactsCount = 1; + } +} + +// Solves collision between a polygon to a circle shape physics bodies +static void SolvePolygonToCircle(PhysicsManifold manifold) +{ + PhysicsBody bodyA = manifold->bodyA; + PhysicsBody bodyB = manifold->bodyB; + + manifold->bodyA = bodyB; + manifold->bodyB = bodyA; + SolveCircleToPolygon(manifold); + + manifold->normal.x *= -1; + manifold->normal.y *= -1; +} + +// Solves collision between two polygons shape physics bodies +static void SolvePolygonToPolygon(PhysicsManifold manifold) +{ + PhysicsShape bodyA = manifold->bodyA->shape; + PhysicsShape bodyB = manifold->bodyB->shape; + manifold->contactsCount = 0; + + // Check for separating axis with A shape's face planes + int faceA = 0; + float penetrationA = FindAxisLeastPenetration(&faceA, bodyA, bodyB); + if (penetrationA >= 0) return; + + // Check for separating axis with B shape's face planes + int faceB = 0; + float penetrationB = FindAxisLeastPenetration(&faceB, bodyB, bodyA); + if (penetrationB >= 0) return; + + int referenceIndex = 0; + bool flip = false; // Always point from A shape to B shape + + PhysicsShape refPoly; // Reference + PhysicsShape incPoly; // Incident + + // Determine which shape contains reference face + if (BiasGreaterThan(penetrationA, penetrationB)) + { + refPoly = bodyA; + incPoly = bodyB; + referenceIndex = faceA; + } + else + { + refPoly = bodyB; + incPoly = bodyA; + referenceIndex = faceB; + flip = true; + } + + // World space incident face + Vector2 incidentFace[2]; + FindIncidentFace(&incidentFace[0], &incidentFace[1], refPoly, incPoly, referenceIndex); + + // Setup reference face vertices + PolygonData refData = refPoly.vertexData; + Vector2 v1 = refData.vertices[referenceIndex]; + referenceIndex = (((referenceIndex + 1) < refData.vertexCount) ? (referenceIndex + 1) : 0); + Vector2 v2 = refData.vertices[referenceIndex]; + + // Transform vertices to world space + v1 = Mat2MultiplyVector2(refData.transform, v1); + v1 = Vector2Add(v1, refPoly.body->position); + v2 = Mat2MultiplyVector2(refData.transform, v2); + v2 = Vector2Add(v2, refPoly.body->position); + + // Calculate reference face side normal in world space + Vector2 sidePlaneNormal = Vector2Subtract(v2, v1); + MathNormalize(&sidePlaneNormal); + + // Orthogonalize + Vector2 refFaceNormal = { sidePlaneNormal.y, -sidePlaneNormal.x }; + float refC = MathDot(refFaceNormal, v1); + float negSide = MathDot(sidePlaneNormal, v1)*-1; + float posSide = MathDot(sidePlaneNormal, v2); + + // Clip incident face to reference face side planes (due to floating point error, possible to not have required points + if (Clip((Vector2){ -sidePlaneNormal.x, -sidePlaneNormal.y }, negSide, &incidentFace[0], &incidentFace[1]) < 2) return; + if (Clip(sidePlaneNormal, posSide, &incidentFace[0], &incidentFace[1]) < 2) return; + + // Flip normal if required + manifold->normal = (flip ? (Vector2){ -refFaceNormal.x, -refFaceNormal.y } : refFaceNormal); + + // Keep points behind reference face + int currentPoint = 0; // Clipped points behind reference face + float separation = MathDot(refFaceNormal, incidentFace[0]) - refC; + if (separation <= 0) + { + manifold->contacts[currentPoint] = incidentFace[0]; + manifold->penetration = -separation; + currentPoint++; + } + else manifold->penetration = 0; + + separation = MathDot(refFaceNormal, incidentFace[1]) - refC; + + if (separation <= 0) + { + manifold->contacts[currentPoint] = incidentFace[1]; + manifold->penetration += -separation; + currentPoint++; + + // Calculate total penetration average + manifold->penetration /= currentPoint; + } + + manifold->contactsCount = currentPoint; +} + +// Integrates physics forces into velocity +static void IntegratePhysicsForces(PhysicsBody body) +{ + if (body->inverseMass == 0 || !body->enabled) return; + + body->velocity.x += (body->force.x*body->inverseMass)*(deltaTime/2); + body->velocity.y += (body->force.y*body->inverseMass)*(deltaTime/2); + + if (body->useGravity) + { + body->velocity.x += gravityForce.x*(deltaTime/2); + body->velocity.y += gravityForce.y*(deltaTime/2); + } + + if (!body->freezeOrient) body->angularVelocity += body->torque*body->inverseInertia*(deltaTime/2); +} + +// Initializes physics manifolds to solve collisions +static void InitializePhysicsManifolds(PhysicsManifold manifold) +{ + PhysicsBody bodyA = manifold->bodyA; + PhysicsBody bodyB = manifold->bodyB; + + // Calculate average restitution, static and dynamic friction + manifold->restitution = sqrtf(bodyA->restitution*bodyB->restitution); + manifold->staticFriction = sqrtf(bodyA->staticFriction*bodyB->staticFriction); + manifold->dynamicFriction = sqrtf(bodyA->dynamicFriction*bodyB->dynamicFriction); + + for (int i = 0; i < 2; i++) + { + // Caculate radius from center of mass to contact + Vector2 radiusA = Vector2Subtract(manifold->contacts[i], bodyA->position); + Vector2 radiusB = Vector2Subtract(manifold->contacts[i], bodyB->position); + + Vector2 crossA = MathCross(bodyA->angularVelocity, radiusA); + Vector2 crossB = MathCross(bodyB->angularVelocity, radiusB); + + Vector2 radiusV = { 0 }; + radiusV.x = bodyB->velocity.x + crossB.x - bodyA->velocity.x - crossA.x; + radiusV.y = bodyB->velocity.y + crossB.y - bodyA->velocity.y - crossA.y; + + // Determine if we should perform a resting collision or not; + // The idea is if the only thing moving this object is gravity, then the collision should be performed without any restitution + if (MathLenSqr(radiusV) < (MathLenSqr((Vector2){ gravityForce.x*deltaTime, gravityForce.y*deltaTime }) + PHYSAC_EPSILON)) manifold->restitution = 0; + } +} + +// Integrates physics collisions impulses to solve collisions +static void IntegratePhysicsImpulses(PhysicsManifold manifold) +{ + PhysicsBody bodyA = manifold->bodyA; + PhysicsBody bodyB = manifold->bodyB; + + // Early out and positional correct if both objects have infinite mass + if (fabs(bodyA->inverseMass + bodyB->inverseMass) <= PHYSAC_EPSILON) + { + bodyA->velocity = (Vector2){ 0 }; + bodyB->velocity = (Vector2){ 0 }; + return; + } + + for (int i = 0; i < manifold->contactsCount; i++) + { + // Calculate radius from center of mass to contact + Vector2 radiusA = Vector2Subtract(manifold->contacts[i], bodyA->position); + Vector2 radiusB = Vector2Subtract(manifold->contacts[i], bodyB->position); + + // Calculate relative velocity + Vector2 radiusV = { 0 }; + radiusV.x = bodyB->velocity.x + MathCross(bodyB->angularVelocity, radiusB).x - bodyA->velocity.x - MathCross(bodyA->angularVelocity, radiusA).x; + radiusV.y = bodyB->velocity.y + MathCross(bodyB->angularVelocity, radiusB).y - bodyA->velocity.y - MathCross(bodyA->angularVelocity, radiusA).y; + + // Relative velocity along the normal + float contactVelocity = MathDot(radiusV, manifold->normal); + + // Do not resolve if velocities are separating + if (contactVelocity > 0) return; + + float raCrossN = MathCrossVector2(radiusA, manifold->normal); + float rbCrossN = MathCrossVector2(radiusB, manifold->normal); + + float inverseMassSum = bodyA->inverseMass + bodyB->inverseMass + (raCrossN*raCrossN)*bodyA->inverseInertia + (rbCrossN*rbCrossN)*bodyB->inverseInertia; + + // Calculate impulse scalar value + float impulse = -(1.0f + manifold->restitution)*contactVelocity; + impulse /= inverseMassSum; + impulse /= (float)manifold->contactsCount; + + // Apply impulse to each physics body + Vector2 impulseV = { manifold->normal.x*impulse, manifold->normal.y*impulse }; + + if (bodyA->enabled) + { + bodyA->velocity.x += bodyA->inverseMass*(-impulseV.x); + bodyA->velocity.y += bodyA->inverseMass*(-impulseV.y); + if (!bodyA->freezeOrient) bodyA->angularVelocity += bodyA->inverseInertia*MathCrossVector2(radiusA, (Vector2){ -impulseV.x, -impulseV.y }); + } + + if (bodyB->enabled) + { + bodyB->velocity.x += bodyB->inverseMass*(impulseV.x); + bodyB->velocity.y += bodyB->inverseMass*(impulseV.y); + if (!bodyB->freezeOrient) bodyB->angularVelocity += bodyB->inverseInertia*MathCrossVector2(radiusB, impulseV); + } + + // Apply friction impulse to each physics body + radiusV.x = bodyB->velocity.x + MathCross(bodyB->angularVelocity, radiusB).x - bodyA->velocity.x - MathCross(bodyA->angularVelocity, radiusA).x; + radiusV.y = bodyB->velocity.y + MathCross(bodyB->angularVelocity, radiusB).y - bodyA->velocity.y - MathCross(bodyA->angularVelocity, radiusA).y; + + Vector2 tangent = { radiusV.x - (manifold->normal.x*MathDot(radiusV, manifold->normal)), radiusV.y - (manifold->normal.y*MathDot(radiusV, manifold->normal)) }; + MathNormalize(&tangent); + + // Calculate impulse tangent magnitude + float impulseTangent = -MathDot(radiusV, tangent); + impulseTangent /= inverseMassSum; + impulseTangent /= (float)manifold->contactsCount; + + float absImpulseTangent = fabs(impulseTangent); + + // Don't apply tiny friction impulses + if (absImpulseTangent <= PHYSAC_EPSILON) return; + + // Apply coulumb's law + Vector2 tangentImpulse = { 0 }; + if (absImpulseTangent < impulse*manifold->staticFriction) tangentImpulse = (Vector2){ tangent.x*impulseTangent, tangent.y*impulseTangent }; + else tangentImpulse = (Vector2){ tangent.x*-impulse*manifold->dynamicFriction, tangent.y*-impulse*manifold->dynamicFriction }; + + // Apply friction impulse + if (bodyA->enabled) + { + bodyA->velocity.x += bodyA->inverseMass*(-tangentImpulse.x); + bodyA->velocity.y += bodyA->inverseMass*(-tangentImpulse.y); + + if (!bodyA->freezeOrient) bodyA->angularVelocity += bodyA->inverseInertia*MathCrossVector2(radiusA, (Vector2){ -tangentImpulse.x, -tangentImpulse.y }); + } + + if (bodyB->enabled) + { + bodyB->velocity.x += bodyB->inverseMass*(tangentImpulse.x); + bodyB->velocity.y += bodyB->inverseMass*(tangentImpulse.y); + + if (!bodyB->freezeOrient) bodyB->angularVelocity += bodyB->inverseInertia*MathCrossVector2(radiusB, tangentImpulse); + } + } +} + +// Integrates physics velocity into position and forces +static void IntegratePhysicsVelocity(PhysicsBody body) +{ + if (!body->enabled) return; + + body->position.x += body->velocity.x*deltaTime; + body->position.y += body->velocity.y*deltaTime; + + if (!body->freezeOrient) body->orient += body->angularVelocity*deltaTime; + Mat2Set(&body->shape.vertexData.transform, body->orient); + + IntegratePhysicsForces(body); +} + +// Corrects physics bodies positions based on manifolds collision information +static void CorrectPhysicsPositions(PhysicsManifold manifold) +{ + PhysicsBody bodyA = manifold->bodyA; + PhysicsBody bodyB = manifold->bodyB; + + Vector2 correction = { 0 }; + correction.x = (max(manifold->penetration - PHYSAC_PENETRATION_ALLOWANCE, 0)/(bodyA->inverseMass + bodyB->inverseMass))*manifold->normal.x*PHYSAC_PENETRATION_CORRECTION; + correction.y = (max(manifold->penetration - PHYSAC_PENETRATION_ALLOWANCE, 0)/(bodyA->inverseMass + bodyB->inverseMass))*manifold->normal.y*PHYSAC_PENETRATION_CORRECTION; + + if (bodyA->enabled) + { + bodyA->position.x -= correction.x*bodyA->inverseMass; + bodyA->position.y -= correction.y*bodyA->inverseMass; + } + + if (bodyB->enabled) + { + bodyB->position.x += correction.x*bodyB->inverseMass; + bodyB->position.y += correction.y*bodyB->inverseMass; + } +} + +// Returns the extreme point along a direction within a polygon +static Vector2 GetSupport(PhysicsShape shape, Vector2 dir) +{ + float bestProjection = -PHYSAC_FLT_MAX; + Vector2 bestVertex = { 0 }; + PolygonData data = shape.vertexData; + + for (int i = 0; i < data.vertexCount; i++) + { + Vector2 vertex = data.vertices[i]; + float projection = MathDot(vertex, dir); + + if (projection > bestProjection) + { + bestVertex = vertex; + bestProjection = projection; + } + } + + return bestVertex; +} + +// Finds polygon shapes axis least penetration +static float FindAxisLeastPenetration(int *faceIndex, PhysicsShape shapeA, PhysicsShape shapeB) +{ + float bestDistance = -PHYSAC_FLT_MAX; + int bestIndex = 0; + + PolygonData dataA = shapeA.vertexData; + PolygonData dataB = shapeB.vertexData; + + for (int i = 0; i < dataA.vertexCount; i++) + { + // Retrieve a face normal from A shape + Vector2 normal = dataA.normals[i]; + Vector2 transNormal = Mat2MultiplyVector2(dataA.transform, normal); + + // Transform face normal into B shape's model space + Mat2 buT = Mat2Transpose(dataB.transform); + normal = Mat2MultiplyVector2(buT, transNormal); + + // Retrieve support point from B shape along -n + Vector2 support = GetSupport(shapeB, (Vector2){ -normal.x, -normal.y }); + + // Retrieve vertex on face from A shape, transform into B shape's model space + Vector2 vertex = dataA.vertices[i]; + vertex = Mat2MultiplyVector2(dataA.transform, vertex); + vertex = Vector2Add(vertex, shapeA.body->position); + vertex = Vector2Subtract(vertex, shapeB.body->position); + vertex = Mat2MultiplyVector2(buT, vertex); + + // Compute penetration distance in B shape's model space + float distance = MathDot(normal, Vector2Subtract(support, vertex)); + + // Store greatest distance + if (distance > bestDistance) + { + bestDistance = distance; + bestIndex = i; + } + } + + *faceIndex = bestIndex; + return bestDistance; +} + +// Finds two polygon shapes incident face +static void FindIncidentFace(Vector2 *v0, Vector2 *v1, PhysicsShape ref, PhysicsShape inc, int index) +{ + PolygonData refData = ref.vertexData; + PolygonData incData = inc.vertexData; + + Vector2 referenceNormal = refData.normals[index]; + + // Calculate normal in incident's frame of reference + referenceNormal = Mat2MultiplyVector2(refData.transform, referenceNormal); // To world space + referenceNormal = Mat2MultiplyVector2(Mat2Transpose(incData.transform), referenceNormal); // To incident's model space + + // Find most anti-normal face on polygon + int incidentFace = 0; + float minDot = PHYSAC_FLT_MAX; + + for (int i = 0; i < incData.vertexCount; i++) + { + float dot = MathDot(referenceNormal, incData.normals[i]); + + if (dot < minDot) + { + minDot = dot; + incidentFace = i; + } + } + + // Assign face vertices for incident face + *v0 = Mat2MultiplyVector2(incData.transform, incData.vertices[incidentFace]); + *v0 = Vector2Add(*v0, inc.body->position); + incidentFace = (((incidentFace + 1) < incData.vertexCount) ? (incidentFace + 1) : 0); + *v1 = Mat2MultiplyVector2(incData.transform, incData.vertices[incidentFace]); + *v1 = Vector2Add(*v1, inc.body->position); +} + +// Calculates clipping based on a normal and two faces +static int Clip(Vector2 normal, float clip, Vector2 *faceA, Vector2 *faceB) +{ + int sp = 0; + Vector2 out[2] = { *faceA, *faceB }; + + // Retrieve distances from each endpoint to the line + float distanceA = MathDot(normal, *faceA) - clip; + float distanceB = MathDot(normal, *faceB) - clip; + + // If negative (behind plane) + if (distanceA <= 0) out[sp++] = *faceA; + if (distanceB <= 0) out[sp++] = *faceB; + + // If the points are on different sides of the plane + if ((distanceA*distanceB) < 0) + { + // Push intersection point + float alpha = distanceA/(distanceA - distanceB); + out[sp] = *faceA; + Vector2 delta = Vector2Subtract(*faceB, *faceA); + delta.x *= alpha; + delta.y *= alpha; + out[sp] = Vector2Add(out[sp], delta); + sp++; + } + + // Assign the new converted values + *faceA = out[0]; + *faceB = out[1]; + + return sp; +} + +// Check if values are between bias range +static bool BiasGreaterThan(float valueA, float valueB) +{ + return (valueA >= (valueB*0.95f + valueA*0.01f)); +} + +// Returns the barycenter of a triangle given by 3 points +static Vector2 TriangleBarycenter(Vector2 v1, Vector2 v2, Vector2 v3) +{ + Vector2 result = { 0 }; + + result.x = (v1.x + v2.x + v3.x)/3; + result.y = (v1.y + v2.y + v3.y)/3; + + return result; +} + +// Initializes hi-resolution timer +static void InitTimer(void) +{ + srand(time(NULL)); // Initialize random seed + + #if defined(PLATFORM_ANDROID) || defined(PLATFORM_RPI) + struct timespec now; + if (clock_gettime(CLOCK_MONOTONIC, &now) == 0) baseTime = (uint64_t)now.tv_sec*1000000000LLU + (uint64_t)now.tv_nsec; + #endif + + startTime = GetCurrentTime(); +} + +// Get current time in milliseconds +static double GetCurrentTime(void) +{ + double time = 0; + + #if defined(PLATFORM_DESKTOP) || defined(PLATFORM_WEB) + unsigned long long int clockFrequency, currentTime; + + QueryPerformanceFrequency(&clockFrequency); + QueryPerformanceCounter(¤tTime); + + time = (double)((double)currentTime/clockFrequency)*1000; + #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)((double)(temp - baseTime)*1e-6); + #endif + + return time; +} + +// Returns a random number between min and max (both included) +static int GetRandomNumber(int min, int max) +{ + if (min > max) + { + int tmp = max; + max = min; + min = tmp; + } + + return (rand()%(abs(max - min) + 1) + min); +} + +// Clamp a value in a range +static inline void MathClamp(double *value, double min, double max) +{ + if (*value < min) *value = min; + else if (*value > max) *value = max; +} + +// Returns the cross product of a vector and a value +static inline Vector2 MathCross(float value, Vector2 vector) +{ + return (Vector2){ -value*vector.y, value*vector.x }; +} + +// Returns the cross product of two vectors +static inline float MathCrossVector2(Vector2 v1, Vector2 v2) +{ + return (v1.x*v2.y - v1.y*v2.x); +} + +// Returns the len square root of a vector +static inline float MathLenSqr(Vector2 vector) +{ + return (vector.x*vector.x + vector.y*vector.y); +} + +// Returns the dot product of two vectors +static inline float MathDot(Vector2 v1, Vector2 v2) +{ + return (v1.x*v2.x + v1.y*v2.y); +} + +// Returns the square root of distance between two vectors +static inline float DistSqr(Vector2 v1, Vector2 v2) +{ + Vector2 dir = Vector2Subtract(v1, v2); + return MathDot(dir, dir); +} + +// Returns the normalized values of a vector +static void MathNormalize(Vector2 *vector) +{ + float length, ilength; + + Vector2 aux = *vector; + length = sqrtf(aux.x*aux.x + aux.y*aux.y); + + if (length == 0) length = 1.0f; + + ilength = 1.0f/length; + + vector->x *= ilength; + vector->y *= ilength; +} + +// Returns the sum of two given vectors +static inline Vector2 Vector2Add(Vector2 v1, Vector2 v2) +{ + return (Vector2){ v1.x + v2.x, v1.y + v2.y }; +} + +// Returns the subtract of two given vectors +static inline Vector2 Vector2Subtract(Vector2 v1, Vector2 v2) +{ + return (Vector2){ v1.x - v2.x, v1.y - v2.y }; +} + +// Creates a matrix 2x2 from a given radians value +static inline Mat2 Mat2Radians(float radians) +{ + float c = cosf(radians); + float s = sinf(radians); + + return (Mat2){ c, -s, s, c }; +} + +// Set values from radians to a created matrix 2x2 +static void Mat2Set(Mat2 *matrix, float radians) +{ + float cos = cosf(radians); + float sin = sinf(radians); + + matrix->m00 = cos; + matrix->m01 = -sin; + matrix->m10 = sin; + matrix->m11 = cos; +} + +// Returns the transpose of a given matrix 2x2 +static inline Mat2 Mat2Transpose(Mat2 matrix) +{ + return (Mat2){ matrix.m00, matrix.m10, matrix.m01, matrix.m11 }; +} + +// Multiplies a vector by a matrix 2x2 +static inline Vector2 Mat2MultiplyVector2(Mat2 matrix, Vector2 vector) +{ + return (Vector2){ matrix.m00*vector.x + matrix.m01*vector.y, matrix.m10*vector.x + matrix.m11*vector.y }; +} + +#endif // PHYSAC_IMPLEMENTATION