diff options
Diffstat (limited to 'libs/raylib/src/physac.h')
| -rw-r--r-- | libs/raylib/src/physac.h | 1988 |
1 files changed, 0 insertions, 1988 deletions
diff --git a/libs/raylib/src/physac.h b/libs/raylib/src/physac.h deleted file mode 100644 index 4b419bb..0000000 --- a/libs/raylib/src/physac.h +++ /dev/null @@ -1,1988 +0,0 @@ -/********************************************************************************************** -* -* Physac v1.1 - 2D Physics library for videogames -* -* 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: -* -* #define PHYSAC_IMPLEMENTATION -* Generates the implementation of the library into the included file. -* If not defined, the library is in header only mode and can be included in other headers -* or source files without problems. But only ONE file should hold the implementation. -* -* #define PHYSAC_STATIC (defined by default) -* The generated implementation will stay private inside implementation file and all -* internal symbols and functions will only be visible inside that file. -* -* #define PHYSAC_DEBUG -* Show debug traces log messages about physic bodies creation/destruction, physic system errors, -* some calculations results and NULL reference exceptions -* -* #define PHYSAC_DEFINE_VECTOR2_TYPE -* Forces library to define struct Vector2 data type (float x; float y) -* -* #define PHYSAC_AVOID_TIMMING_SYSTEM -* Disables internal timming system, used by UpdatePhysics() to launch timmed physic steps, -* it allows just running UpdatePhysics() automatically on a separate thread at a desired time step. -* In case physics steps update needs to be controlled by user with a custom timming mechanism, -* just define this flag and the internal timming mechanism will be avoided, in that case, -* timming libraries are neither required by the module. -* -* #define PHYSAC_MALLOC() -* #define PHYSAC_CALLOC() -* #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. -* -* COMPILATION: -* -* Use the following code to compile with GCC: -* gcc -o $(NAME_PART).exe $(FILE_NAME) -s -static -lraylib -lopengl32 -lgdi32 -lwinmm -std=c99 -* -* VERSIONS HISTORY: -* 1.1 (20-Jan-2021) @raysan5: Library general revision -* Removed threading system (up to the user) -* Support MSVC C++ compilation using CLITERAL() -* Review DEBUG mechanism for TRACELOG() and all TRACELOG() messages -* Review internal variables/functions naming for consistency -* Allow option to avoid internal timming system, to allow app manage the steps -* 1.0 (12-Jun-2017) First release of the library -* -* -* LICENSE: zlib/libpng -* -* Copyright (c) 2016-2021 Victor Fisac (@victorfisac) and Ramon Santamaria (@raysan5) -* -* This software is provided "as-is", without any express or implied warranty. In no event -* will the authors be held liable for any damages arising from the use of this software. -* -* Permission is granted to anyone to use this software for any purpose, including commercial -* applications, and to alter it and redistribute it freely, subject to the following restrictions: -* -* 1. The origin of this software must not be misrepresented; you must not claim that you -* wrote the original software. If you use this software in a product, an acknowledgment -* in the product documentation would be appreciated but is not required. -* -* 2. Altered source versions must be plainly marked as such, and must not be misrepresented -* as being the original software. -* -* 3. This notice may not be removed or altered from any source distribution. -* -**********************************************************************************************/ - -#if !defined(PHYSAC_H) -#define PHYSAC_H - -#if defined(PHYSAC_STATIC) - #define PHYSACDEF static // Functions just visible to module including this file -#else - #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 - #endif -#endif - -// Allow custom memory allocators -#ifndef PHYSAC_MALLOC - #define PHYSAC_MALLOC(size) malloc(size) -#endif -#ifndef PHYSAC_CALLOC - #define PHYSAC_CALLOC(size, n) calloc(size, n) -#endif -#ifndef PHYSAC_FREE - #define PHYSAC_FREE(ptr) free(ptr) -#endif - -//---------------------------------------------------------------------------------- -// Defines and Macros -//---------------------------------------------------------------------------------- -#define PHYSAC_MAX_BODIES 64 // Maximum number of physic bodies supported -#define PHYSAC_MAX_MANIFOLDS 4096 // Maximum number of physic bodies interactions (64x64) -#define PHYSAC_MAX_VERTICES 24 // Maximum number of vertex for polygons shapes -#define PHYSAC_DEFAULT_CIRCLE_VERTICES 24 // Default number of vertices for circle shapes - -#define PHYSAC_COLLISION_ITERATIONS 100 -#define PHYSAC_PENETRATION_ALLOWANCE 0.05f -#define PHYSAC_PENETRATION_CORRECTION 0.4f - -#define PHYSAC_PI 3.14159265358979323846f -#define PHYSAC_DEG2RAD (PHYSAC_PI/180.0f) - -//---------------------------------------------------------------------------------- -// Data Types Structure Definition -//---------------------------------------------------------------------------------- -#if defined(__STDC__) && __STDC_VERSION__ >= 199901L - #include <stdbool.h> -#endif - -typedef enum PhysicsShapeType { PHYSICS_CIRCLE = 0, PHYSICS_POLYGON } PhysicsShapeType; - -// Previously defined to be used in PhysicsShape struct as circular dependencies -typedef struct PhysicsBodyData *PhysicsBody; - -#if defined(PHYSAC_DEFINE_VECTOR2_TYPE) -// Vector2 type -typedef struct Vector2 { - float x; - float y; -} Vector2; -#endif - -// Matrix2x2 type (used for polygon shape rotation matrix) -typedef struct Matrix2x2 { - float m00; - float m01; - float m10; - float m11; -} Matrix2x2; - -typedef struct PhysicsVertexData { - unsigned int vertexCount; // Vertex count (positions and normals) - Vector2 positions[PHYSAC_MAX_VERTICES]; // Vertex positions vectors - Vector2 normals[PHYSAC_MAX_VERTICES]; // Vertex normals vectors -} PhysicsVertexData; - -typedef struct PhysicsShape { - PhysicsShapeType type; // Shape type (circle or polygon) - PhysicsBody body; // Shape physics body data pointer - PhysicsVertexData vertexData; // Shape vertices data (used for polygon shapes) - float radius; // Shape radius (used for circle shapes) - Matrix2x2 transform; // Vertices transform matrix 2x2 -} PhysicsShape; - -typedef struct PhysicsBodyData { - unsigned int id; // 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, transform) -} PhysicsBodyData; - -typedef struct PhysicsManifoldData { - unsigned int id; // 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 - -//---------------------------------------------------------------------------------- -// Module Functions Declaration -//---------------------------------------------------------------------------------- -// Physics system management -PHYSACDEF void InitPhysics(void); // Initializes physics system -PHYSACDEF void UpdatePhysics(void); // Update physics system -PHYSACDEF void ResetPhysics(void); // Reset physics system (global variables) -PHYSACDEF void ClosePhysics(void); // Close physics system and unload used memory -PHYSACDEF void SetPhysicsTimeStep(double delta); // Sets physics fixed time step in milliseconds. 1.666666 by default -PHYSACDEF void SetPhysicsGravity(float x, float y); // Sets physics global gravity force - -// Physic body creation/destroy -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 DestroyPhysicsBody(PhysicsBody body); // Destroy a physics body - -// Physic body forces -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 void SetPhysicsBodyRotation(PhysicsBody body, float radians); // Sets physics body shape transform based on radians parameter - -// Query physics info -PHYSACDEF PhysicsBody GetPhysicsBody(int index); // Returns a physics body of the bodies pool at a specific index -PHYSACDEF int GetPhysicsBodiesCount(void); // Returns the current amount of created physics bodies -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) - -#if defined(__cplusplus) -} -#endif - -#endif // PHYSAC_H - -/*********************************************************************************** -* -* PHYSAC IMPLEMENTATION -* -************************************************************************************/ - -#if defined(PHYSAC_IMPLEMENTATION) - -// Support TRACELOG macros -#if defined(PHYSAC_DEBUG) - #include <stdio.h> // Required for: printf() - #define TRACELOG(...) printf(__VA_ARGS__) -#else - #define TRACELOG(...) (void)0; -#endif - -#include <stdlib.h> // Required for: malloc(), calloc(), free() -#include <math.h> // Required for: cosf(), sinf(), fabs(), sqrtf() - -#if !defined(PHYSAC_AVOID_TIMMING_SYSTEM) - // Time management functionality - #include <time.h> // Required for: time(), clock_gettime() - #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); - #endif - #if defined(__linux__) || defined(__FreeBSD__) - #if _POSIX_C_SOURCE < 199309L - #undef _POSIX_C_SOURCE - #define _POSIX_C_SOURCE 199309L // Required for CLOCK_MONOTONIC if compiled with c99 without gnu ext. - #endif - #include <sys/time.h> // Required for: timespec - #endif - #if defined(__APPLE__) // macOS also defines __MACH__ - #include <mach/mach_time.h> // Required for: mach_absolute_time() - #endif -#endif - -// NOTE: MSVC C++ compiler does not support compound literals (C99 feature) -// Plain structures in C++ (without constructors) can be initialized from { } initializers. -#if defined(__cplusplus) - #define CLITERAL(type) type -#else - #define CLITERAL(type) (type) -#endif - -//---------------------------------------------------------------------------------- -// Defines and Macros -//---------------------------------------------------------------------------------- -#define PHYSAC_MIN(a,b) (((a)<(b))?(a):(b)) -#define PHYSAC_MAX(a,b) (((a)>(b))?(a):(b)) -#define PHYSAC_FLT_MAX 3.402823466e+38f -#define PHYSAC_EPSILON 0.000001f -#define PHYSAC_K 1.0f/3.0f -#define PHYSAC_VECTOR_ZERO CLITERAL(Vector2){ 0.0f, 0.0f } - -//---------------------------------------------------------------------------------- -// Global Variables Definition -//---------------------------------------------------------------------------------- -static double deltaTime = 1.0/60.0/10.0 * 1000; // Delta time in milliseconds used for physics steps - -#if !defined(PHYSAC_AVOID_TIMMING_SYSTEM) -// Time measure variables -static double baseClockTicks = 0.0; // Offset clock ticks for MONOTONIC clock -static unsigned long long int frequency = 0; // Hi-res clock frequency -static double startTime = 0.0; // Start time in milliseconds -static double currentTime = 0.0; // Current time in milliseconds -#endif - -// Physics system configuration -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 - -static Vector2 gravityForce = { 0.0f, 9.81f }; // Physics world gravity force - -// Utilities variables -static unsigned int usedMemory = 0; // Total allocated dynamic memory - -//---------------------------------------------------------------------------------- -// Module Internal Functions Declaration -//---------------------------------------------------------------------------------- -#if !defined(PHYSAC_AVOID_TIMMING_SYSTEM) -// Timming measure functions -static void InitTimer(void); // Initializes hi-resolution MONOTONIC timer -static unsigned long long int GetClockTicks(void); // Get hi-res MONOTONIC time measure in mseconds -static double GetCurrentTime(void); // Get current time measure in milliseconds -#endif - -static void UpdatePhysicsStep(void); // Update physics step (dynamics, collisions and position corrections) - -static int FindAvailableBodyIndex(); // Finds a valid index for a new physics body initialization -static int FindAvailableManifoldIndex(); // Finds a valid index for a new manifold initialization -static PhysicsVertexData CreateDefaultPolygon(float radius, int sides); // Creates a random polygon shape with max vertex distance from polygon pivot -static PhysicsVertexData CreateRectanglePolygon(Vector2 pos, Vector2 size); // Creates a rectangle polygon shape based on a min and max positions - -static void InitializePhysicsManifolds(PhysicsManifold manifold); // Initializes physics manifolds to solve collisions -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 IntegratePhysicsVelocity(PhysicsBody body); // Integrates physics velocity into position and forces -static void IntegratePhysicsImpulses(PhysicsManifold manifold); // Integrates physics collisions impulses to solve collisions -static void CorrectPhysicsPositions(PhysicsManifold manifold); // Corrects physics bodies positions based on manifolds collision information -static void FindIncidentFace(Vector2 *v0, Vector2 *v1, PhysicsShape ref, PhysicsShape inc, int index); // Finds two polygon shapes incident face -static float FindAxisLeastPenetration(int *faceIndex, PhysicsShape shapeA, PhysicsShape shapeB); // Finds polygon shapes axis least penetration - -// Math required functions -static Vector2 MathVector2Product(Vector2 vector, float value); // Returns the product of a vector and a value -static float MathVector2CrossProduct(Vector2 v1, Vector2 v2); // Returns the cross product of two vectors -static float MathVector2SqrLen(Vector2 vector); // Returns the len square root of a vector -static float MathVector2DotProduct(Vector2 v1, Vector2 v2); // Returns the dot product of two vectors -static inline float MathVector2SqrDistance(Vector2 v1, Vector2 v2); // Returns the square root of distance between two vectors -static void MathVector2Normalize(Vector2 *vector); // Returns the normalized values of a vector -static Vector2 MathVector2Add(Vector2 v1, Vector2 v2); // Returns the sum of two given vectors -static Vector2 MathVector2Subtract(Vector2 v1, Vector2 v2); // Returns the subtract of two given vectors -static Matrix2x2 MathMatFromRadians(float radians); // Returns a matrix 2x2 from a given radians value -static inline Matrix2x2 MathMatTranspose(Matrix2x2 matrix); // Returns the transpose of a given matrix 2x2 -static inline Vector2 MathMatVector2Product(Matrix2x2 matrix, Vector2 vector); // Returns product between matrix 2x2 and vector -static int MathVector2Clip(Vector2 normal, Vector2 *faceA, Vector2 *faceB, float clip); // Returns clipping value based on a normal and two faces -static Vector2 MathTriangleBarycenter(Vector2 v1, Vector2 v2, Vector2 v3); // Returns the barycenter of a triangle given by 3 points - -//---------------------------------------------------------------------------------- -// Module Functions Definition -//---------------------------------------------------------------------------------- - -// Initializes physics values, pointers and creates physics loop thread -PHYSACDEF void InitPhysics(void) -{ -#if !defined(PHYSAC_AVOID_TIMMING_SYSTEM) - // Initialize high resolution timer - InitTimer(); -#endif - - TRACELOG("[PHYSAC] Physics module initialized successfully\n"); -} - -// Sets physics global gravity force -PHYSACDEF void SetPhysicsGravity(float x, float y) -{ - gravityForce.x = x; - gravityForce.y = y; -} - -// Creates a new circle physics body with generic parameters -PHYSACDEF PhysicsBody CreatePhysicsBodyCircle(Vector2 pos, float radius, float density) -{ - PhysicsBody body = CreatePhysicsBodyPolygon(pos, radius, PHYSAC_DEFAULT_CIRCLE_VERTICES, density); - return body; -} - -// Creates a new rectangle physics body with generic parameters -PHYSACDEF PhysicsBody CreatePhysicsBodyRectangle(Vector2 pos, float width, float height, float density) -{ - // NOTE: Make sure body data is initialized to 0 - PhysicsBody body = (PhysicsBody)PHYSAC_CALLOC(sizeof(PhysicsBodyData), 1); - usedMemory += sizeof(PhysicsBodyData); - - int id = FindAvailableBodyIndex(); - if (id != -1) - { - // Initialize new body with generic values - body->id = id; - body->enabled = true; - body->position = pos; - body->shape.type = PHYSICS_POLYGON; - body->shape.body = body; - body->shape.transform = MathMatFromRadians(0.0f); - body->shape.vertexData = CreateRectanglePolygon(pos, CLITERAL(Vector2){ width, height }); - - // Calculate centroid and moment of inertia - Vector2 center = { 0.0f, 0.0f }; - float area = 0.0f; - float inertia = 0.0f; - - for (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++) - { - // Triangle vertices, third vertex implied as (0, 0) - Vector2 p1 = body->shape.vertexData.positions[i]; - unsigned int nextIndex = (((i + 1) < body->shape.vertexData.vertexCount) ? (i + 1) : 0); - Vector2 p2 = body->shape.vertexData.positions[nextIndex]; - - float D = MathVector2CrossProduct(p1, p2); - float triangleArea = D/2; - - area += triangleArea; - - // Use area to weight the centroid average, not just vertex position - center.x += triangleArea*PHYSAC_K*(p1.x + p2.x); - center.y += triangleArea*PHYSAC_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*PHYSAC_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 (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++) - { - body->shape.vertexData.positions[i].x -= center.x; - body->shape.vertexData.positions[i].y -= center.y; - } - - body->mass = density*area; - body->inverseMass = ((body->mass != 0.0f) ? 1.0f/body->mass : 0.0f); - body->inertia = density*inertia; - body->inverseInertia = ((body->inertia != 0.0f) ? 1.0f/body->inertia : 0.0f); - body->staticFriction = 0.4f; - body->dynamicFriction = 0.2f; - body->restitution = 0.0f; - body->useGravity = true; - body->isGrounded = false; - body->freezeOrient = false; - - // Add new body to bodies pointers array and update bodies count - bodies[physicsBodiesCount] = body; - physicsBodiesCount++; - - TRACELOG("[PHYSAC] Physic body created successfully (id: %i)\n", body->id); - } - else TRACELOG("[PHYSAC] Physic body could not be created, PHYSAC_MAX_BODIES reached\n"); - - return body; -} - -// Creates a new polygon physics body with generic parameters -PHYSACDEF PhysicsBody CreatePhysicsBodyPolygon(Vector2 pos, float radius, int sides, float density) -{ - PhysicsBody body = (PhysicsBody)PHYSAC_MALLOC(sizeof(PhysicsBodyData)); - usedMemory += sizeof(PhysicsBodyData); - - int id = FindAvailableBodyIndex(); - if (id != -1) - { - // Initialize new body with generic values - body->id = id; - body->enabled = true; - body->position = pos; - body->velocity = PHYSAC_VECTOR_ZERO; - body->force = PHYSAC_VECTOR_ZERO; - body->angularVelocity = 0.0f; - body->torque = 0.0f; - body->orient = 0.0f; - body->shape.type = PHYSICS_POLYGON; - body->shape.body = body; - body->shape.transform = MathMatFromRadians(0.0f); - body->shape.vertexData = CreateDefaultPolygon(radius, sides); - - // Calculate centroid and moment of inertia - Vector2 center = { 0.0f, 0.0f }; - float area = 0.0f; - float inertia = 0.0f; - - for (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++) - { - // Triangle vertices, third vertex implied as (0, 0) - Vector2 position1 = body->shape.vertexData.positions[i]; - unsigned int nextIndex = (((i + 1) < body->shape.vertexData.vertexCount) ? (i + 1) : 0); - Vector2 position2 = body->shape.vertexData.positions[nextIndex]; - - float cross = MathVector2CrossProduct(position1, position2); - float triangleArea = cross/2; - - area += triangleArea; - - // Use area to weight the centroid average, not just vertex position - center.x += triangleArea*PHYSAC_K*(position1.x + position2.x); - center.y += triangleArea*PHYSAC_K*(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*PHYSAC_K*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 (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++) - { - body->shape.vertexData.positions[i].x -= center.x; - body->shape.vertexData.positions[i].y -= center.y; - } - - body->mass = density*area; - body->inverseMass = ((body->mass != 0.0f) ? 1.0f/body->mass : 0.0f); - body->inertia = density*inertia; - body->inverseInertia = ((body->inertia != 0.0f) ? 1.0f/body->inertia : 0.0f); - body->staticFriction = 0.4f; - body->dynamicFriction = 0.2f; - body->restitution = 0.0f; - body->useGravity = true; - body->isGrounded = false; - body->freezeOrient = false; - - // Add new body to bodies pointers array and update bodies count - bodies[physicsBodiesCount] = body; - physicsBodiesCount++; - - TRACELOG("[PHYSAC] Physic body created successfully (id: %i)\n", body->id); - } - else TRACELOG("[PHYSAC] Physics body could not be created, PHYSAC_MAX_BODIES reached\n"); - - return body; -} - -// Adds a force to a physics body -PHYSACDEF void PhysicsAddForce(PhysicsBody body, Vector2 force) -{ - if (body != NULL) body->force = MathVector2Add(body->force, force); -} - -// Adds an angular force to a physics body -PHYSACDEF void PhysicsAddTorque(PhysicsBody body, float amount) -{ - if (body != NULL) body->torque += amount; -} - -// Shatters a polygon shape physics body to little physics bodies with explosion force -PHYSACDEF void PhysicsShatter(PhysicsBody body, Vector2 position, float force) -{ - if (body != NULL) - { - if (body->shape.type == PHYSICS_POLYGON) - { - PhysicsVertexData vertexData = body->shape.vertexData; - bool collision = false; - - for (unsigned int i = 0; i < vertexData.vertexCount; i++) - { - Vector2 positionA = body->position; - Vector2 positionB = MathMatVector2Product(body->shape.transform, MathVector2Add(body->position, vertexData.positions[i])); - unsigned int nextIndex = (((i + 1) < vertexData.vertexCount) ? (i + 1) : 0); - Vector2 positionC = MathMatVector2Product(body->shape.transform, MathVector2Add(body->position, vertexData.positions[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.0f) && (beta > 0.0f) & (gamma > 0.0f)) - { - collision = true; - break; - } - } - - if (collision) - { - int count = vertexData.vertexCount; - Vector2 bodyPos = body->position; - Vector2 *vertices = (Vector2 *)PHYSAC_MALLOC(sizeof(Vector2)*count); - Matrix2x2 trans = body->shape.transform; - for (int i = 0; i < count; i++) vertices[i] = vertexData.positions[i]; - - // Destroy shattered physics body - DestroyPhysicsBody(body); - - for (int i = 0; i < count; i++) - { - int nextIndex = (((i + 1) < count) ? (i + 1) : 0); - Vector2 center = MathTriangleBarycenter(vertices[i], vertices[nextIndex], PHYSAC_VECTOR_ZERO); - center = MathVector2Add(bodyPos, center); - Vector2 offset = MathVector2Subtract(center, bodyPos); - - PhysicsBody body = CreatePhysicsBodyPolygon(center, 10, 3, 10); // Create polygon physics body with relevant values - - PhysicsVertexData vertexData = { 0 }; - vertexData.vertexCount = 3; - - vertexData.positions[0] = MathVector2Subtract(vertices[i], offset); - vertexData.positions[1] = MathVector2Subtract(vertices[nextIndex], offset); - vertexData.positions[2] = MathVector2Subtract(position, center); - - // Separate vertices to avoid unnecessary physics collisions - vertexData.positions[0].x *= 0.95f; - vertexData.positions[0].y *= 0.95f; - vertexData.positions[1].x *= 0.95f; - vertexData.positions[1].y *= 0.95f; - vertexData.positions[2].x *= 0.95f; - vertexData.positions[2].y *= 0.95f; - - // Calculate polygon faces normals - for (unsigned int j = 0; j < vertexData.vertexCount; j++) - { - unsigned int nextVertex = (((j + 1) < vertexData.vertexCount) ? (j + 1) : 0); - Vector2 face = MathVector2Subtract(vertexData.positions[nextVertex], vertexData.positions[j]); - - vertexData.normals[j] = CLITERAL(Vector2){ face.y, -face.x }; - MathVector2Normalize(&vertexData.normals[j]); - } - - // Apply computed vertex data to new physics body shape - body->shape.vertexData = vertexData; - body->shape.transform = trans; - - // Calculate centroid and moment of inertia - center = PHYSAC_VECTOR_ZERO; - float area = 0.0f; - float inertia = 0.0f; - - for (unsigned int j = 0; j < body->shape.vertexData.vertexCount; j++) - { - // Triangle vertices, third vertex implied as (0, 0) - Vector2 p1 = body->shape.vertexData.positions[j]; - unsigned int nextVertex = (((j + 1) < body->shape.vertexData.vertexCount) ? (j + 1) : 0); - Vector2 p2 = body->shape.vertexData.positions[nextVertex]; - - float D = MathVector2CrossProduct(p1, p2); - float triangleArea = D/2; - - area += triangleArea; - - // Use area to weight the centroid average, not just vertex position - center.x += triangleArea*PHYSAC_K*(p1.x + p2.x); - center.y += triangleArea*PHYSAC_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*PHYSAC_K*D)*(intx2 + inty2); - } - - center.x *= 1.0f/area; - center.y *= 1.0f/area; - - body->mass = area; - body->inverseMass = ((body->mass != 0.0f) ? 1.0f/body->mass : 0.0f); - body->inertia = inertia; - body->inverseInertia = ((body->inertia != 0.0f) ? 1.0f/body->inertia : 0.0f); - - // Calculate explosion force direction - Vector2 pointA = body->position; - Vector2 pointB = MathVector2Subtract(vertexData.positions[1], vertexData.positions[0]); - pointB.x /= 2.0f; - pointB.y /= 2.0f; - Vector2 forceDirection = MathVector2Subtract(MathVector2Add(pointA, MathVector2Add(vertexData.positions[0], pointB)), body->position); - MathVector2Normalize(&forceDirection); - forceDirection.x *= force; - forceDirection.y *= force; - - // Apply force to new physics body - PhysicsAddForce(body, forceDirection); - } - - PHYSAC_FREE(vertices); - } - } - } - else TRACELOG("[PHYSAC] WARNING: PhysicsShatter: NULL physic body\n"); -} - -// Returns the current amount of created physics bodies -PHYSACDEF int GetPhysicsBodiesCount(void) -{ - return physicsBodiesCount; -} - -// Returns a physics body of the bodies pool at a specific index -PHYSACDEF PhysicsBody GetPhysicsBody(int index) -{ - PhysicsBody body = NULL; - - if (index < (int)physicsBodiesCount) - { - body = bodies[index]; - - if (body == NULL) TRACELOG("[PHYSAC] WARNING: GetPhysicsBody: NULL physic body\n"); - } - else TRACELOG("[PHYSAC] WARNING: Physic body index is out of bounds\n"); - - return body; -} - -// Returns the physics body shape type (PHYSICS_CIRCLE or PHYSICS_POLYGON) -PHYSACDEF int GetPhysicsShapeType(int index) -{ - int result = -1; - - if (index < (int)physicsBodiesCount) - { - PhysicsBody body = bodies[index]; - - if (body != NULL) result = body->shape.type; - else TRACELOG("[PHYSAC] WARNING: GetPhysicsShapeType: NULL physic body\n"); - } - else TRACELOG("[PHYSAC] WARNING: Physic body index is out of bounds\n"); - - return result; -} - -// Returns the amount of vertices of a physics body shape -PHYSACDEF int GetPhysicsShapeVerticesCount(int index) -{ - int result = 0; - - if (index < (int)physicsBodiesCount) - { - PhysicsBody body = bodies[index]; - - if (body != NULL) - { - switch (body->shape.type) - { - case PHYSICS_CIRCLE: result = PHYSAC_DEFAULT_CIRCLE_VERTICES; break; - case PHYSICS_POLYGON: result = body->shape.vertexData.vertexCount; break; - default: break; - } - } - else TRACELOG("[PHYSAC] WARNING: GetPhysicsShapeVerticesCount: NULL physic body\n"); - } - else TRACELOG("[PHYSAC] WARNING: Physic body index is out of bounds\n"); - - return result; -} - -// Returns transformed position of a body shape (body position + vertex transformed position) -PHYSACDEF Vector2 GetPhysicsShapeVertex(PhysicsBody body, int vertex) -{ - Vector2 position = { 0.0f, 0.0f }; - - if (body != NULL) - { - switch (body->shape.type) - { - case PHYSICS_CIRCLE: - { - position.x = body->position.x + cosf(360.0f/PHYSAC_DEFAULT_CIRCLE_VERTICES*vertex*PHYSAC_DEG2RAD)*body->shape.radius; - position.y = body->position.y + sinf(360.0f/PHYSAC_DEFAULT_CIRCLE_VERTICES*vertex*PHYSAC_DEG2RAD)*body->shape.radius; - } break; - case PHYSICS_POLYGON: - { - PhysicsVertexData vertexData = body->shape.vertexData; - position = MathVector2Add(body->position, MathMatVector2Product(body->shape.transform, vertexData.positions[vertex])); - } break; - default: break; - } - } - else TRACELOG("[PHYSAC] WARNING: GetPhysicsShapeVertex: NULL physic body\n"); - - return position; -} - -// Sets physics body shape transform based on radians parameter -PHYSACDEF void SetPhysicsBodyRotation(PhysicsBody body, float radians) -{ - if (body != NULL) - { - body->orient = radians; - - if (body->shape.type == PHYSICS_POLYGON) body->shape.transform = MathMatFromRadians(radians); - } -} - -// Unitializes and destroys a physics body -PHYSACDEF void DestroyPhysicsBody(PhysicsBody body) -{ - if (body != NULL) - { - int id = body->id; - int index = -1; - - for (unsigned int i = 0; i < physicsBodiesCount; i++) - { - if (bodies[i]->id == id) - { - index = i; - break; - } - } - - if (index == -1) - { - TRACELOG("[PHYSAC] WARNING: Requested body (id: %i) can not be found\n", id); - return; // Prevent access to index -1 - } - - // Free body allocated memory - PHYSAC_FREE(body); - usedMemory -= sizeof(PhysicsBodyData); - bodies[index] = NULL; - - // Reorder physics bodies pointers array and its catched index - for (unsigned int i = index; i < physicsBodiesCount; i++) - { - if ((i + 1) < physicsBodiesCount) bodies[i] = bodies[i + 1]; - } - - // Update physics bodies count - physicsBodiesCount--; - - TRACELOG("[PHYSAC] Physic body destroyed successfully (id: %i)\n", id); - } - else TRACELOG("[PHYSAC] WARNING: DestroyPhysicsBody: NULL physic body\n"); -} - -// Destroys created physics bodies and manifolds and resets global values -PHYSACDEF void ResetPhysics(void) -{ - if (physicsBodiesCount > 0) - { - // Unitialize physics bodies dynamic memory allocations - for (unsigned int i = physicsBodiesCount - 1; i >= 0; i--) - { - PhysicsBody body = bodies[i]; - - if (body != NULL) - { - PHYSAC_FREE(body); - bodies[i] = NULL; - usedMemory -= sizeof(PhysicsBodyData); - } - } - - physicsBodiesCount = 0; - } - - if (physicsManifoldsCount > 0) - { - // Unitialize physics manifolds dynamic memory allocations - for (unsigned int i = physicsManifoldsCount - 1; i >= 0; i--) - { - PhysicsManifold manifold = contacts[i]; - - if (manifold != NULL) - { - PHYSAC_FREE(manifold); - contacts[i] = NULL; - usedMemory -= sizeof(PhysicsManifoldData); - } - } - - physicsManifoldsCount = 0; - } - - TRACELOG("[PHYSAC] Physics module reseted successfully\n"); -} - -// Unitializes physics pointers and exits physics loop thread -PHYSACDEF void ClosePhysics(void) -{ - // Unitialize physics manifolds dynamic memory allocations - if (physicsManifoldsCount > 0) - { - for (unsigned int i = physicsManifoldsCount - 1; i >= 0; i--) - DestroyPhysicsManifold(contacts[i]); - } - - // Unitialize physics bodies dynamic memory allocations - if (physicsBodiesCount > 0) - { - for (unsigned int i = physicsBodiesCount - 1; i >= 0; i--) - DestroyPhysicsBody(bodies[i]); - } - - // Trace log info - if ((physicsBodiesCount > 0) || (usedMemory != 0)) - { - TRACELOG("[PHYSAC] WARNING: Physics module closed with unallocated bodies (BODIES: %i, MEMORY: %i bytes)\n", physicsBodiesCount, usedMemory); - } - else if ((physicsManifoldsCount > 0) || (usedMemory != 0)) - { - TRACELOG("[PHYSAC] WARNING: Pysics module closed with unallocated manifolds (MANIFOLDS: %i, MEMORY: %i bytes)\n", physicsManifoldsCount, usedMemory); - } - else TRACELOG("[PHYSAC] Physics module closed successfully\n"); -} - -//---------------------------------------------------------------------------------- -// Module Internal Functions Definition -//---------------------------------------------------------------------------------- -// Finds a valid index for a new physics body initialization -static int FindAvailableBodyIndex() -{ - int index = -1; - for (int i = 0; i < PHYSAC_MAX_BODIES; i++) - { - int currentId = i; - - // Check if current id already exist in other physics body - for (unsigned 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 == (int)i) - { - index = (int)i; - break; - } - } - - return index; -} - -// Creates a default polygon shape with max vertex distance from polygon pivot -static PhysicsVertexData CreateDefaultPolygon(float radius, int sides) -{ - PhysicsVertexData data = { 0 }; - data.vertexCount = sides; - - // Calculate polygon vertices positions - for (unsigned int i = 0; i < data.vertexCount; i++) - { - data.positions[i].x = (float)cosf(360.0f/sides*i*PHYSAC_DEG2RAD)*radius; - data.positions[i].y = (float)sinf(360.0f/sides*i*PHYSAC_DEG2RAD)*radius; - } - - // Calculate polygon faces normals - for (int i = 0; i < (int)data.vertexCount; i++) - { - int nextIndex = (((i + 1) < sides) ? (i + 1) : 0); - Vector2 face = MathVector2Subtract(data.positions[nextIndex], data.positions[i]); - - data.normals[i] = CLITERAL(Vector2){ face.y, -face.x }; - MathVector2Normalize(&data.normals[i]); - } - - return data; -} - -// Creates a rectangle polygon shape based on a min and max positions -static PhysicsVertexData CreateRectanglePolygon(Vector2 pos, Vector2 size) -{ - PhysicsVertexData data = { 0 }; - data.vertexCount = 4; - - // Calculate polygon vertices positions - data.positions[0] = CLITERAL(Vector2){ pos.x + size.x/2, pos.y - size.y/2 }; - data.positions[1] = CLITERAL(Vector2){ pos.x + size.x/2, pos.y + size.y/2 }; - data.positions[2] = CLITERAL(Vector2){ pos.x - size.x/2, pos.y + size.y/2 }; - data.positions[3] = CLITERAL(Vector2){ pos.x - size.x/2, pos.y - size.y/2 }; - - // Calculate polygon faces normals - for (unsigned int i = 0; i < data.vertexCount; i++) - { - int nextIndex = (((i + 1) < data.vertexCount) ? (i + 1) : 0); - Vector2 face = MathVector2Subtract(data.positions[nextIndex], data.positions[i]); - - data.normals[i] = CLITERAL(Vector2){ face.y, -face.x }; - MathVector2Normalize(&data.normals[i]); - } - - return data; -} - -// Update physics step (dynamics, collisions and position corrections) -void UpdatePhysicsStep(void) -{ - // Clear previous generated collisions information - for (int i = (int)physicsManifoldsCount - 1; i >= 0; i--) - { - PhysicsManifold manifold = contacts[i]; - if (manifold != NULL) DestroyPhysicsManifold(manifold); - } - - // Reset physics bodies grounded state - for (unsigned int i = 0; i < physicsBodiesCount; i++) - { - PhysicsBody body = bodies[i]; - body->isGrounded = false; - } - - // Generate new collision information - for (unsigned int i = 0; i < physicsBodiesCount; i++) - { - PhysicsBody bodyA = bodies[i]; - - if (bodyA != NULL) - { - for (unsigned int j = i + 1; j < physicsBodiesCount; j++) - { - PhysicsBody bodyB = bodies[j]; - - if (bodyB != NULL) - { - if ((bodyA->inverseMass == 0) && (bodyB->inverseMass == 0)) continue; - - PhysicsManifold 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 manifold = CreatePhysicsManifold(bodyA, bodyB); - manifold->penetration = manifold->penetration; - manifold->normal = manifold->normal; - manifold->contacts[0] = manifold->contacts[0]; - manifold->contacts[1] = manifold->contacts[1]; - manifold->contactsCount = manifold->contactsCount; - manifold->restitution = manifold->restitution; - manifold->dynamicFriction = manifold->dynamicFriction; - manifold->staticFriction = manifold->staticFriction; - } - } - } - } - } - - // Integrate forces to physics bodies - for (unsigned int i = 0; i < physicsBodiesCount; i++) - { - PhysicsBody body = bodies[i]; - if (body != NULL) IntegratePhysicsForces(body); - } - - // Initialize physics manifolds to solve collisions - for (unsigned int i = 0; i < physicsManifoldsCount; i++) - { - PhysicsManifold manifold = contacts[i]; - if (manifold != NULL) InitializePhysicsManifolds(manifold); - } - - // Integrate physics collisions impulses to solve collisions - for (unsigned int i = 0; i < PHYSAC_COLLISION_ITERATIONS; i++) - { - for (unsigned int j = 0; j < physicsManifoldsCount; j++) - { - PhysicsManifold manifold = contacts[i]; - if (manifold != NULL) IntegratePhysicsImpulses(manifold); - } - } - - // Integrate velocity to physics bodies - for (unsigned 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 (unsigned int i = 0; i < physicsManifoldsCount; i++) - { - PhysicsManifold manifold = contacts[i]; - if (manifold != NULL) CorrectPhysicsPositions(manifold); - } - - // Clear physics bodies forces - for (unsigned int i = 0; i < physicsBodiesCount; i++) - { - PhysicsBody body = bodies[i]; - if (body != NULL) - { - body->force = PHYSAC_VECTOR_ZERO; - body->torque = 0.0f; - } - } -} - -// Update physics system -// Physics steps are launched at a fixed time step if enabled -PHYSACDEF void UpdatePhysics(void) -{ -#if !defined(PHYSAC_AVOID_TIMMING_SYSTEM) - static double deltaTimeAccumulator = 0.0; - - // Calculate current time (ms) - currentTime = GetCurrentTime(); - - // Calculate current delta time (ms) - const double delta = currentTime - startTime; - - // Store the time elapsed since the last frame began - deltaTimeAccumulator += delta; - - // Fixed time stepping loop - while (deltaTimeAccumulator >= deltaTime) - { - UpdatePhysicsStep(); - deltaTimeAccumulator -= deltaTime; - } - - // Record the starting of this frame - startTime = currentTime; -#else - UpdatePhysicsStep(); -#endif -} - -PHYSACDEF void SetPhysicsTimeStep(double delta) -{ - deltaTime = delta; -} - -// Finds a valid index for a new manifold initialization -static int FindAvailableManifoldIndex() -{ - int index = -1; - for (int i = 0; i < PHYSAC_MAX_MANIFOLDS; i++) - { - int currentId = i; - - // Check if current id already exist in other physics body - for (unsigned 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) - { - index = i; - break; - } - } - - return index; -} - -// Creates a new physics manifold to solve collision -static PhysicsManifold CreatePhysicsManifold(PhysicsBody a, PhysicsBody b) -{ - PhysicsManifold manifold = (PhysicsManifold)PHYSAC_MALLOC(sizeof(PhysicsManifoldData)); - usedMemory += sizeof(PhysicsManifoldData); - - int id = FindAvailableManifoldIndex(); - if (id != -1) - { - // Initialize new manifold with generic values - manifold->id = id; - manifold->bodyA = a; - manifold->bodyB = b; - manifold->penetration = 0; - manifold->normal = PHYSAC_VECTOR_ZERO; - manifold->contacts[0] = PHYSAC_VECTOR_ZERO; - manifold->contacts[1] = PHYSAC_VECTOR_ZERO; - manifold->contactsCount = 0; - manifold->restitution = 0.0f; - manifold->dynamicFriction = 0.0f; - manifold->staticFriction = 0.0f; - - // Add new body to bodies pointers array and update bodies count - contacts[physicsManifoldsCount] = manifold; - physicsManifoldsCount++; - } - else TRACELOG("[PHYSAC] Physic manifold could not be created, PHYSAC_MAX_MANIFOLDS reached\n"); - - return manifold; -} - -// Unitializes and destroys a physics manifold -static void DestroyPhysicsManifold(PhysicsManifold manifold) -{ - if (manifold != NULL) - { - int id = manifold->id; - int index = -1; - - for (unsigned int i = 0; i < physicsManifoldsCount; i++) - { - if (contacts[i]->id == id) - { - index = i; - break; - } - } - - if (index == -1) return; // Prevent access to index -1 - - // Free manifold allocated memory - PHYSAC_FREE(manifold); - usedMemory -= sizeof(PhysicsManifoldData); - contacts[index] = NULL; - - // Reorder physics manifolds pointers array and its catched index - for (unsigned int i = index; i < physicsManifoldsCount; i++) - { - if ((i + 1) < physicsManifoldsCount) contacts[i] = contacts[i + 1]; - } - - // Update physics manifolds count - physicsManifoldsCount--; - } - else TRACELOG("[PHYSAC] WARNING: DestroyPhysicsManifold: NULL physic manifold\n"); -} - -// 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 down and grounded state is not set yet in previous manifolds - if (!manifold->bodyB->isGrounded) 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; - - if ((bodyA == NULL) || (bodyB == NULL)) return; - - // Calculate translational vector, which is normal - Vector2 normal = MathVector2Subtract(bodyB->position, bodyA->position); - - float distSqr = MathVector2SqrLen(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.0f) - { - manifold->penetration = bodyA->shape.radius; - manifold->normal = CLITERAL(Vector2){ 1.0f, 0.0f }; - manifold->contacts[0] = bodyA->position; - } - else - { - manifold->penetration = radius - distance; - manifold->normal = CLITERAL(Vector2){ normal.x/distance, normal.y/distance }; // Faster than using MathVector2Normalize() due to sqrt is already performed - manifold->contacts[0] = CLITERAL(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 (!bodyA->isGrounded) bodyA->isGrounded = (manifold->normal.y < 0); -} - -// Solves collision between a circle to a polygon shape physics bodies -static void SolveCircleToPolygon(PhysicsManifold manifold) -{ - PhysicsBody bodyA = manifold->bodyA; - PhysicsBody bodyB = manifold->bodyB; - - if ((bodyA == NULL) || (bodyB == NULL)) return; - - manifold->contactsCount = 0; - - // Transform circle center to polygon transform space - Vector2 center = bodyA->position; - center = MathMatVector2Product(MathMatTranspose(bodyB->shape.transform), MathVector2Subtract(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; - PhysicsVertexData vertexData = bodyB->shape.vertexData; - - for (unsigned int i = 0; i < vertexData.vertexCount; i++) - { - float currentSeparation = MathVector2DotProduct(vertexData.normals[i], MathVector2Subtract(center, vertexData.positions[i])); - - if (currentSeparation > bodyA->shape.radius) return; - - if (currentSeparation > separation) - { - separation = currentSeparation; - faceNormal = i; - } - } - - // Grab face's vertices - Vector2 v1 = vertexData.positions[faceNormal]; - int nextIndex = (((faceNormal + 1) < (int)vertexData.vertexCount) ? (faceNormal + 1) : 0); - Vector2 v2 = vertexData.positions[nextIndex]; - - // Check to see if center is within polygon - if (separation < PHYSAC_EPSILON) - { - manifold->contactsCount = 1; - Vector2 normal = MathMatVector2Product(bodyB->shape.transform, vertexData.normals[faceNormal]); - manifold->normal = CLITERAL(Vector2){ -normal.x, -normal.y }; - manifold->contacts[0] = CLITERAL(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 = MathVector2DotProduct(MathVector2Subtract(center, v1), MathVector2Subtract(v2, v1)); - float dot2 = MathVector2DotProduct(MathVector2Subtract(center, v2), MathVector2Subtract(v1, v2)); - manifold->penetration = bodyA->shape.radius - separation; - - if (dot1 <= 0.0f) // Closest to v1 - { - if (MathVector2SqrDistance(center, v1) > bodyA->shape.radius*bodyA->shape.radius) return; - - manifold->contactsCount = 1; - Vector2 normal = MathVector2Subtract(v1, center); - normal = MathMatVector2Product(bodyB->shape.transform, normal); - MathVector2Normalize(&normal); - manifold->normal = normal; - v1 = MathMatVector2Product(bodyB->shape.transform, v1); - v1 = MathVector2Add(v1, bodyB->position); - manifold->contacts[0] = v1; - } - else if (dot2 <= 0.0f) // Closest to v2 - { - if (MathVector2SqrDistance(center, v2) > bodyA->shape.radius*bodyA->shape.radius) return; - - manifold->contactsCount = 1; - Vector2 normal = MathVector2Subtract(v2, center); - v2 = MathMatVector2Product(bodyB->shape.transform, v2); - v2 = MathVector2Add(v2, bodyB->position); - manifold->contacts[0] = v2; - normal = MathMatVector2Product(bodyB->shape.transform, normal); - MathVector2Normalize(&normal); - manifold->normal = normal; - } - else // Closest to face - { - Vector2 normal = vertexData.normals[faceNormal]; - - if (MathVector2DotProduct(MathVector2Subtract(center, v1), normal) > bodyA->shape.radius) return; - - normal = MathMatVector2Product(bodyB->shape.transform, normal); - manifold->normal = CLITERAL(Vector2){ -normal.x, -normal.y }; - manifold->contacts[0] = CLITERAL(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; - - if ((bodyA == NULL) || (bodyB == NULL)) return; - - manifold->bodyA = bodyB; - manifold->bodyB = bodyA; - SolveCircleToPolygon(manifold); - - manifold->normal.x *= -1.0f; - manifold->normal.y *= -1.0f; -} - -// Solves collision between two polygons shape physics bodies -static void SolvePolygonToPolygon(PhysicsManifold manifold) -{ - if ((manifold->bodyA == NULL) || (manifold->bodyB == NULL)) return; - - 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.0f) return; - - // Check for separating axis with B shape's face planes - int faceB = 0; - float penetrationB = FindAxisLeastPenetration(&faceB, bodyB, bodyA); - if (penetrationB >= 0.0f) 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 - // Checking bias range for penetration - if (penetrationA >= (penetrationB*0.95f + penetrationA*0.01f)) - { - 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 - PhysicsVertexData refData = refPoly.vertexData; - Vector2 v1 = refData.positions[referenceIndex]; - referenceIndex = (((referenceIndex + 1) < (int)refData.vertexCount) ? (referenceIndex + 1) : 0); - Vector2 v2 = refData.positions[referenceIndex]; - - // Transform vertices to world space - v1 = MathMatVector2Product(refPoly.transform, v1); - v1 = MathVector2Add(v1, refPoly.body->position); - v2 = MathMatVector2Product(refPoly.transform, v2); - v2 = MathVector2Add(v2, refPoly.body->position); - - // Calculate reference face side normal in world space - Vector2 sidePlaneNormal = MathVector2Subtract(v2, v1); - MathVector2Normalize(&sidePlaneNormal); - - // Orthogonalize - Vector2 refFaceNormal = { sidePlaneNormal.y, -sidePlaneNormal.x }; - float refC = MathVector2DotProduct(refFaceNormal, v1); - float negSide = MathVector2DotProduct(sidePlaneNormal, v1)*-1; - float posSide = MathVector2DotProduct(sidePlaneNormal, v2); - - // MathVector2Clip incident face to reference face side planes (due to floating point error, possible to not have required points - if (MathVector2Clip(CLITERAL(Vector2){ -sidePlaneNormal.x, -sidePlaneNormal.y }, &incidentFace[0], &incidentFace[1], negSide) < 2) return; - if (MathVector2Clip(sidePlaneNormal, &incidentFace[0], &incidentFace[1], posSide) < 2) return; - - // Flip normal if required - manifold->normal = (flip ? CLITERAL(Vector2){ -refFaceNormal.x, -refFaceNormal.y } : refFaceNormal); - - // Keep points behind reference face - int currentPoint = 0; // MathVector2Clipped points behind reference face - float separation = MathVector2DotProduct(refFaceNormal, incidentFace[0]) - refC; - if (separation <= 0.0f) - { - manifold->contacts[currentPoint] = incidentFace[0]; - manifold->penetration = -separation; - currentPoint++; - } - else manifold->penetration = 0.0f; - - separation = MathVector2DotProduct(refFaceNormal, incidentFace[1]) - refC; - - if (separation <= 0.0f) - { - 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 == NULL) || (body->inverseMass == 0.0f) || !body->enabled) return; - - body->velocity.x += (float)((body->force.x*body->inverseMass)*(deltaTime/2.0)); - body->velocity.y += (float)((body->force.y*body->inverseMass)*(deltaTime/2.0)); - - if (body->useGravity) - { - body->velocity.x += (float)(gravityForce.x*(deltaTime/1000/2.0)); - body->velocity.y += (float)(gravityForce.y*(deltaTime/1000/2.0)); - } - - if (!body->freezeOrient) body->angularVelocity += (float)(body->torque*body->inverseInertia*(deltaTime/2.0)); -} - -// Initializes physics manifolds to solve collisions -static void InitializePhysicsManifolds(PhysicsManifold manifold) -{ - PhysicsBody bodyA = manifold->bodyA; - PhysicsBody bodyB = manifold->bodyB; - - if ((bodyA == NULL) || (bodyB == NULL)) return; - - // 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 (unsigned int i = 0; i < manifold->contactsCount; i++) - { - // Caculate radius from center of mass to contact - Vector2 radiusA = MathVector2Subtract(manifold->contacts[i], bodyA->position); - Vector2 radiusB = MathVector2Subtract(manifold->contacts[i], bodyB->position); - - Vector2 crossA = MathVector2Product(radiusA, bodyA->angularVelocity); - Vector2 crossB = MathVector2Product(radiusB, bodyB->angularVelocity); - - Vector2 radiusV = { 0.0f, 0.0f }; - 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 (MathVector2SqrLen(radiusV) < (MathVector2SqrLen(CLITERAL(Vector2){ (float)(gravityForce.x*deltaTime/1000), (float)(gravityForce.y*deltaTime/1000) }) + 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; - - if ((bodyA == NULL) || (bodyB == NULL)) return; - - // Early out and positional correct if both objects have infinite mass - if (fabs(bodyA->inverseMass + bodyB->inverseMass) <= PHYSAC_EPSILON) - { - bodyA->velocity = PHYSAC_VECTOR_ZERO; - bodyB->velocity = PHYSAC_VECTOR_ZERO; - return; - } - - for (unsigned int i = 0; i < manifold->contactsCount; i++) - { - // Calculate radius from center of mass to contact - Vector2 radiusA = MathVector2Subtract(manifold->contacts[i], bodyA->position); - Vector2 radiusB = MathVector2Subtract(manifold->contacts[i], bodyB->position); - - // Calculate relative velocity - Vector2 radiusV = { 0.0f, 0.0f }; - radiusV.x = bodyB->velocity.x + MathVector2Product(radiusB, bodyB->angularVelocity).x - bodyA->velocity.x - MathVector2Product(radiusA, bodyA->angularVelocity).x; - radiusV.y = bodyB->velocity.y + MathVector2Product(radiusB, bodyB->angularVelocity).y - bodyA->velocity.y - MathVector2Product(radiusA, bodyA->angularVelocity).y; - - // Relative velocity along the normal - float contactVelocity = MathVector2DotProduct(radiusV, manifold->normal); - - // Do not resolve if velocities are separating - if (contactVelocity > 0.0f) return; - - float raCrossN = MathVector2CrossProduct(radiusA, manifold->normal); - float rbCrossN = MathVector2CrossProduct(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*MathVector2CrossProduct(radiusA, CLITERAL(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*MathVector2CrossProduct(radiusB, impulseV); - } - - // Apply friction impulse to each physics body - radiusV.x = bodyB->velocity.x + MathVector2Product(radiusB, bodyB->angularVelocity).x - bodyA->velocity.x - MathVector2Product(radiusA, bodyA->angularVelocity).x; - radiusV.y = bodyB->velocity.y + MathVector2Product(radiusB, bodyB->angularVelocity).y - bodyA->velocity.y - MathVector2Product(radiusA, bodyA->angularVelocity).y; - - Vector2 tangent = { radiusV.x - (manifold->normal.x*MathVector2DotProduct(radiusV, manifold->normal)), radiusV.y - (manifold->normal.y*MathVector2DotProduct(radiusV, manifold->normal)) }; - MathVector2Normalize(&tangent); - - // Calculate impulse tangent magnitude - float impulseTangent = -MathVector2DotProduct(radiusV, tangent); - impulseTangent /= inverseMassSum; - impulseTangent /= (float)manifold->contactsCount; - - float absImpulseTangent = (float)fabs(impulseTangent); - - // Don't apply tiny friction impulses - if (absImpulseTangent <= PHYSAC_EPSILON) return; - - // Apply coulumb's law - Vector2 tangentImpulse = { 0.0f, 0.0f }; - if (absImpulseTangent < impulse*manifold->staticFriction) tangentImpulse = CLITERAL(Vector2){ tangent.x*impulseTangent, tangent.y*impulseTangent }; - else tangentImpulse = CLITERAL(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*MathVector2CrossProduct(radiusA, CLITERAL(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*MathVector2CrossProduct(radiusB, tangentImpulse); - } - } -} - -// Integrates physics velocity into position and forces -static void IntegratePhysicsVelocity(PhysicsBody body) -{ - if ((body == NULL) ||!body->enabled) return; - - body->position.x += (float)(body->velocity.x*deltaTime); - body->position.y += (float)(body->velocity.y*deltaTime); - - if (!body->freezeOrient) body->orient += (float)(body->angularVelocity*deltaTime); - body->shape.transform = MathMatFromRadians(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; - - if ((bodyA == NULL) || (bodyB == NULL)) return; - - Vector2 correction = { 0.0f, 0.0f }; - correction.x = (PHYSAC_MAX(manifold->penetration - PHYSAC_PENETRATION_ALLOWANCE, 0.0f)/(bodyA->inverseMass + bodyB->inverseMass))*manifold->normal.x*PHYSAC_PENETRATION_CORRECTION; - correction.y = (PHYSAC_MAX(manifold->penetration - PHYSAC_PENETRATION_ALLOWANCE, 0.0f)/(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.0f, 0.0f }; - PhysicsVertexData data = shape.vertexData; - - for (unsigned int i = 0; i < data.vertexCount; i++) - { - Vector2 vertex = data.positions[i]; - float projection = MathVector2DotProduct(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; - - PhysicsVertexData dataA = shapeA.vertexData; - //PhysicsVertexData dataB = shapeB.vertexData; - - for (unsigned int i = 0; i < dataA.vertexCount; i++) - { - // Retrieve a face normal from A shape - Vector2 normal = dataA.normals[i]; - Vector2 transNormal = MathMatVector2Product(shapeA.transform, normal); - - // Transform face normal into B shape's model space - Matrix2x2 buT = MathMatTranspose(shapeB.transform); - normal = MathMatVector2Product(buT, transNormal); - - // Retrieve support point from B shape along -n - Vector2 support = GetSupport(shapeB, CLITERAL(Vector2){ -normal.x, -normal.y }); - - // Retrieve vertex on face from A shape, transform into B shape's model space - Vector2 vertex = dataA.positions[i]; - vertex = MathMatVector2Product(shapeA.transform, vertex); - vertex = MathVector2Add(vertex, shapeA.body->position); - vertex = MathVector2Subtract(vertex, shapeB.body->position); - vertex = MathMatVector2Product(buT, vertex); - - // Compute penetration distance in B shape's model space - float distance = MathVector2DotProduct(normal, MathVector2Subtract(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) -{ - PhysicsVertexData refData = ref.vertexData; - PhysicsVertexData incData = inc.vertexData; - - Vector2 referenceNormal = refData.normals[index]; - - // Calculate normal in incident's frame of reference - referenceNormal = MathMatVector2Product(ref.transform, referenceNormal); // To world space - referenceNormal = MathMatVector2Product(MathMatTranspose(inc.transform), referenceNormal); // To incident's model space - - // Find most anti-normal face on polygon - int incidentFace = 0; - float minDot = PHYSAC_FLT_MAX; - - for (unsigned int i = 0; i < incData.vertexCount; i++) - { - float dot = MathVector2DotProduct(referenceNormal, incData.normals[i]); - - if (dot < minDot) - { - minDot = dot; - incidentFace = i; - } - } - - // Assign face vertices for incident face - *v0 = MathMatVector2Product(inc.transform, incData.positions[incidentFace]); - *v0 = MathVector2Add(*v0, inc.body->position); - incidentFace = (((incidentFace + 1) < (int)incData.vertexCount) ? (incidentFace + 1) : 0); - *v1 = MathMatVector2Product(inc.transform, incData.positions[incidentFace]); - *v1 = MathVector2Add(*v1, inc.body->position); -} - -// Returns clipping value based on a normal and two faces -static int MathVector2Clip(Vector2 normal, Vector2 *faceA, Vector2 *faceB, float clip) -{ - int sp = 0; - Vector2 out[2] = { *faceA, *faceB }; - - // Retrieve distances from each endpoint to the line - float distanceA = MathVector2DotProduct(normal, *faceA) - clip; - float distanceB = MathVector2DotProduct(normal, *faceB) - clip; - - // If negative (behind plane) - if (distanceA <= 0.0f) out[sp++] = *faceA; - if (distanceB <= 0.0f) out[sp++] = *faceB; - - // If the points are on different sides of the plane - if ((distanceA*distanceB) < 0.0f) - { - // Push intersection point - float alpha = distanceA/(distanceA - distanceB); - out[sp] = *faceA; - Vector2 delta = MathVector2Subtract(*faceB, *faceA); - delta.x *= alpha; - delta.y *= alpha; - out[sp] = MathVector2Add(out[sp], delta); - sp++; - } - - // Assign the new converted values - *faceA = out[0]; - *faceB = out[1]; - - return sp; -} - -// Returns the barycenter of a triangle given by 3 points -static Vector2 MathTriangleBarycenter(Vector2 v1, Vector2 v2, Vector2 v3) -{ - Vector2 result = { 0.0f, 0.0f }; - - result.x = (v1.x + v2.x + v3.x)/3; - result.y = (v1.y + v2.y + v3.y)/3; - - return result; -} - -#if !defined(PHYSAC_AVOID_TIMMING_SYSTEM) -// Initializes hi-resolution MONOTONIC timer -static void InitTimer(void) -{ -#if defined(_WIN32) - QueryPerformanceFrequency((unsigned long long int *) &frequency); -#endif - -#if defined(__EMSCRIPTEN__) || defined(__linux__) - struct timespec now; - if (clock_gettime(CLOCK_MONOTONIC, &now) == 0) frequency = 1000000000; -#endif - -#if defined(__APPLE__) - mach_timebase_info_data_t timebase; - mach_timebase_info(&timebase); - frequency = (timebase.denom*1e9)/timebase.numer; -#endif - - baseClockTicks = (double)GetClockTicks(); // Get MONOTONIC clock time offset - startTime = GetCurrentTime(); // Get current time in milliseconds -} - -// Get hi-res MONOTONIC time measure in clock ticks -static unsigned long long int GetClockTicks(void) -{ - unsigned long long int value = 0; - -#if defined(_WIN32) - QueryPerformanceCounter((unsigned long long int *) &value); -#endif - -#if defined(__linux__) - struct timespec now; - clock_gettime(CLOCK_MONOTONIC, &now); - value = (unsigned long long int)now.tv_sec*(unsigned long long int)1000000000 + (unsigned long long int)now.tv_nsec; -#endif - -#if defined(__APPLE__) - value = mach_absolute_time(); -#endif - - return value; -} - -// Get current time in milliseconds -static double GetCurrentTime(void) -{ - return (double)(GetClockTicks() - baseClockTicks)/frequency*1000; -} -#endif // !PHYSAC_AVOID_TIMMING_SYSTEM - - -// Returns the cross product of a vector and a value -static inline Vector2 MathVector2Product(Vector2 vector, float value) -{ - Vector2 result = { -value*vector.y, value*vector.x }; - return result; -} - -// Returns the cross product of two vectors -static inline float MathVector2CrossProduct(Vector2 v1, Vector2 v2) -{ - return (v1.x*v2.y - v1.y*v2.x); -} - -// Returns the len square root of a vector -static inline float MathVector2SqrLen(Vector2 vector) -{ - return (vector.x*vector.x + vector.y*vector.y); -} - -// Returns the dot product of two vectors -static inline float MathVector2DotProduct(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 MathVector2SqrDistance(Vector2 v1, Vector2 v2) -{ - Vector2 dir = MathVector2Subtract(v1, v2); - return MathVector2DotProduct(dir, dir); -} - -// Returns the normalized values of a vector -static void MathVector2Normalize(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 MathVector2Add(Vector2 v1, Vector2 v2) -{ - Vector2 result = { v1.x + v2.x, v1.y + v2.y }; - return result; -} - -// Returns the subtract of two given vectors -static inline Vector2 MathVector2Subtract(Vector2 v1, Vector2 v2) -{ - Vector2 result = { v1.x - v2.x, v1.y - v2.y }; - return result; -} - -// Creates a matrix 2x2 from a given radians value -static Matrix2x2 MathMatFromRadians(float radians) -{ - float cos = cosf(radians); - float sin = sinf(radians); - - Matrix2x2 result = { cos, -sin, sin, cos }; - return result; -} - -// Returns the transpose of a given matrix 2x2 -static inline Matrix2x2 MathMatTranspose(Matrix2x2 matrix) -{ - Matrix2x2 result = { matrix.m00, matrix.m10, matrix.m01, matrix.m11 }; - return result; -} - -// Multiplies a vector by a matrix 2x2 -static inline Vector2 MathMatVector2Product(Matrix2x2 matrix, Vector2 vector) -{ - Vector2 result = { matrix.m00*vector.x + matrix.m01*vector.y, matrix.m10*vector.x + matrix.m11*vector.y }; - return result; -} - -#endif // PHYSAC_IMPLEMENTATION |