|
|
@@ -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 PhysicsShapeType { PHYSICS_CIRCLE, PHYSICS_POLYGON } PhysicsShapeType;
|
|
|
+
|
|
|
+// Previously defined to be used in PhysicsShape struct as circular dependencies
|
|
|
+typedef struct PhysicsBodyData *PhysicsBody;
|
|
|
+
|
|
|
+// Mat2 type (used for polygon shape rotation matrix)
|
|
|
+typedef struct Mat2
|
|
|
+{
|
|
|
+ float m00;
|
|
|
+ float m01;
|
|
|
+ float m10;
|
|
|
+ float m11;
|
|
|
+} Mat2;
|
|
|
+
|
|
|
+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 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
|
|
|
|
|
|
-typedef enum { COLLIDER_CIRCLE, COLLIDER_RECTANGLE } ColliderType;
|
|
|
-
|
|
|
-typedef struct Transform {
|
|
|
- Vector2 position;
|
|
|
- float rotation; // Radians (not used)
|
|
|
- Vector2 scale; // Just for rectangle physic objects, for circle physic objects use collider radius and keep scale as { 0, 0 }
|
|
|
-} Transform;
|
|
|
-
|
|
|
-typedef struct Rigidbody {
|
|
|
- bool enabled; // Acts as kinematic state (collisions are calculated anyway)
|
|
|
- float mass;
|
|
|
- Vector2 acceleration;
|
|
|
- Vector2 velocity;
|
|
|
- bool applyGravity;
|
|
|
- bool isGrounded;
|
|
|
- float friction; // Normalized value
|
|
|
- float bounciness;
|
|
|
-} Rigidbody;
|
|
|
-
|
|
|
-typedef struct Collider {
|
|
|
- bool enabled;
|
|
|
- ColliderType type;
|
|
|
- Rectangle bounds; // Used for COLLIDER_RECTANGLE
|
|
|
- int radius; // Used for COLLIDER_CIRCLE
|
|
|
-} Collider;
|
|
|
-
|
|
|
-typedef struct PhysicBodyData {
|
|
|
- unsigned int id;
|
|
|
- Transform transform;
|
|
|
- Rigidbody rigidbody;
|
|
|
- Collider collider;
|
|
|
- bool enabled;
|
|
|
-} PhysicBodyData, *PhysicBody;
|
|
|
+//----------------------------------------------------------------------------------
|
|
|
+// 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 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 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 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 <stdlib.h> // Required for: malloc(), free()
|
|
|
-
|
|
|
- #define PHYSAC_MALLOC(size) malloc(size)
|
|
|
- #define PHYSAC_FREE(ptr) free(ptr)
|
|
|
+#if !defined(PHYSAC_NO_THREADS)
|
|
|
+ #include <pthread.h> // Required for: pthread_t, pthread_create()
|
|
|
#endif
|
|
|
|
|
|
-#include <math.h> // Required for: cos(), sin(), abs(), fminf()
|
|
|
-#include <stdint.h> // Required for typedef unsigned long long int uint64_t, used by hi-res timer
|
|
|
-
|
|
|
-#ifndef PHYSAC_NO_THREADS
|
|
|
- #include <pthread.h> // Required for: pthread_create()
|
|
|
+#if defined(PHYSAC_DEBUG)
|
|
|
+ #include <stdio.h> // Required for: printf()
|
|
|
#endif
|
|
|
|
|
|
-#if defined(PLATFORM_DESKTOP)
|
|
|
+#include <stdlib.h> // Required for: malloc(), free(), srand(), rand()
|
|
|
+#include <math.h> // 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 <sys/time.h> // Required for: timespec
|
|
|
- #include <time.h> // Required for: clock_gettime()
|
|
|
+#elif defined(__linux)
|
|
|
+ #include <sys/time.h> // Required for: timespec
|
|
|
+ #include <time.h> // Required for: clock_gettime()
|
|
|
#endif
|
|
|
|
|
|
//----------------------------------------------------------------------------------
|
|
|
// Defines and Macros
|
|
|
//----------------------------------------------------------------------------------
|
|
|
-#define MAX_PHYSIC_BODIES 256 // Maximum available physic bodies slots in bodies pool
|
|
|
-#define PHYSICS_TIMESTEP 0.016666 // Physics fixed time step (1/fps)
|
|
|
-#define PHYSICS_ACCURACY 0.0001f // Velocity subtract operations round filter (friction)
|
|
|
-#define PHYSICS_ERRORPERCENT 0.001f // Collision resolve position fix
|
|
|
+#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 physics values, pointers and creates physics loop thread
|
|
|
+PHYSACDEF void InitPhysics(void)
|
|
|
+{
|
|
|
+ #if defined(PHYSAC_DEBUG)
|
|
|
+ printf("[PHYSAC] physics module initialized successfully\n");
|
|
|
+ #endif
|
|
|
|
|
|
-// Initializes pointers array (just pointers, fixed size)
|
|
|
-PHYSACDEF void InitPhysics(Vector2 gravity)
|
|
|
-{
|
|
|
- // Initialize physics variables
|
|
|
- physicBodiesCount = 0;
|
|
|
- gravityForce = gravity;
|
|
|
-
|
|
|
- #ifndef PHYSAC_NO_THREADS // NOTE: if defined, user will need to create a thread for PhysicsThread function manually
|
|
|
- // Create physics thread
|
|
|
- pthread_t tid;
|
|
|
- pthread_create(&tid, NULL, &PhysicsThread, NULL);
|
|
|
+ #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
|
|
|
- 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;
|
|
|
+ return physicsThreadEnabled;
|
|
|
+}
|
|
|
+
|
|
|
+// Sets physics global gravity force
|
|
|
+PHYSACDEF void SetPhysicsGravity(float x, float y)
|
|
|
+{
|
|
|
+ gravityForce.x = x;
|
|
|
+ gravityForce.y = y;
|
|
|
}
|
|
|
|
|
|
-// Create a new physic body dinamically, initialize it and add to pool
|
|
|
-PHYSACDEF PhysicBody CreatePhysicBody(Vector2 position, float rotation, Vector2 scale)
|
|
|
+// Creates a new circle physics body with generic parameters
|
|
|
+PHYSACDEF PhysicsBody CreatePhysicsBodyCircle(Vector2 pos, float radius, float density)
|
|
|
{
|
|
|
- // 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;
|
|
|
+ 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;*/
|
|
|
}
|
|
|
|
|
|
-// Destroy a specific physic body and take it out of the list
|
|
|
-PHYSACDEF void DestroyPhysicBody(PhysicBody pbody)
|
|
|
+// Creates a new rectangle physics body with generic parameters
|
|
|
+PHYSACDEF PhysicsBody CreatePhysicsBodyRectangle(Vector2 pos, float width, float height, float density)
|
|
|
{
|
|
|
- // Free dynamic memory allocation
|
|
|
- PHYSAC_FREE(physicBodies[pbody->id]);
|
|
|
-
|
|
|
- // Remove *obj from the pointers array
|
|
|
- for (int i = pbody->id; i < physicBodiesCount; i++)
|
|
|
+ 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)
|
|
|
{
|
|
|
- // Resort all the following pointers of the array
|
|
|
- if ((i + 1) < physicBodiesCount)
|
|
|
+ // 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++)
|
|
|
{
|
|
|
- physicBodies[i] = physicBodies[i + 1];
|
|
|
- physicBodies[i]->id = physicBodies[i + 1]->id;
|
|
|
+ newBody->shape.vertexData.vertices[i].x -= center.x;
|
|
|
+ newBody->shape.vertexData.vertices[i].y -= center.y;
|
|
|
}
|
|
|
- else PHYSAC_FREE(physicBodies[i]);
|
|
|
+
|
|
|
+ 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
|
|
|
}
|
|
|
-
|
|
|
- // Decrease enabled physic bodies count
|
|
|
- physicBodiesCount--;
|
|
|
+ #if defined(PHYSAC_DEBUG)
|
|
|
+ else printf("[PHYSAC] new physics body creation failed because there is any available id to use\n");
|
|
|
+ #endif
|
|
|
+
|
|
|
+ return newBody;
|
|
|
}
|
|
|
|
|
|
-// Apply directional force to a physic body
|
|
|
-PHYSACDEF void ApplyForce(PhysicBody pbody, Vector2 force)
|
|
|
+// Creates a new polygon physics body with generic parameters
|
|
|
+PHYSACDEF PhysicsBody CreatePhysicsBodyPolygon(Vector2 pos, float radius, int sides, float density)
|
|
|
{
|
|
|
- if (pbody->rigidbody.enabled)
|
|
|
+ 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)
|
|
|
{
|
|
|
- pbody->rigidbody.velocity.x += force.x/pbody->rigidbody.mass;
|
|
|
- pbody->rigidbody.velocity.y += force.y/pbody->rigidbody.mass;
|
|
|
+ // 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;
|
|
|
}
|
|
|
|
|
|
-// Apply radial force to all physic objects in range
|
|
|
-PHYSACDEF void ApplyForceAtPosition(Vector2 position, float force, float radius)
|
|
|
+// Shatters a polygon shape physics body to little physics bodies with explosion force
|
|
|
+PHYSACDEF void PhysicsShatter(PhysicsBody body, Vector2 position, float force)
|
|
|
{
|
|
|
- for (int i = 0; i < physicBodiesCount; i++)
|
|
|
+ if (body != NULL)
|
|
|
{
|
|
|
- if (physicBodies[i]->rigidbody.enabled)
|
|
|
+ if (body->shape.type == PHYSICS_POLYGON)
|
|
|
{
|
|
|
- // 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 };
|
|
|
+ PolygonData vertexData = body->shape.vertexData;
|
|
|
+ bool collision = false;
|
|
|
|
|
|
- if (physicBodies[i]->collider.type == COLLIDER_RECTANGLE)
|
|
|
+ for (int i = 0; i < vertexData.vertexCount; i++)
|
|
|
{
|
|
|
- distance.x += physicBodies[i]->transform.scale.x/2;
|
|
|
- distance.y += physicBodies[i]->transform.scale.y/2;
|
|
|
+ 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;
|
|
|
+ }
|
|
|
}
|
|
|
-
|
|
|
- float distanceLength = Vector2Length(distance);
|
|
|
-
|
|
|
- // Check if physic body is in force range
|
|
|
- if (distanceLength <= radius)
|
|
|
+
|
|
|
+ if (collision)
|
|
|
{
|
|
|
- // 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);
|
|
|
+ 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
|
|
|
}
|
|
|
|
|
|
-// Convert Transform data type to Rectangle (position and scale)
|
|
|
-PHYSACDEF Rectangle TransformToRectangle(Transform transform)
|
|
|
+// Returns the current amount of created physics bodies
|
|
|
+PHYSACDEF int GetPhysicsBodiesCount(void)
|
|
|
{
|
|
|
- return (Rectangle){transform.position.x, transform.position.y, transform.scale.x, transform.scale.y};
|
|
|
+ return physicsBodiesCount;
|
|
|
}
|
|
|
|
|
|
-// Physics calculations thread function
|
|
|
-PHYSACDEF void* PhysicsThread(void *arg)
|
|
|
+// Returns a physics body of the bodies pool at a specific index
|
|
|
+PHYSACDEF PhysicsBody GetPhysicsBody(int index)
|
|
|
{
|
|
|
- // Initialize thread loop state
|
|
|
- physicsThreadEnabled = true;
|
|
|
-
|
|
|
- // Initialize hi-resolution timer
|
|
|
- InitTimer();
|
|
|
-
|
|
|
- // Physics update loop
|
|
|
- while (physicsThreadEnabled)
|
|
|
+ if (index < physicsBodiesCount)
|
|
|
{
|
|
|
- currentTime = GetCurrentTime();
|
|
|
- double deltaTime = (double)(currentTime - previousTime);
|
|
|
- previousTime = currentTime;
|
|
|
+ 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
|
|
|
|
|
|
- // Delta time value needs to be inverse multiplied by physics time step value (1/target fps)
|
|
|
- UpdatePhysics(deltaTime/PHYSICS_TIMESTEP);
|
|
|
+ return NULL;
|
|
|
+ }
|
|
|
}
|
|
|
-
|
|
|
- return NULL;
|
|
|
+ #if defined(PHYSAC_DEBUG)
|
|
|
+ else
|
|
|
+ {
|
|
|
+ printf("[PHYSAC] physics body index is out of bounds");
|
|
|
+ return NULL;
|
|
|
+ }
|
|
|
+ #endif
|
|
|
}
|
|
|
|
|
|
-//----------------------------------------------------------------------------------
|
|
|
-// Module specific Functions Definition
|
|
|
-//----------------------------------------------------------------------------------
|
|
|
-// Initialize hi-resolution timer
|
|
|
-static void InitTimer(void)
|
|
|
+// Returns the physics body shape type (PHYSICS_CIRCLE or PHYSICS_POLYGON)
|
|
|
+PHYSACDEF int GetPhysicsShapeType(int index)
|
|
|
{
|
|
|
-#if defined(PLATFORM_ANDROID) || defined(PLATFORM_RPI)
|
|
|
- struct timespec now;
|
|
|
-
|
|
|
- if (clock_gettime(CLOCK_MONOTONIC, &now) == 0) // Success
|
|
|
+ if (index < physicsBodiesCount)
|
|
|
{
|
|
|
- baseTime = (uint64_t)now.tv_sec*1000000000LLU + (uint64_t)now.tv_nsec;
|
|
|
+ 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
|
|
|
}
|
|
|
-#endif
|
|
|
-
|
|
|
- previousTime = GetCurrentTime(); // Get time as double
|
|
|
+ #if defined(PHYSAC_DEBUG)
|
|
|
+ else
|
|
|
+ {
|
|
|
+ printf("[PHYSAC] physics body index is out of bounds");
|
|
|
+ return -1;
|
|
|
+ }
|
|
|
+ #endif
|
|
|
}
|
|
|
|
|
|
-// Time measure returned are microseconds
|
|
|
-static double GetCurrentTime(void)
|
|
|
+// Returns the amount of vertices of a physics body shape
|
|
|
+PHYSACDEF int GetPhysicsShapeVerticesCount(int index)
|
|
|
{
|
|
|
- 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;
|
|
|
+ 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 the dot product of two Vector2
|
|
|
-static float Vector2DotProduct(Vector2 v1, Vector2 v2)
|
|
|
+// Returns transformed position of a body shape (body position + vertex transformed position)
|
|
|
+PHYSACDEF Vector2 GetPhysicsShapeVertex(PhysicsBody body, int vertex)
|
|
|
{
|
|
|
- float result;
|
|
|
+ Vector2 position = { 0 };
|
|
|
|
|
|
- result = v1.x*v2.x + v1.y*v2.y;
|
|
|
+ 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 result;
|
|
|
+ return position;
|
|
|
}
|
|
|
|
|
|
-static float Vector2Length(Vector2 v)
|
|
|
+// Sets physics body shape transform based on radians parameter
|
|
|
+PHYSACDEF void SetPhysicsBodyRotation(PhysicsBody body, float radians)
|
|
|
{
|
|
|
- float result;
|
|
|
-
|
|
|
- result = sqrt(v.x*v.x + v.y*v.y);
|
|
|
-
|
|
|
- return result;
|
|
|
+ if (body != NULL)
|
|
|
+ {
|
|
|
+ body->orient = radians;
|
|
|
+
|
|
|
+ if (body->shape.type == PHYSICS_POLYGON) body->shape.vertexData.transform = Mat2Radians(radians);
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
-// Update physic objects, calculating physic behaviours and collisions detection
|
|
|
-static void UpdatePhysics(double deltaTime)
|
|
|
+// Unitializes and destroys a physics body
|
|
|
+PHYSACDEF void DestroyPhysicsBody(PhysicsBody body)
|
|
|
{
|
|
|
- for (int i = 0; i < physicBodiesCount; i++)
|
|
|
+ if (body != NULL)
|
|
|
{
|
|
|
- if (physicBodies[i]->enabled)
|
|
|
+ int id = body->id;
|
|
|
+ int index = -1;
|
|
|
+
|
|
|
+ for (int i = 0; i < physicsBodiesCount; i++)
|
|
|
{
|
|
|
- // Update physic behaviour
|
|
|
- if (physicBodies[i]->rigidbody.enabled)
|
|
|
- {
|
|
|
- // Apply friction to acceleration in X axis
|
|
|
- if (physicBodies[i]->rigidbody.acceleration.x > PHYSICS_ACCURACY) physicBodies[i]->rigidbody.acceleration.x -= physicBodies[i]->rigidbody.friction*deltaTime;
|
|
|
- else if (physicBodies[i]->rigidbody.acceleration.x < PHYSICS_ACCURACY) physicBodies[i]->rigidbody.acceleration.x += physicBodies[i]->rigidbody.friction*deltaTime;
|
|
|
- else physicBodies[i]->rigidbody.acceleration.x = 0.0f;
|
|
|
-
|
|
|
- // Apply friction to acceleration in Y axis
|
|
|
- if (physicBodies[i]->rigidbody.acceleration.y > PHYSICS_ACCURACY) physicBodies[i]->rigidbody.acceleration.y -= physicBodies[i]->rigidbody.friction*deltaTime;
|
|
|
- else if (physicBodies[i]->rigidbody.acceleration.y < PHYSICS_ACCURACY) physicBodies[i]->rigidbody.acceleration.y += physicBodies[i]->rigidbody.friction*deltaTime;
|
|
|
- else physicBodies[i]->rigidbody.acceleration.y = 0.0f;
|
|
|
-
|
|
|
- // Apply friction to velocity in X axis
|
|
|
- if (physicBodies[i]->rigidbody.velocity.x > PHYSICS_ACCURACY) physicBodies[i]->rigidbody.velocity.x -= physicBodies[i]->rigidbody.friction*deltaTime;
|
|
|
- else if (physicBodies[i]->rigidbody.velocity.x < PHYSICS_ACCURACY) physicBodies[i]->rigidbody.velocity.x += physicBodies[i]->rigidbody.friction*deltaTime;
|
|
|
- else physicBodies[i]->rigidbody.velocity.x = 0.0f;
|
|
|
-
|
|
|
- // Apply friction to velocity in Y axis
|
|
|
- if (physicBodies[i]->rigidbody.velocity.y > PHYSICS_ACCURACY) physicBodies[i]->rigidbody.velocity.y -= physicBodies[i]->rigidbody.friction*deltaTime;
|
|
|
- else if (physicBodies[i]->rigidbody.velocity.y < PHYSICS_ACCURACY) physicBodies[i]->rigidbody.velocity.y += physicBodies[i]->rigidbody.friction*deltaTime;
|
|
|
- else physicBodies[i]->rigidbody.velocity.y = 0.0f;
|
|
|
-
|
|
|
- // Apply gravity to velocity
|
|
|
- if (physicBodies[i]->rigidbody.applyGravity)
|
|
|
- {
|
|
|
- physicBodies[i]->rigidbody.velocity.x += gravityForce.x*deltaTime;
|
|
|
- physicBodies[i]->rigidbody.velocity.y += gravityForce.y*deltaTime;
|
|
|
- }
|
|
|
-
|
|
|
- // Apply acceleration to velocity
|
|
|
- physicBodies[i]->rigidbody.velocity.x += physicBodies[i]->rigidbody.acceleration.x*deltaTime;
|
|
|
- physicBodies[i]->rigidbody.velocity.y += physicBodies[i]->rigidbody.acceleration.y*deltaTime;
|
|
|
-
|
|
|
- // Apply velocity to position
|
|
|
- physicBodies[i]->transform.position.x += physicBodies[i]->rigidbody.velocity.x*deltaTime;
|
|
|
- physicBodies[i]->transform.position.y -= physicBodies[i]->rigidbody.velocity.y*deltaTime;
|
|
|
- }
|
|
|
-
|
|
|
- // Update collision detection
|
|
|
- if (physicBodies[i]->collider.enabled)
|
|
|
+ if (bodies[i]->id == id)
|
|
|
{
|
|
|
- // Update collider bounds
|
|
|
- physicBodies[i]->collider.bounds = TransformToRectangle(physicBodies[i]->transform);
|
|
|
-
|
|
|
- // Check collision with other colliders
|
|
|
- for (int k = 0; k < physicBodiesCount; k++)
|
|
|
- {
|
|
|
- if (physicBodies[k]->collider.enabled && i != k)
|
|
|
- {
|
|
|
- // Resolve physic collision
|
|
|
- // NOTE: collision resolve is generic for all directions and conditions (no axis separated cases behaviours)
|
|
|
- // and it is separated in rigidbody attributes resolve (velocity changes by impulse) and position correction (position overlap)
|
|
|
-
|
|
|
- // 1. Calculate collision normal
|
|
|
- // -------------------------------------------------------------------------------------------------------------------------------------
|
|
|
-
|
|
|
- // Define collision contact normal, direction and penetration depth
|
|
|
- Vector2 contactNormal = { 0.0f, 0.0f };
|
|
|
- Vector2 direction = { 0.0f, 0.0f };
|
|
|
- float penetrationDepth = 0.0f;
|
|
|
-
|
|
|
- switch (physicBodies[i]->collider.type)
|
|
|
- {
|
|
|
- case COLLIDER_RECTANGLE:
|
|
|
- {
|
|
|
- switch (physicBodies[k]->collider.type)
|
|
|
- {
|
|
|
- case COLLIDER_RECTANGLE:
|
|
|
- {
|
|
|
- // Check if colliders are overlapped
|
|
|
- if (CheckCollisionRecs(physicBodies[i]->collider.bounds, physicBodies[k]->collider.bounds))
|
|
|
- {
|
|
|
- // Calculate direction vector from i to k
|
|
|
- direction.x = (physicBodies[k]->transform.position.x + physicBodies[k]->transform.scale.x/2) - (physicBodies[i]->transform.position.x + physicBodies[i]->transform.scale.x/2);
|
|
|
- direction.y = (physicBodies[k]->transform.position.y + physicBodies[k]->transform.scale.y/2) - (physicBodies[i]->transform.position.y + physicBodies[i]->transform.scale.y/2);
|
|
|
-
|
|
|
- // Define overlapping and penetration attributes
|
|
|
- Vector2 overlap;
|
|
|
-
|
|
|
- // Calculate overlap on X axis
|
|
|
- overlap.x = (physicBodies[i]->transform.scale.x + physicBodies[k]->transform.scale.x)/2 - abs(direction.x);
|
|
|
-
|
|
|
- // SAT test on X axis
|
|
|
- if (overlap.x > 0.0f)
|
|
|
- {
|
|
|
- // Calculate overlap on Y axis
|
|
|
- overlap.y = (physicBodies[i]->transform.scale.y + physicBodies[k]->transform.scale.y)/2 - abs(direction.y);
|
|
|
-
|
|
|
- // SAT test on Y axis
|
|
|
- if (overlap.y > 0.0f)
|
|
|
- {
|
|
|
- // Find out which axis is axis of least penetration
|
|
|
- if (overlap.y > overlap.x)
|
|
|
- {
|
|
|
- // Point towards k knowing that direction points from i to k
|
|
|
- if (direction.x < 0.0f) contactNormal = (Vector2){ -1.0f, 0.0f };
|
|
|
- else contactNormal = (Vector2){ 1.0f, 0.0f };
|
|
|
-
|
|
|
- // Update penetration depth for position correction
|
|
|
- penetrationDepth = overlap.x;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // Point towards k knowing that direction points from i to k
|
|
|
- if (direction.y < 0.0f) contactNormal = (Vector2){ 0.0f, 1.0f };
|
|
|
- else contactNormal = (Vector2){ 0.0f, -1.0f };
|
|
|
-
|
|
|
- // Update penetration depth for position correction
|
|
|
- penetrationDepth = overlap.y;
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- } break;
|
|
|
- case COLLIDER_CIRCLE:
|
|
|
- {
|
|
|
- if (CheckCollisionCircleRec(physicBodies[k]->transform.position, physicBodies[k]->collider.radius, physicBodies[i]->collider.bounds))
|
|
|
- {
|
|
|
- // Calculate direction vector between circles
|
|
|
- direction.x = physicBodies[k]->transform.position.x - physicBodies[i]->transform.position.x + physicBodies[i]->transform.scale.x/2;
|
|
|
- direction.y = physicBodies[k]->transform.position.y - physicBodies[i]->transform.position.y + physicBodies[i]->transform.scale.y/2;
|
|
|
-
|
|
|
- // Calculate closest point on rectangle to circle
|
|
|
- Vector2 closestPoint = { 0.0f, 0.0f };
|
|
|
- if (direction.x > 0.0f) closestPoint.x = physicBodies[i]->collider.bounds.x + physicBodies[i]->collider.bounds.width;
|
|
|
- else closestPoint.x = physicBodies[i]->collider.bounds.x;
|
|
|
-
|
|
|
- if (direction.y > 0.0f) closestPoint.y = physicBodies[i]->collider.bounds.y + physicBodies[i]->collider.bounds.height;
|
|
|
- else closestPoint.y = physicBodies[i]->collider.bounds.y;
|
|
|
-
|
|
|
- // Check if the closest point is inside the circle
|
|
|
- if (CheckCollisionPointCircle(closestPoint, physicBodies[k]->transform.position, physicBodies[k]->collider.radius))
|
|
|
- {
|
|
|
- // Recalculate direction based on closest point position
|
|
|
- direction.x = physicBodies[k]->transform.position.x - closestPoint.x;
|
|
|
- direction.y = physicBodies[k]->transform.position.y - closestPoint.y;
|
|
|
- float distance = Vector2Length(direction);
|
|
|
-
|
|
|
- // Calculate final contact normal
|
|
|
- contactNormal.x = direction.x/distance;
|
|
|
- contactNormal.y = -direction.y/distance;
|
|
|
-
|
|
|
- // Calculate penetration depth
|
|
|
- penetrationDepth = physicBodies[k]->collider.radius - distance;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- if (abs(direction.y) < abs(direction.x))
|
|
|
- {
|
|
|
- // Calculate final contact normal
|
|
|
- if (direction.y > 0.0f)
|
|
|
- {
|
|
|
- contactNormal = (Vector2){ 0.0f, -1.0f };
|
|
|
- penetrationDepth = fabs(physicBodies[i]->collider.bounds.y - physicBodies[k]->transform.position.y - physicBodies[k]->collider.radius);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- contactNormal = (Vector2){ 0.0f, 1.0f };
|
|
|
- penetrationDepth = fabs(physicBodies[i]->collider.bounds.y - physicBodies[k]->transform.position.y + physicBodies[k]->collider.radius);
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // Calculate final contact normal
|
|
|
- if (direction.x > 0.0f)
|
|
|
- {
|
|
|
- contactNormal = (Vector2){ 1.0f, 0.0f };
|
|
|
- penetrationDepth = fabs(physicBodies[k]->transform.position.x + physicBodies[k]->collider.radius - physicBodies[i]->collider.bounds.x);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- contactNormal = (Vector2){ -1.0f, 0.0f };
|
|
|
- penetrationDepth = fabs(physicBodies[i]->collider.bounds.x + physicBodies[i]->collider.bounds.width - physicBodies[k]->transform.position.x - physicBodies[k]->collider.radius);
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- } break;
|
|
|
- }
|
|
|
- } break;
|
|
|
- case COLLIDER_CIRCLE:
|
|
|
- {
|
|
|
- switch (physicBodies[k]->collider.type)
|
|
|
- {
|
|
|
- case COLLIDER_RECTANGLE:
|
|
|
- {
|
|
|
- if (CheckCollisionCircleRec(physicBodies[i]->transform.position, physicBodies[i]->collider.radius, physicBodies[k]->collider.bounds))
|
|
|
- {
|
|
|
- // Calculate direction vector between circles
|
|
|
- direction.x = physicBodies[k]->transform.position.x + physicBodies[i]->transform.scale.x/2 - physicBodies[i]->transform.position.x;
|
|
|
- direction.y = physicBodies[k]->transform.position.y + physicBodies[i]->transform.scale.y/2 - physicBodies[i]->transform.position.y;
|
|
|
-
|
|
|
- // Calculate closest point on rectangle to circle
|
|
|
- Vector2 closestPoint = { 0.0f, 0.0f };
|
|
|
- if (direction.x > 0.0f) closestPoint.x = physicBodies[k]->collider.bounds.x + physicBodies[k]->collider.bounds.width;
|
|
|
- else closestPoint.x = physicBodies[k]->collider.bounds.x;
|
|
|
-
|
|
|
- if (direction.y > 0.0f) closestPoint.y = physicBodies[k]->collider.bounds.y + physicBodies[k]->collider.bounds.height;
|
|
|
- else closestPoint.y = physicBodies[k]->collider.bounds.y;
|
|
|
-
|
|
|
- // Check if the closest point is inside the circle
|
|
|
- if (CheckCollisionPointCircle(closestPoint, physicBodies[i]->transform.position, physicBodies[i]->collider.radius))
|
|
|
- {
|
|
|
- // Recalculate direction based on closest point position
|
|
|
- direction.x = physicBodies[i]->transform.position.x - closestPoint.x;
|
|
|
- direction.y = physicBodies[i]->transform.position.y - closestPoint.y;
|
|
|
- float distance = Vector2Length(direction);
|
|
|
-
|
|
|
- // Calculate final contact normal
|
|
|
- contactNormal.x = direction.x/distance;
|
|
|
- contactNormal.y = -direction.y/distance;
|
|
|
-
|
|
|
- // Calculate penetration depth
|
|
|
- penetrationDepth = physicBodies[k]->collider.radius - distance;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- if (abs(direction.y) < abs(direction.x))
|
|
|
- {
|
|
|
- // Calculate final contact normal
|
|
|
- if (direction.y > 0.0f)
|
|
|
- {
|
|
|
- contactNormal = (Vector2){ 0.0f, -1.0f };
|
|
|
- penetrationDepth = fabs(physicBodies[k]->collider.bounds.y - physicBodies[i]->transform.position.y - physicBodies[i]->collider.radius);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- contactNormal = (Vector2){ 0.0f, 1.0f };
|
|
|
- penetrationDepth = fabs(physicBodies[k]->collider.bounds.y - physicBodies[i]->transform.position.y + physicBodies[i]->collider.radius);
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // Calculate final contact normal and penetration depth
|
|
|
- if (direction.x > 0.0f)
|
|
|
- {
|
|
|
- contactNormal = (Vector2){ 1.0f, 0.0f };
|
|
|
- penetrationDepth = fabs(physicBodies[i]->transform.position.x + physicBodies[i]->collider.radius - physicBodies[k]->collider.bounds.x);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- contactNormal = (Vector2){ -1.0f, 0.0f };
|
|
|
- penetrationDepth = fabs(physicBodies[k]->collider.bounds.x + physicBodies[k]->collider.bounds.width - physicBodies[i]->transform.position.x - physicBodies[i]->collider.radius);
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- } break;
|
|
|
- case COLLIDER_CIRCLE:
|
|
|
- {
|
|
|
- // Check if colliders are overlapped
|
|
|
- if (CheckCollisionCircles(physicBodies[i]->transform.position, physicBodies[i]->collider.radius, physicBodies[k]->transform.position, physicBodies[k]->collider.radius))
|
|
|
- {
|
|
|
- // Calculate direction vector between circles
|
|
|
- direction.x = physicBodies[k]->transform.position.x - physicBodies[i]->transform.position.x;
|
|
|
- direction.y = physicBodies[k]->transform.position.y - physicBodies[i]->transform.position.y;
|
|
|
-
|
|
|
- // Calculate distance between circles
|
|
|
- float distance = Vector2Length(direction);
|
|
|
-
|
|
|
- // Check if circles are not completely overlapped
|
|
|
- if (distance != 0.0f)
|
|
|
- {
|
|
|
- // Calculate contact normal direction (Y axis needs to be flipped)
|
|
|
- contactNormal.x = direction.x/distance;
|
|
|
- contactNormal.y = -direction.y/distance;
|
|
|
- }
|
|
|
- else contactNormal = (Vector2){ 1.0f, 0.0f }; // Choose random (but consistent) values
|
|
|
- }
|
|
|
- } break;
|
|
|
- default: break;
|
|
|
- }
|
|
|
- } break;
|
|
|
- default: break;
|
|
|
- }
|
|
|
-
|
|
|
- // Update rigidbody grounded state
|
|
|
- if (physicBodies[i]->rigidbody.enabled) physicBodies[i]->rigidbody.isGrounded = (contactNormal.y < 0.0f);
|
|
|
-
|
|
|
- // 2. Calculate collision impulse
|
|
|
- // -------------------------------------------------------------------------------------------------------------------------------------
|
|
|
-
|
|
|
- // Calculate relative velocity
|
|
|
- Vector2 relVelocity = { 0.0f, 0.0f };
|
|
|
- relVelocity.x = physicBodies[k]->rigidbody.velocity.x - physicBodies[i]->rigidbody.velocity.x;
|
|
|
- relVelocity.y = physicBodies[k]->rigidbody.velocity.y - physicBodies[i]->rigidbody.velocity.y;
|
|
|
-
|
|
|
- // Calculate relative velocity in terms of the normal direction
|
|
|
- float velAlongNormal = Vector2DotProduct(relVelocity, contactNormal);
|
|
|
-
|
|
|
- // Dot not resolve if velocities are separating
|
|
|
- if (velAlongNormal <= 0.0f)
|
|
|
- {
|
|
|
- // Calculate minimum bounciness value from both objects
|
|
|
- float e = fminf(physicBodies[i]->rigidbody.bounciness, physicBodies[k]->rigidbody.bounciness);
|
|
|
-
|
|
|
- // Calculate impulse scalar value
|
|
|
- float j = -(1.0f + e)*velAlongNormal;
|
|
|
- j /= 1.0f/physicBodies[i]->rigidbody.mass + 1.0f/physicBodies[k]->rigidbody.mass;
|
|
|
-
|
|
|
- // Calculate final impulse vector
|
|
|
- Vector2 impulse = { j*contactNormal.x, j*contactNormal.y };
|
|
|
-
|
|
|
- // Calculate collision mass ration
|
|
|
- float massSum = physicBodies[i]->rigidbody.mass + physicBodies[k]->rigidbody.mass;
|
|
|
- float ratio = 0.0f;
|
|
|
-
|
|
|
- // Apply impulse to current rigidbodies velocities if they are enabled
|
|
|
- if (physicBodies[i]->rigidbody.enabled)
|
|
|
- {
|
|
|
- // Calculate inverted mass ration
|
|
|
- ratio = physicBodies[i]->rigidbody.mass/massSum;
|
|
|
-
|
|
|
- // Apply impulse direction to velocity
|
|
|
- physicBodies[i]->rigidbody.velocity.x -= impulse.x*ratio*(1.0f+physicBodies[i]->rigidbody.bounciness);
|
|
|
- physicBodies[i]->rigidbody.velocity.y -= impulse.y*ratio*(1.0f+physicBodies[i]->rigidbody.bounciness);
|
|
|
- }
|
|
|
-
|
|
|
- if (physicBodies[k]->rigidbody.enabled)
|
|
|
- {
|
|
|
- // Calculate inverted mass ration
|
|
|
- ratio = physicBodies[k]->rigidbody.mass/massSum;
|
|
|
-
|
|
|
- // Apply impulse direction to velocity
|
|
|
- physicBodies[k]->rigidbody.velocity.x += impulse.x*ratio*(1.0f+physicBodies[i]->rigidbody.bounciness);
|
|
|
- physicBodies[k]->rigidbody.velocity.y += impulse.y*ratio*(1.0f+physicBodies[i]->rigidbody.bounciness);
|
|
|
- }
|
|
|
-
|
|
|
- // 3. Correct colliders overlaping (transform position)
|
|
|
- // ---------------------------------------------------------------------------------------------------------------------------------
|
|
|
-
|
|
|
- // Calculate transform position penetration correction
|
|
|
- Vector2 posCorrection;
|
|
|
- posCorrection.x = penetrationDepth/((1.0f/physicBodies[i]->rigidbody.mass) + (1.0f/physicBodies[k]->rigidbody.mass))*PHYSICS_ERRORPERCENT*contactNormal.x;
|
|
|
- posCorrection.y = penetrationDepth/((1.0f/physicBodies[i]->rigidbody.mass) + (1.0f/physicBodies[k]->rigidbody.mass))*PHYSICS_ERRORPERCENT*contactNormal.y;
|
|
|
-
|
|
|
- // Fix transform positions
|
|
|
- if (physicBodies[i]->rigidbody.enabled)
|
|
|
- {
|
|
|
- // Fix physic objects transform position
|
|
|
- physicBodies[i]->transform.position.x -= 1.0f/physicBodies[i]->rigidbody.mass*posCorrection.x;
|
|
|
- physicBodies[i]->transform.position.y += 1.0f/physicBodies[i]->rigidbody.mass*posCorrection.y;
|
|
|
-
|
|
|
- // Update collider bounds
|
|
|
- physicBodies[i]->collider.bounds = TransformToRectangle(physicBodies[i]->transform);
|
|
|
-
|
|
|
- if (physicBodies[k]->rigidbody.enabled)
|
|
|
- {
|
|
|
- // Fix physic objects transform position
|
|
|
- physicBodies[k]->transform.position.x += 1.0f/physicBodies[k]->rigidbody.mass*posCorrection.x;
|
|
|
- physicBodies[k]->transform.position.y -= 1.0f/physicBodies[k]->rigidbody.mass*posCorrection.y;
|
|
|
-
|
|
|
- // Update collider bounds
|
|
|
- physicBodies[k]->collider.bounds = TransformToRectangle(physicBodies[k]->transform);
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
+ 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;
|
|
|
+
|
|
|
+ #if !defined(PHYSAC_NO_THREADS)
|
|
|
+ pthread_join(physicsThreadId, NULL);
|
|
|
+ #endif
|
|
|
+}
|
|
|
+
|
|
|
+//----------------------------------------------------------------------------------
|
|
|
+// Module Internal Functions Definition
|
|
|
+//----------------------------------------------------------------------------------
|
|
|
+// Creates a random polygon shape with max vertex distance from polygon pivot
|
|
|
+static PolygonData CreateRandomPolygon(float radius, int sides)
|
|
|
+{
|
|
|
+ PolygonData data = { 0 };
|
|
|
+ data.vertexCount = sides;
|
|
|
+
|
|
|
+ float orient = GetRandomNumber(0, 360);
|
|
|
+ data.transform = Mat2Radians(orient*PHYSAC_DEG2RAD);
|
|
|
+
|
|
|
+ // Calculate polygon vertices positions
|
|
|
+ for (int i = 0; i < data.vertexCount; i++)
|
|
|
+ {
|
|
|
+ data.vertices[i].x = cosf(360/sides*i*PHYSAC_DEG2RAD)*radius;
|
|
|
+ data.vertices[i].y = sinf(360/sides*i*PHYSAC_DEG2RAD)*radius;
|
|
|
+ }
|
|
|
+
|
|
|
+ // Calculate polygon faces normals
|
|
|
+ for (int i = 0; i < data.vertexCount; i++)
|
|
|
+ {
|
|
|
+ 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;
|
|
|
+}
|
|
|
+
|
|
|
+// Creates a rectangle polygon shape based on a min and max positions
|
|
|
+static PolygonData CreateRectanglePolygon(Vector2 pos, Vector2 size)
|
|
|
+{
|
|
|
+ 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++)
|
|
|
+ {
|
|
|
+ int nextIndex = (((i + 1) < data.vertexCount) ? (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;
|
|
|
+}
|
|
|
+
|
|
|
+// Physics loop thread function
|
|
|
+static void *PhysicsLoop(void *arg)
|
|
|
+{
|
|
|
+ #if defined(PHYSAC_DEBUG)
|
|
|
+ printf("[PHYSAC] physics thread created with successfully\n");
|
|
|
+ #endif
|
|
|
+
|
|
|
+ // Initialize physics loop thread values
|
|
|
+ physicsThreadEnabled = true;
|
|
|
+ accumulator = 0;
|
|
|
+
|
|
|
+ // 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;
|
|
|
+}
|
|
|
+
|
|
|
+// Physics steps calculations (dynamics, collisions and position corrections)
|
|
|
+static void PhysicsStep(void)
|
|
|
+{
|
|
|
+ stepsCount++;
|
|
|
+
|
|
|
+ // Clear previous generated collisions information
|
|
|
+ for (int i = physicsManifoldsCount - 1; i >= 0; i--)
|
|
|
+ {
|
|
|
+ PhysicsManifold manifold = contacts[i];
|
|
|
+ if (manifold != NULL) DestroyPhysicsManifold(manifold);
|
|
|
+ }
|
|
|
+
|
|
|
+ // Generate new collision information
|
|
|
+ for (int i = 0; i < physicsBodiesCount; i++)
|
|
|
+ {
|
|
|
+ PhysicsBody bodyA = bodies[i];
|
|
|
+
|
|
|
+ if (bodyA != NULL)
|
|
|
+ {
|
|
|
+ for (int j = i + 1; j < physicsBodiesCount; j++)
|
|
|
+ {
|
|
|
+ PhysicsBody bodyB = bodies[j];
|
|
|
+
|
|
|
+ if (bodyB != NULL)
|
|
|
+ {
|
|
|
+ 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)
|
|
|
+ {
|
|
|
+ // 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;
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+// 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
|
|
|
+#endif // PHYSAC_IMPLEMENTATION
|
Ray
