1 /**********************************************************************************************
3 * Physac v1.1 - 2D Physics library for videogames
7 * Physac is a small 2D physics engine written in pure C. The engine uses a fixed time-step thread loop
8 * to simluate physics. A physics step contains the following phases: get collision information,
9 * apply dynamics, collision solving and position correction. It uses a very simple struct for physic
10 * bodies with a position vector to be used in any 3D rendering API.
14 * #define PHYSAC_IMPLEMENTATION
15 * Generates the implementation of the library into the included file.
16 * If not defined, the library is in header only mode and can be included in other headers
17 * or source files without problems. But only ONE file should hold the implementation.
19 * #define PHYSAC_STATIC (defined by default)
20 * The generated implementation will stay private inside implementation file and all
21 * internal symbols and functions will only be visible inside that file.
23 * #define PHYSAC_DEBUG
24 * Show debug traces log messages about physic bodies creation/destruction, physic system errors,
25 * some calculations results and NULL reference exceptions
27 * #define PHYSAC_DEFINE_VECTOR2_TYPE
28 * Forces library to define struct Vector2 data type (float x; float y)
30 * #define PHYSAC_AVOID_TIMMING_SYSTEM
31 * Disables internal timming system, used by UpdatePhysics() to launch timmed physic steps,
32 * it allows just running UpdatePhysics() automatically on a separate thread at a desired time step.
33 * In case physics steps update needs to be controlled by user with a custom timming mechanism,
34 * just define this flag and the internal timming mechanism will be avoided, in that case,
35 * timming libraries are neither required by the module.
37 * #define PHYSAC_MALLOC()
38 * #define PHYSAC_CALLOC()
39 * #define PHYSAC_FREE()
40 * You can define your own malloc/free implementation replacing stdlib.h malloc()/free() functions.
41 * Otherwise it will include stdlib.h and use the C standard library malloc()/free() function.
45 * Use the following code to compile with GCC:
46 * gcc -o $(NAME_PART).exe $(FILE_NAME) -s -static -lraylib -lopengl32 -lgdi32 -lwinmm -std=c99
49 * 1.1 (20-Jan-2021) @raysan5: Library general revision
50 * Removed threading system (up to the user)
51 * Support MSVC C++ compilation using CLITERAL()
52 * Review DEBUG mechanism for TRACELOG() and all TRACELOG() messages
53 * Review internal variables/functions naming for consistency
54 * Allow option to avoid internal timming system, to allow app manage the steps
55 * 1.0 (12-Jun-2017) First release of the library
58 * LICENSE: zlib/libpng
60 * Copyright (c) 2016-2021 Victor Fisac (@victorfisac) and Ramon Santamaria (@raysan5)
62 * This software is provided "as-is", without any express or implied warranty. In no event
63 * will the authors be held liable for any damages arising from the use of this software.
65 * Permission is granted to anyone to use this software for any purpose, including commercial
66 * applications, and to alter it and redistribute it freely, subject to the following restrictions:
68 * 1. The origin of this software must not be misrepresented; you must not claim that you
69 * wrote the original software. If you use this software in a product, an acknowledgment
70 * in the product documentation would be appreciated but is not required.
72 * 2. Altered source versions must be plainly marked as such, and must not be misrepresented
73 * as being the original software.
75 * 3. This notice may not be removed or altered from any source distribution.
77 **********************************************************************************************/
79 #if !defined(PHYSAC_H)
82 #if defined(PHYSAC_STATIC)
83 #define PHYSACDEF static // Functions just visible to module including this file
85 #if defined(__cplusplus)
86 #define PHYSACDEF extern "C" // Functions visible from other files (no name mangling of functions in C++)
88 #define PHYSACDEF extern // Functions visible from other files
92 // Allow custom memory allocators
94 #define PHYSAC_MALLOC(size) malloc(size)
97 #define PHYSAC_CALLOC(size, n) calloc(size, n)
100 #define PHYSAC_FREE(ptr) free(ptr)
103 //----------------------------------------------------------------------------------
104 // Defines and Macros
105 //----------------------------------------------------------------------------------
106 #define PHYSAC_MAX_BODIES 64 // Maximum number of physic bodies supported
107 #define PHYSAC_MAX_MANIFOLDS 4096 // Maximum number of physic bodies interactions (64x64)
108 #define PHYSAC_MAX_VERTICES 24 // Maximum number of vertex for polygons shapes
109 #define PHYSAC_DEFAULT_CIRCLE_VERTICES 24 // Default number of vertices for circle shapes
111 #define PHYSAC_COLLISION_ITERATIONS 100
112 #define PHYSAC_PENETRATION_ALLOWANCE 0.05f
113 #define PHYSAC_PENETRATION_CORRECTION 0.4f
115 #define PHYSAC_PI 3.14159265358979323846f
116 #define PHYSAC_DEG2RAD (PHYSAC_PI/180.0f)
118 //----------------------------------------------------------------------------------
119 // Data Types Structure Definition
120 //----------------------------------------------------------------------------------
121 #if defined(__STDC__) && __STDC_VERSION__ >= 199901L
125 typedef enum PhysicsShapeType { PHYSICS_CIRCLE = 0, PHYSICS_POLYGON } PhysicsShapeType;
127 // Previously defined to be used in PhysicsShape struct as circular dependencies
128 typedef struct PhysicsBodyData *PhysicsBody;
130 #if defined(PHYSAC_DEFINE_VECTOR2_TYPE)
132 typedef struct Vector2 {
138 // Matrix2x2 type (used for polygon shape rotation matrix)
139 typedef struct Matrix2x2 {
146 typedef struct PhysicsVertexData {
147 unsigned int vertexCount; // Vertex count (positions and normals)
148 Vector2 positions[PHYSAC_MAX_VERTICES]; // Vertex positions vectors
149 Vector2 normals[PHYSAC_MAX_VERTICES]; // Vertex normals vectors
152 typedef struct PhysicsShape {
153 PhysicsShapeType type; // Shape type (circle or polygon)
154 PhysicsBody body; // Shape physics body data pointer
155 PhysicsVertexData vertexData; // Shape vertices data (used for polygon shapes)
156 float radius; // Shape radius (used for circle shapes)
157 Matrix2x2 transform; // Vertices transform matrix 2x2
160 typedef struct PhysicsBodyData {
161 unsigned int id; // Unique identifier
162 bool enabled; // Enabled dynamics state (collisions are calculated anyway)
163 Vector2 position; // Physics body shape pivot
164 Vector2 velocity; // Current linear velocity applied to position
165 Vector2 force; // Current linear force (reset to 0 every step)
166 float angularVelocity; // Current angular velocity applied to orient
167 float torque; // Current angular force (reset to 0 every step)
168 float orient; // Rotation in radians
169 float inertia; // Moment of inertia
170 float inverseInertia; // Inverse value of inertia
171 float mass; // Physics body mass
172 float inverseMass; // Inverse value of mass
173 float staticFriction; // Friction when the body has not movement (0 to 1)
174 float dynamicFriction; // Friction when the body has movement (0 to 1)
175 float restitution; // Restitution coefficient of the body (0 to 1)
176 bool useGravity; // Apply gravity force to dynamics
177 bool isGrounded; // Physics grounded on other body state
178 bool freezeOrient; // Physics rotation constraint
179 PhysicsShape shape; // Physics body shape information (type, radius, vertices, transform)
182 typedef struct PhysicsManifoldData {
183 unsigned int id; // Unique identifier
184 PhysicsBody bodyA; // Manifold first physics body reference
185 PhysicsBody bodyB; // Manifold second physics body reference
186 float penetration; // Depth of penetration from collision
187 Vector2 normal; // Normal direction vector from 'a' to 'b'
188 Vector2 contacts[2]; // Points of contact during collision
189 unsigned int contactsCount; // Current collision number of contacts
190 float restitution; // Mixed restitution during collision
191 float dynamicFriction; // Mixed dynamic friction during collision
192 float staticFriction; // Mixed static friction during collision
193 } PhysicsManifoldData, *PhysicsManifold;
195 #if defined(__cplusplus)
196 extern "C" { // Prevents name mangling of functions
199 //----------------------------------------------------------------------------------
200 // Module Functions Declaration
201 //----------------------------------------------------------------------------------
202 // Physics system management
203 PHYSACDEF void InitPhysics(void); // Initializes physics system
204 PHYSACDEF void UpdatePhysics(void); // Update physics system
205 PHYSACDEF void ResetPhysics(void); // Reset physics system (global variables)
206 PHYSACDEF void ClosePhysics(void); // Close physics system and unload used memory
207 PHYSACDEF void SetPhysicsTimeStep(double delta); // Sets physics fixed time step in milliseconds. 1.666666 by default
208 PHYSACDEF void SetPhysicsGravity(float x, float y); // Sets physics global gravity force
210 // Physic body creation/destroy
211 PHYSACDEF PhysicsBody CreatePhysicsBodyCircle(Vector2 pos, float radius, float density); // Creates a new circle physics body with generic parameters
212 PHYSACDEF PhysicsBody CreatePhysicsBodyRectangle(Vector2 pos, float width, float height, float density); // Creates a new rectangle physics body with generic parameters
213 PHYSACDEF PhysicsBody CreatePhysicsBodyPolygon(Vector2 pos, float radius, int sides, float density); // Creates a new polygon physics body with generic parameters
214 PHYSACDEF void DestroyPhysicsBody(PhysicsBody body); // Destroy a physics body
216 // Physic body forces
217 PHYSACDEF void PhysicsAddForce(PhysicsBody body, Vector2 force); // Adds a force to a physics body
218 PHYSACDEF void PhysicsAddTorque(PhysicsBody body, float amount); // Adds an angular force to a physics body
219 PHYSACDEF void PhysicsShatter(PhysicsBody body, Vector2 position, float force); // Shatters a polygon shape physics body to little physics bodies with explosion force
220 PHYSACDEF void SetPhysicsBodyRotation(PhysicsBody body, float radians); // Sets physics body shape transform based on radians parameter
222 // Query physics info
223 PHYSACDEF PhysicsBody GetPhysicsBody(int index); // Returns a physics body of the bodies pool at a specific index
224 PHYSACDEF int GetPhysicsBodiesCount(void); // Returns the current amount of created physics bodies
225 PHYSACDEF int GetPhysicsShapeType(int index); // Returns the physics body shape type (PHYSICS_CIRCLE or PHYSICS_POLYGON)
226 PHYSACDEF int GetPhysicsShapeVerticesCount(int index); // Returns the amount of vertices of a physics body shape
227 PHYSACDEF Vector2 GetPhysicsShapeVertex(PhysicsBody body, int vertex); // Returns transformed position of a body shape (body position + vertex transformed position)
229 #if defined(__cplusplus)
235 /***********************************************************************************
237 * PHYSAC IMPLEMENTATION
239 ************************************************************************************/
241 #if defined(PHYSAC_IMPLEMENTATION)
243 // Support TRACELOG macros
244 #if defined(PHYSAC_DEBUG)
245 #include <stdio.h> // Required for: printf()
246 #define TRACELOG(...) printf(__VA_ARGS__)
248 #define TRACELOG(...) (void)0;
251 #include <stdlib.h> // Required for: malloc(), calloc(), free()
252 #include <math.h> // Required for: cosf(), sinf(), fabs(), sqrtf()
254 #if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
255 // Time management functionality
256 #include <time.h> // Required for: time(), clock_gettime()
258 // Functions required to query time on Windows
259 int __stdcall QueryPerformanceCounter(unsigned long long int *lpPerformanceCount);
260 int __stdcall QueryPerformanceFrequency(unsigned long long int *lpFrequency);
262 #if defined(__linux__) || defined(__FreeBSD__)
263 #if _POSIX_C_SOURCE < 199309L
264 #undef _POSIX_C_SOURCE
265 #define _POSIX_C_SOURCE 199309L // Required for CLOCK_MONOTONIC if compiled with c99 without gnu ext.
267 #include <sys/time.h> // Required for: timespec
269 #if defined(__APPLE__) // macOS also defines __MACH__
270 #include <mach/mach_time.h> // Required for: mach_absolute_time()
274 // NOTE: MSVC C++ compiler does not support compound literals (C99 feature)
275 // Plain structures in C++ (without constructors) can be initialized from { } initializers.
276 #if defined(__cplusplus)
277 #define CLITERAL(type) type
279 #define CLITERAL(type) (type)
282 //----------------------------------------------------------------------------------
283 // Defines and Macros
284 //----------------------------------------------------------------------------------
285 #define PHYSAC_MIN(a,b) (((a)<(b))?(a):(b))
286 #define PHYSAC_MAX(a,b) (((a)>(b))?(a):(b))
287 #define PHYSAC_FLT_MAX 3.402823466e+38f
288 #define PHYSAC_EPSILON 0.000001f
289 #define PHYSAC_K 1.0f/3.0f
290 #define PHYSAC_VECTOR_ZERO CLITERAL(Vector2){ 0.0f, 0.0f }
292 //----------------------------------------------------------------------------------
293 // Global Variables Definition
294 //----------------------------------------------------------------------------------
295 static double deltaTime = 1.0/60.0/10.0 * 1000; // Delta time in milliseconds used for physics steps
297 #if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
298 // Time measure variables
299 static double baseClockTicks = 0.0; // Offset clock ticks for MONOTONIC clock
300 static unsigned long long int frequency = 0; // Hi-res clock frequency
301 static double startTime = 0.0; // Start time in milliseconds
302 static double currentTime = 0.0; // Current time in milliseconds
305 // Physics system configuration
306 static PhysicsBody bodies[PHYSAC_MAX_BODIES]; // Physics bodies pointers array
307 static unsigned int physicsBodiesCount = 0; // Physics world current bodies counter
308 static PhysicsManifold contacts[PHYSAC_MAX_MANIFOLDS]; // Physics bodies pointers array
309 static unsigned int physicsManifoldsCount = 0; // Physics world current manifolds counter
311 static Vector2 gravityForce = { 0.0f, 9.81f }; // Physics world gravity force
313 // Utilities variables
314 static unsigned int usedMemory = 0; // Total allocated dynamic memory
316 //----------------------------------------------------------------------------------
317 // Module Internal Functions Declaration
318 //----------------------------------------------------------------------------------
319 #if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
320 // Timming measure functions
321 static void InitTimer(void); // Initializes hi-resolution MONOTONIC timer
322 static unsigned long long int GetClockTicks(void); // Get hi-res MONOTONIC time measure in mseconds
323 static double GetCurrentTime(void); // Get current time measure in milliseconds
326 static void UpdatePhysicsStep(void); // Update physics step (dynamics, collisions and position corrections)
328 static int FindAvailableBodyIndex(); // Finds a valid index for a new physics body initialization
329 static int FindAvailableManifoldIndex(); // Finds a valid index for a new manifold initialization
330 static PhysicsVertexData CreateDefaultPolygon(float radius, int sides); // Creates a random polygon shape with max vertex distance from polygon pivot
331 static PhysicsVertexData CreateRectanglePolygon(Vector2 pos, Vector2 size); // Creates a rectangle polygon shape based on a min and max positions
333 static void InitializePhysicsManifolds(PhysicsManifold manifold); // Initializes physics manifolds to solve collisions
334 static PhysicsManifold CreatePhysicsManifold(PhysicsBody a, PhysicsBody b); // Creates a new physics manifold to solve collision
335 static void DestroyPhysicsManifold(PhysicsManifold manifold); // Unitializes and destroys a physics manifold
337 static void SolvePhysicsManifold(PhysicsManifold manifold); // Solves a created physics manifold between two physics bodies
338 static void SolveCircleToCircle(PhysicsManifold manifold); // Solves collision between two circle shape physics bodies
339 static void SolveCircleToPolygon(PhysicsManifold manifold); // Solves collision between a circle to a polygon shape physics bodies
340 static void SolvePolygonToCircle(PhysicsManifold manifold); // Solves collision between a polygon to a circle shape physics bodies
341 static void SolvePolygonToPolygon(PhysicsManifold manifold); // Solves collision between two polygons shape physics bodies
342 static void IntegratePhysicsForces(PhysicsBody body); // Integrates physics forces into velocity
343 static void IntegratePhysicsVelocity(PhysicsBody body); // Integrates physics velocity into position and forces
344 static void IntegratePhysicsImpulses(PhysicsManifold manifold); // Integrates physics collisions impulses to solve collisions
345 static void CorrectPhysicsPositions(PhysicsManifold manifold); // Corrects physics bodies positions based on manifolds collision information
346 static void FindIncidentFace(Vector2 *v0, Vector2 *v1, PhysicsShape ref, PhysicsShape inc, int index); // Finds two polygon shapes incident face
347 static float FindAxisLeastPenetration(int *faceIndex, PhysicsShape shapeA, PhysicsShape shapeB); // Finds polygon shapes axis least penetration
349 // Math required functions
350 static Vector2 MathVector2Product(Vector2 vector, float value); // Returns the product of a vector and a value
351 static float MathVector2CrossProduct(Vector2 v1, Vector2 v2); // Returns the cross product of two vectors
352 static float MathVector2SqrLen(Vector2 vector); // Returns the len square root of a vector
353 static float MathVector2DotProduct(Vector2 v1, Vector2 v2); // Returns the dot product of two vectors
354 static inline float MathVector2SqrDistance(Vector2 v1, Vector2 v2); // Returns the square root of distance between two vectors
355 static void MathVector2Normalize(Vector2 *vector); // Returns the normalized values of a vector
356 static Vector2 MathVector2Add(Vector2 v1, Vector2 v2); // Returns the sum of two given vectors
357 static Vector2 MathVector2Subtract(Vector2 v1, Vector2 v2); // Returns the subtract of two given vectors
358 static Matrix2x2 MathMatFromRadians(float radians); // Returns a matrix 2x2 from a given radians value
359 static inline Matrix2x2 MathMatTranspose(Matrix2x2 matrix); // Returns the transpose of a given matrix 2x2
360 static inline Vector2 MathMatVector2Product(Matrix2x2 matrix, Vector2 vector); // Returns product between matrix 2x2 and vector
361 static int MathVector2Clip(Vector2 normal, Vector2 *faceA, Vector2 *faceB, float clip); // Returns clipping value based on a normal and two faces
362 static Vector2 MathTriangleBarycenter(Vector2 v1, Vector2 v2, Vector2 v3); // Returns the barycenter of a triangle given by 3 points
364 //----------------------------------------------------------------------------------
365 // Module Functions Definition
366 //----------------------------------------------------------------------------------
368 // Initializes physics values, pointers and creates physics loop thread
369 PHYSACDEF void InitPhysics(void)
371 #if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
372 // Initialize high resolution timer
376 TRACELOG("[PHYSAC] Physics module initialized successfully\n");
379 // Sets physics global gravity force
380 PHYSACDEF void SetPhysicsGravity(float x, float y)
386 // Creates a new circle physics body with generic parameters
387 PHYSACDEF PhysicsBody CreatePhysicsBodyCircle(Vector2 pos, float radius, float density)
389 PhysicsBody body = CreatePhysicsBodyPolygon(pos, radius, PHYSAC_DEFAULT_CIRCLE_VERTICES, density);
393 // Creates a new rectangle physics body with generic parameters
394 PHYSACDEF PhysicsBody CreatePhysicsBodyRectangle(Vector2 pos, float width, float height, float density)
396 // NOTE: Make sure body data is initialized to 0
397 PhysicsBody body = (PhysicsBody)PHYSAC_CALLOC(sizeof(PhysicsBodyData), 1);
398 usedMemory += sizeof(PhysicsBodyData);
400 int id = FindAvailableBodyIndex();
403 // Initialize new body with generic values
405 body->enabled = true;
406 body->position = pos;
407 body->shape.type = PHYSICS_POLYGON;
408 body->shape.body = body;
409 body->shape.transform = MathMatFromRadians(0.0f);
410 body->shape.vertexData = CreateRectanglePolygon(pos, CLITERAL(Vector2){ width, height });
412 // Calculate centroid and moment of inertia
413 Vector2 center = { 0.0f, 0.0f };
415 float inertia = 0.0f;
417 for (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++)
419 // Triangle vertices, third vertex implied as (0, 0)
420 Vector2 p1 = body->shape.vertexData.positions[i];
421 unsigned int nextIndex = (((i + 1) < body->shape.vertexData.vertexCount) ? (i + 1) : 0);
422 Vector2 p2 = body->shape.vertexData.positions[nextIndex];
424 float D = MathVector2CrossProduct(p1, p2);
425 float triangleArea = D/2;
427 area += triangleArea;
429 // Use area to weight the centroid average, not just vertex position
430 center.x += triangleArea*PHYSAC_K*(p1.x + p2.x);
431 center.y += triangleArea*PHYSAC_K*(p1.y + p2.y);
433 float intx2 = p1.x*p1.x + p2.x*p1.x + p2.x*p2.x;
434 float inty2 = p1.y*p1.y + p2.y*p1.y + p2.y*p2.y;
435 inertia += (0.25f*PHYSAC_K*D)*(intx2 + inty2);
438 center.x *= 1.0f/area;
439 center.y *= 1.0f/area;
441 // Translate vertices to centroid (make the centroid (0, 0) for the polygon in model space)
442 // Note: this is not really necessary
443 for (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++)
445 body->shape.vertexData.positions[i].x -= center.x;
446 body->shape.vertexData.positions[i].y -= center.y;
449 body->mass = density*area;
450 body->inverseMass = ((body->mass != 0.0f) ? 1.0f/body->mass : 0.0f);
451 body->inertia = density*inertia;
452 body->inverseInertia = ((body->inertia != 0.0f) ? 1.0f/body->inertia : 0.0f);
453 body->staticFriction = 0.4f;
454 body->dynamicFriction = 0.2f;
455 body->restitution = 0.0f;
456 body->useGravity = true;
457 body->isGrounded = false;
458 body->freezeOrient = false;
460 // Add new body to bodies pointers array and update bodies count
461 bodies[physicsBodiesCount] = body;
462 physicsBodiesCount++;
464 TRACELOG("[PHYSAC] Physic body created successfully (id: %i)\n", body->id);
466 else TRACELOG("[PHYSAC] Physic body could not be created, PHYSAC_MAX_BODIES reached\n");
471 // Creates a new polygon physics body with generic parameters
472 PHYSACDEF PhysicsBody CreatePhysicsBodyPolygon(Vector2 pos, float radius, int sides, float density)
474 PhysicsBody body = (PhysicsBody)PHYSAC_MALLOC(sizeof(PhysicsBodyData));
475 usedMemory += sizeof(PhysicsBodyData);
477 int id = FindAvailableBodyIndex();
480 // Initialize new body with generic values
482 body->enabled = true;
483 body->position = pos;
484 body->velocity = PHYSAC_VECTOR_ZERO;
485 body->force = PHYSAC_VECTOR_ZERO;
486 body->angularVelocity = 0.0f;
489 body->shape.type = PHYSICS_POLYGON;
490 body->shape.body = body;
491 body->shape.transform = MathMatFromRadians(0.0f);
492 body->shape.vertexData = CreateDefaultPolygon(radius, sides);
494 // Calculate centroid and moment of inertia
495 Vector2 center = { 0.0f, 0.0f };
497 float inertia = 0.0f;
499 for (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++)
501 // Triangle vertices, third vertex implied as (0, 0)
502 Vector2 position1 = body->shape.vertexData.positions[i];
503 unsigned int nextIndex = (((i + 1) < body->shape.vertexData.vertexCount) ? (i + 1) : 0);
504 Vector2 position2 = body->shape.vertexData.positions[nextIndex];
506 float cross = MathVector2CrossProduct(position1, position2);
507 float triangleArea = cross/2;
509 area += triangleArea;
511 // Use area to weight the centroid average, not just vertex position
512 center.x += triangleArea*PHYSAC_K*(position1.x + position2.x);
513 center.y += triangleArea*PHYSAC_K*(position1.y + position2.y);
515 float intx2 = position1.x*position1.x + position2.x*position1.x + position2.x*position2.x;
516 float inty2 = position1.y*position1.y + position2.y*position1.y + position2.y*position2.y;
517 inertia += (0.25f*PHYSAC_K*cross)*(intx2 + inty2);
520 center.x *= 1.0f/area;
521 center.y *= 1.0f/area;
523 // Translate vertices to centroid (make the centroid (0, 0) for the polygon in model space)
524 // Note: this is not really necessary
525 for (unsigned int i = 0; i < body->shape.vertexData.vertexCount; i++)
527 body->shape.vertexData.positions[i].x -= center.x;
528 body->shape.vertexData.positions[i].y -= center.y;
531 body->mass = density*area;
532 body->inverseMass = ((body->mass != 0.0f) ? 1.0f/body->mass : 0.0f);
533 body->inertia = density*inertia;
534 body->inverseInertia = ((body->inertia != 0.0f) ? 1.0f/body->inertia : 0.0f);
535 body->staticFriction = 0.4f;
536 body->dynamicFriction = 0.2f;
537 body->restitution = 0.0f;
538 body->useGravity = true;
539 body->isGrounded = false;
540 body->freezeOrient = false;
542 // Add new body to bodies pointers array and update bodies count
543 bodies[physicsBodiesCount] = body;
544 physicsBodiesCount++;
546 TRACELOG("[PHYSAC] Physic body created successfully (id: %i)\n", body->id);
548 else TRACELOG("[PHYSAC] Physics body could not be created, PHYSAC_MAX_BODIES reached\n");
553 // Adds a force to a physics body
554 PHYSACDEF void PhysicsAddForce(PhysicsBody body, Vector2 force)
556 if (body != NULL) body->force = MathVector2Add(body->force, force);
559 // Adds an angular force to a physics body
560 PHYSACDEF void PhysicsAddTorque(PhysicsBody body, float amount)
562 if (body != NULL) body->torque += amount;
565 // Shatters a polygon shape physics body to little physics bodies with explosion force
566 PHYSACDEF void PhysicsShatter(PhysicsBody body, Vector2 position, float force)
570 if (body->shape.type == PHYSICS_POLYGON)
572 PhysicsVertexData vertexData = body->shape.vertexData;
573 bool collision = false;
575 for (unsigned int i = 0; i < vertexData.vertexCount; i++)
577 Vector2 positionA = body->position;
578 Vector2 positionB = MathMatVector2Product(body->shape.transform, MathVector2Add(body->position, vertexData.positions[i]));
579 unsigned int nextIndex = (((i + 1) < vertexData.vertexCount) ? (i + 1) : 0);
580 Vector2 positionC = MathMatVector2Product(body->shape.transform, MathVector2Add(body->position, vertexData.positions[nextIndex]));
582 // Check collision between each triangle
583 float alpha = ((positionB.y - positionC.y)*(position.x - positionC.x) + (positionC.x - positionB.x)*(position.y - positionC.y))/
584 ((positionB.y - positionC.y)*(positionA.x - positionC.x) + (positionC.x - positionB.x)*(positionA.y - positionC.y));
586 float beta = ((positionC.y - positionA.y)*(position.x - positionC.x) + (positionA.x - positionC.x)*(position.y - positionC.y))/
587 ((positionB.y - positionC.y)*(positionA.x - positionC.x) + (positionC.x - positionB.x)*(positionA.y - positionC.y));
589 float gamma = 1.0f - alpha - beta;
591 if ((alpha > 0.0f) && (beta > 0.0f) & (gamma > 0.0f))
600 int count = vertexData.vertexCount;
601 Vector2 bodyPos = body->position;
602 Vector2 *vertices = (Vector2 *)PHYSAC_MALLOC(sizeof(Vector2)*count);
603 Matrix2x2 trans = body->shape.transform;
604 for (int i = 0; i < count; i++) vertices[i] = vertexData.positions[i];
606 // Destroy shattered physics body
607 DestroyPhysicsBody(body);
609 for (int i = 0; i < count; i++)
611 int nextIndex = (((i + 1) < count) ? (i + 1) : 0);
612 Vector2 center = MathTriangleBarycenter(vertices[i], vertices[nextIndex], PHYSAC_VECTOR_ZERO);
613 center = MathVector2Add(bodyPos, center);
614 Vector2 offset = MathVector2Subtract(center, bodyPos);
616 PhysicsBody body = CreatePhysicsBodyPolygon(center, 10, 3, 10); // Create polygon physics body with relevant values
618 PhysicsVertexData vertexData = { 0 };
619 vertexData.vertexCount = 3;
621 vertexData.positions[0] = MathVector2Subtract(vertices[i], offset);
622 vertexData.positions[1] = MathVector2Subtract(vertices[nextIndex], offset);
623 vertexData.positions[2] = MathVector2Subtract(position, center);
625 // Separate vertices to avoid unnecessary physics collisions
626 vertexData.positions[0].x *= 0.95f;
627 vertexData.positions[0].y *= 0.95f;
628 vertexData.positions[1].x *= 0.95f;
629 vertexData.positions[1].y *= 0.95f;
630 vertexData.positions[2].x *= 0.95f;
631 vertexData.positions[2].y *= 0.95f;
633 // Calculate polygon faces normals
634 for (unsigned int j = 0; j < vertexData.vertexCount; j++)
636 unsigned int nextVertex = (((j + 1) < vertexData.vertexCount) ? (j + 1) : 0);
637 Vector2 face = MathVector2Subtract(vertexData.positions[nextVertex], vertexData.positions[j]);
639 vertexData.normals[j] = CLITERAL(Vector2){ face.y, -face.x };
640 MathVector2Normalize(&vertexData.normals[j]);
643 // Apply computed vertex data to new physics body shape
644 body->shape.vertexData = vertexData;
645 body->shape.transform = trans;
647 // Calculate centroid and moment of inertia
648 center = PHYSAC_VECTOR_ZERO;
650 float inertia = 0.0f;
652 for (unsigned int j = 0; j < body->shape.vertexData.vertexCount; j++)
654 // Triangle vertices, third vertex implied as (0, 0)
655 Vector2 p1 = body->shape.vertexData.positions[j];
656 unsigned int nextVertex = (((j + 1) < body->shape.vertexData.vertexCount) ? (j + 1) : 0);
657 Vector2 p2 = body->shape.vertexData.positions[nextVertex];
659 float D = MathVector2CrossProduct(p1, p2);
660 float triangleArea = D/2;
662 area += triangleArea;
664 // Use area to weight the centroid average, not just vertex position
665 center.x += triangleArea*PHYSAC_K*(p1.x + p2.x);
666 center.y += triangleArea*PHYSAC_K*(p1.y + p2.y);
668 float intx2 = p1.x*p1.x + p2.x*p1.x + p2.x*p2.x;
669 float inty2 = p1.y*p1.y + p2.y*p1.y + p2.y*p2.y;
670 inertia += (0.25f*PHYSAC_K*D)*(intx2 + inty2);
673 center.x *= 1.0f/area;
674 center.y *= 1.0f/area;
677 body->inverseMass = ((body->mass != 0.0f) ? 1.0f/body->mass : 0.0f);
678 body->inertia = inertia;
679 body->inverseInertia = ((body->inertia != 0.0f) ? 1.0f/body->inertia : 0.0f);
681 // Calculate explosion force direction
682 Vector2 pointA = body->position;
683 Vector2 pointB = MathVector2Subtract(vertexData.positions[1], vertexData.positions[0]);
686 Vector2 forceDirection = MathVector2Subtract(MathVector2Add(pointA, MathVector2Add(vertexData.positions[0], pointB)), body->position);
687 MathVector2Normalize(&forceDirection);
688 forceDirection.x *= force;
689 forceDirection.y *= force;
691 // Apply force to new physics body
692 PhysicsAddForce(body, forceDirection);
695 PHYSAC_FREE(vertices);
699 else TRACELOG("[PHYSAC] WARNING: PhysicsShatter: NULL physic body\n");
702 // Returns the current amount of created physics bodies
703 PHYSACDEF int GetPhysicsBodiesCount(void)
705 return physicsBodiesCount;
708 // Returns a physics body of the bodies pool at a specific index
709 PHYSACDEF PhysicsBody GetPhysicsBody(int index)
711 PhysicsBody body = NULL;
713 if (index < (int)physicsBodiesCount)
715 body = bodies[index];
717 if (body == NULL) TRACELOG("[PHYSAC] WARNING: GetPhysicsBody: NULL physic body\n");
719 else TRACELOG("[PHYSAC] WARNING: Physic body index is out of bounds\n");
724 // Returns the physics body shape type (PHYSICS_CIRCLE or PHYSICS_POLYGON)
725 PHYSACDEF int GetPhysicsShapeType(int index)
729 if (index < (int)physicsBodiesCount)
731 PhysicsBody body = bodies[index];
733 if (body != NULL) result = body->shape.type;
734 else TRACELOG("[PHYSAC] WARNING: GetPhysicsShapeType: NULL physic body\n");
736 else TRACELOG("[PHYSAC] WARNING: Physic body index is out of bounds\n");
741 // Returns the amount of vertices of a physics body shape
742 PHYSACDEF int GetPhysicsShapeVerticesCount(int index)
746 if (index < (int)physicsBodiesCount)
748 PhysicsBody body = bodies[index];
752 switch (body->shape.type)
754 case PHYSICS_CIRCLE: result = PHYSAC_DEFAULT_CIRCLE_VERTICES; break;
755 case PHYSICS_POLYGON: result = body->shape.vertexData.vertexCount; break;
759 else TRACELOG("[PHYSAC] WARNING: GetPhysicsShapeVerticesCount: NULL physic body\n");
761 else TRACELOG("[PHYSAC] WARNING: Physic body index is out of bounds\n");
766 // Returns transformed position of a body shape (body position + vertex transformed position)
767 PHYSACDEF Vector2 GetPhysicsShapeVertex(PhysicsBody body, int vertex)
769 Vector2 position = { 0.0f, 0.0f };
773 switch (body->shape.type)
777 position.x = body->position.x + cosf(360.0f/PHYSAC_DEFAULT_CIRCLE_VERTICES*vertex*PHYSAC_DEG2RAD)*body->shape.radius;
778 position.y = body->position.y + sinf(360.0f/PHYSAC_DEFAULT_CIRCLE_VERTICES*vertex*PHYSAC_DEG2RAD)*body->shape.radius;
780 case PHYSICS_POLYGON:
782 PhysicsVertexData vertexData = body->shape.vertexData;
783 position = MathVector2Add(body->position, MathMatVector2Product(body->shape.transform, vertexData.positions[vertex]));
788 else TRACELOG("[PHYSAC] WARNING: GetPhysicsShapeVertex: NULL physic body\n");
793 // Sets physics body shape transform based on radians parameter
794 PHYSACDEF void SetPhysicsBodyRotation(PhysicsBody body, float radians)
798 body->orient = radians;
800 if (body->shape.type == PHYSICS_POLYGON) body->shape.transform = MathMatFromRadians(radians);
804 // Unitializes and destroys a physics body
805 PHYSACDEF void DestroyPhysicsBody(PhysicsBody body)
812 for (unsigned int i = 0; i < physicsBodiesCount; i++)
814 if (bodies[i]->id == id)
823 TRACELOG("[PHYSAC] WARNING: Requested body (id: %i) can not be found\n", id);
824 return; // Prevent access to index -1
827 // Free body allocated memory
829 usedMemory -= sizeof(PhysicsBodyData);
830 bodies[index] = NULL;
832 // Reorder physics bodies pointers array and its catched index
833 for (unsigned int i = index; i < physicsBodiesCount; i++)
835 if ((i + 1) < physicsBodiesCount) bodies[i] = bodies[i + 1];
838 // Update physics bodies count
839 physicsBodiesCount--;
841 TRACELOG("[PHYSAC] Physic body destroyed successfully (id: %i)\n", id);
843 else TRACELOG("[PHYSAC] WARNING: DestroyPhysicsBody: NULL physic body\n");
846 // Destroys created physics bodies and manifolds and resets global values
847 PHYSACDEF void ResetPhysics(void)
849 if (physicsBodiesCount > 0)
851 // Unitialize physics bodies dynamic memory allocations
852 for (int i = physicsBodiesCount - 1; i >= 0; i--)
854 PhysicsBody body = bodies[i];
860 usedMemory -= sizeof(PhysicsBodyData);
864 physicsBodiesCount = 0;
867 if (physicsManifoldsCount > 0)
869 // Unitialize physics manifolds dynamic memory allocations
870 for (int i = physicsManifoldsCount - 1; i >= 0; i--)
872 PhysicsManifold manifold = contacts[i];
874 if (manifold != NULL)
876 PHYSAC_FREE(manifold);
878 usedMemory -= sizeof(PhysicsManifoldData);
882 physicsManifoldsCount = 0;
885 TRACELOG("[PHYSAC] Physics module reseted successfully\n");
888 // Unitializes physics pointers and exits physics loop thread
889 PHYSACDEF void ClosePhysics(void)
891 // Unitialize physics manifolds dynamic memory allocations
892 if (physicsManifoldsCount > 0)
894 for (unsigned int i = physicsManifoldsCount - 1; i >= 0; i--)
895 DestroyPhysicsManifold(contacts[i]);
898 // Unitialize physics bodies dynamic memory allocations
899 if (physicsBodiesCount > 0)
901 for (unsigned int i = physicsBodiesCount - 1; i >= 0; i--)
902 DestroyPhysicsBody(bodies[i]);
906 if ((physicsBodiesCount > 0) || (usedMemory != 0))
908 TRACELOG("[PHYSAC] WARNING: Physics module closed with unallocated bodies (BODIES: %i, MEMORY: %i bytes)\n", physicsBodiesCount, usedMemory);
910 else if ((physicsManifoldsCount > 0) || (usedMemory != 0))
912 TRACELOG("[PHYSAC] WARNING: Pysics module closed with unallocated manifolds (MANIFOLDS: %i, MEMORY: %i bytes)\n", physicsManifoldsCount, usedMemory);
914 else TRACELOG("[PHYSAC] Physics module closed successfully\n");
917 //----------------------------------------------------------------------------------
918 // Module Internal Functions Definition
919 //----------------------------------------------------------------------------------
920 // Finds a valid index for a new physics body initialization
921 static int FindAvailableBodyIndex()
924 for (int i = 0; i < PHYSAC_MAX_BODIES; i++)
928 // Check if current id already exist in other physics body
929 for (unsigned int k = 0; k < physicsBodiesCount; k++)
931 if (bodies[k]->id == currentId)
938 // If it is not used, use it as new physics body id
939 if (currentId == (int)i)
949 // Creates a default polygon shape with max vertex distance from polygon pivot
950 static PhysicsVertexData CreateDefaultPolygon(float radius, int sides)
952 PhysicsVertexData data = { 0 };
953 data.vertexCount = sides;
955 // Calculate polygon vertices positions
956 for (unsigned int i = 0; i < data.vertexCount; i++)
958 data.positions[i].x = (float)cosf(360.0f/sides*i*PHYSAC_DEG2RAD)*radius;
959 data.positions[i].y = (float)sinf(360.0f/sides*i*PHYSAC_DEG2RAD)*radius;
962 // Calculate polygon faces normals
963 for (int i = 0; i < (int)data.vertexCount; i++)
965 int nextIndex = (((i + 1) < sides) ? (i + 1) : 0);
966 Vector2 face = MathVector2Subtract(data.positions[nextIndex], data.positions[i]);
968 data.normals[i] = CLITERAL(Vector2){ face.y, -face.x };
969 MathVector2Normalize(&data.normals[i]);
975 // Creates a rectangle polygon shape based on a min and max positions
976 static PhysicsVertexData CreateRectanglePolygon(Vector2 pos, Vector2 size)
978 PhysicsVertexData data = { 0 };
979 data.vertexCount = 4;
981 // Calculate polygon vertices positions
982 data.positions[0] = CLITERAL(Vector2){ pos.x + size.x/2, pos.y - size.y/2 };
983 data.positions[1] = CLITERAL(Vector2){ pos.x + size.x/2, pos.y + size.y/2 };
984 data.positions[2] = CLITERAL(Vector2){ pos.x - size.x/2, pos.y + size.y/2 };
985 data.positions[3] = CLITERAL(Vector2){ pos.x - size.x/2, pos.y - size.y/2 };
987 // Calculate polygon faces normals
988 for (unsigned int i = 0; i < data.vertexCount; i++)
990 int nextIndex = (((i + 1) < data.vertexCount) ? (i + 1) : 0);
991 Vector2 face = MathVector2Subtract(data.positions[nextIndex], data.positions[i]);
993 data.normals[i] = CLITERAL(Vector2){ face.y, -face.x };
994 MathVector2Normalize(&data.normals[i]);
1000 // Update physics step (dynamics, collisions and position corrections)
1001 void UpdatePhysicsStep(void)
1003 // Clear previous generated collisions information
1004 for (int i = (int)physicsManifoldsCount - 1; i >= 0; i--)
1006 PhysicsManifold manifold = contacts[i];
1007 if (manifold != NULL) DestroyPhysicsManifold(manifold);
1010 // Reset physics bodies grounded state
1011 for (unsigned int i = 0; i < physicsBodiesCount; i++)
1013 PhysicsBody body = bodies[i];
1014 body->isGrounded = false;
1017 // Generate new collision information
1018 for (unsigned int i = 0; i < physicsBodiesCount; i++)
1020 PhysicsBody bodyA = bodies[i];
1024 for (unsigned int j = i + 1; j < physicsBodiesCount; j++)
1026 PhysicsBody bodyB = bodies[j];
1030 if ((bodyA->inverseMass == 0) && (bodyB->inverseMass == 0)) continue;
1032 PhysicsManifold manifold = CreatePhysicsManifold(bodyA, bodyB);
1033 SolvePhysicsManifold(manifold);
1035 if (manifold->contactsCount > 0)
1037 // Create a new manifold with same information as previously solved manifold and add it to the manifolds pool last slot
1038 PhysicsManifold manifold = CreatePhysicsManifold(bodyA, bodyB);
1039 manifold->penetration = manifold->penetration;
1040 manifold->normal = manifold->normal;
1041 manifold->contacts[0] = manifold->contacts[0];
1042 manifold->contacts[1] = manifold->contacts[1];
1043 manifold->contactsCount = manifold->contactsCount;
1044 manifold->restitution = manifold->restitution;
1045 manifold->dynamicFriction = manifold->dynamicFriction;
1046 manifold->staticFriction = manifold->staticFriction;
1053 // Integrate forces to physics bodies
1054 for (unsigned int i = 0; i < physicsBodiesCount; i++)
1056 PhysicsBody body = bodies[i];
1057 if (body != NULL) IntegratePhysicsForces(body);
1060 // Initialize physics manifolds to solve collisions
1061 for (unsigned int i = 0; i < physicsManifoldsCount; i++)
1063 PhysicsManifold manifold = contacts[i];
1064 if (manifold != NULL) InitializePhysicsManifolds(manifold);
1067 // Integrate physics collisions impulses to solve collisions
1068 for (unsigned int i = 0; i < PHYSAC_COLLISION_ITERATIONS; i++)
1070 for (unsigned int j = 0; j < physicsManifoldsCount; j++)
1072 PhysicsManifold manifold = contacts[i];
1073 if (manifold != NULL) IntegratePhysicsImpulses(manifold);
1077 // Integrate velocity to physics bodies
1078 for (unsigned int i = 0; i < physicsBodiesCount; i++)
1080 PhysicsBody body = bodies[i];
1081 if (body != NULL) IntegratePhysicsVelocity(body);
1084 // Correct physics bodies positions based on manifolds collision information
1085 for (unsigned int i = 0; i < physicsManifoldsCount; i++)
1087 PhysicsManifold manifold = contacts[i];
1088 if (manifold != NULL) CorrectPhysicsPositions(manifold);
1091 // Clear physics bodies forces
1092 for (unsigned int i = 0; i < physicsBodiesCount; i++)
1094 PhysicsBody body = bodies[i];
1097 body->force = PHYSAC_VECTOR_ZERO;
1098 body->torque = 0.0f;
1103 // Update physics system
1104 // Physics steps are launched at a fixed time step if enabled
1105 PHYSACDEF void UpdatePhysics(void)
1107 #if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
1108 static double deltaTimeAccumulator = 0.0;
1110 // Calculate current time (ms)
1111 currentTime = GetCurrentTime();
1113 // Calculate current delta time (ms)
1114 const double delta = currentTime - startTime;
1116 // Store the time elapsed since the last frame began
1117 deltaTimeAccumulator += delta;
1119 // Fixed time stepping loop
1120 while (deltaTimeAccumulator >= deltaTime)
1122 UpdatePhysicsStep();
1123 deltaTimeAccumulator -= deltaTime;
1126 // Record the starting of this frame
1127 startTime = currentTime;
1129 UpdatePhysicsStep();
1133 PHYSACDEF void SetPhysicsTimeStep(double delta)
1138 // Finds a valid index for a new manifold initialization
1139 static int FindAvailableManifoldIndex()
1142 for (int i = 0; i < PHYSAC_MAX_MANIFOLDS; i++)
1146 // Check if current id already exist in other physics body
1147 for (unsigned int k = 0; k < physicsManifoldsCount; k++)
1149 if (contacts[k]->id == currentId)
1156 // If it is not used, use it as new physics body id
1167 // Creates a new physics manifold to solve collision
1168 static PhysicsManifold CreatePhysicsManifold(PhysicsBody a, PhysicsBody b)
1170 PhysicsManifold manifold = (PhysicsManifold)PHYSAC_MALLOC(sizeof(PhysicsManifoldData));
1171 usedMemory += sizeof(PhysicsManifoldData);
1173 int id = FindAvailableManifoldIndex();
1176 // Initialize new manifold with generic values
1178 manifold->bodyA = a;
1179 manifold->bodyB = b;
1180 manifold->penetration = 0;
1181 manifold->normal = PHYSAC_VECTOR_ZERO;
1182 manifold->contacts[0] = PHYSAC_VECTOR_ZERO;
1183 manifold->contacts[1] = PHYSAC_VECTOR_ZERO;
1184 manifold->contactsCount = 0;
1185 manifold->restitution = 0.0f;
1186 manifold->dynamicFriction = 0.0f;
1187 manifold->staticFriction = 0.0f;
1189 // Add new body to bodies pointers array and update bodies count
1190 contacts[physicsManifoldsCount] = manifold;
1191 physicsManifoldsCount++;
1193 else TRACELOG("[PHYSAC] Physic manifold could not be created, PHYSAC_MAX_MANIFOLDS reached\n");
1198 // Unitializes and destroys a physics manifold
1199 static void DestroyPhysicsManifold(PhysicsManifold manifold)
1201 if (manifold != NULL)
1203 int id = manifold->id;
1206 for (unsigned int i = 0; i < physicsManifoldsCount; i++)
1208 if (contacts[i]->id == id)
1215 if (index == -1) return; // Prevent access to index -1
1217 // Free manifold allocated memory
1218 PHYSAC_FREE(manifold);
1219 usedMemory -= sizeof(PhysicsManifoldData);
1220 contacts[index] = NULL;
1222 // Reorder physics manifolds pointers array and its catched index
1223 for (unsigned int i = index; i < physicsManifoldsCount; i++)
1225 if ((i + 1) < physicsManifoldsCount) contacts[i] = contacts[i + 1];
1228 // Update physics manifolds count
1229 physicsManifoldsCount--;
1231 else TRACELOG("[PHYSAC] WARNING: DestroyPhysicsManifold: NULL physic manifold\n");
1234 // Solves a created physics manifold between two physics bodies
1235 static void SolvePhysicsManifold(PhysicsManifold manifold)
1237 switch (manifold->bodyA->shape.type)
1239 case PHYSICS_CIRCLE:
1241 switch (manifold->bodyB->shape.type)
1243 case PHYSICS_CIRCLE: SolveCircleToCircle(manifold); break;
1244 case PHYSICS_POLYGON: SolveCircleToPolygon(manifold); break;
1248 case PHYSICS_POLYGON:
1250 switch (manifold->bodyB->shape.type)
1252 case PHYSICS_CIRCLE: SolvePolygonToCircle(manifold); break;
1253 case PHYSICS_POLYGON: SolvePolygonToPolygon(manifold); break;
1260 // Update physics body grounded state if normal direction is down and grounded state is not set yet in previous manifolds
1261 if (!manifold->bodyB->isGrounded) manifold->bodyB->isGrounded = (manifold->normal.y < 0);
1264 // Solves collision between two circle shape physics bodies
1265 static void SolveCircleToCircle(PhysicsManifold manifold)
1267 PhysicsBody bodyA = manifold->bodyA;
1268 PhysicsBody bodyB = manifold->bodyB;
1270 if ((bodyA == NULL) || (bodyB == NULL)) return;
1272 // Calculate translational vector, which is normal
1273 Vector2 normal = MathVector2Subtract(bodyB->position, bodyA->position);
1275 float distSqr = MathVector2SqrLen(normal);
1276 float radius = bodyA->shape.radius + bodyB->shape.radius;
1278 // Check if circles are not in contact
1279 if (distSqr >= radius*radius)
1281 manifold->contactsCount = 0;
1285 float distance = sqrtf(distSqr);
1286 manifold->contactsCount = 1;
1288 if (distance == 0.0f)
1290 manifold->penetration = bodyA->shape.radius;
1291 manifold->normal = CLITERAL(Vector2){ 1.0f, 0.0f };
1292 manifold->contacts[0] = bodyA->position;
1296 manifold->penetration = radius - distance;
1297 manifold->normal = CLITERAL(Vector2){ normal.x/distance, normal.y/distance }; // Faster than using MathVector2Normalize() due to sqrt is already performed
1298 manifold->contacts[0] = CLITERAL(Vector2){ manifold->normal.x*bodyA->shape.radius + bodyA->position.x, manifold->normal.y*bodyA->shape.radius + bodyA->position.y };
1301 // Update physics body grounded state if normal direction is down
1302 if (!bodyA->isGrounded) bodyA->isGrounded = (manifold->normal.y < 0);
1305 // Solves collision between a circle to a polygon shape physics bodies
1306 static void SolveCircleToPolygon(PhysicsManifold manifold)
1308 PhysicsBody bodyA = manifold->bodyA;
1309 PhysicsBody bodyB = manifold->bodyB;
1311 if ((bodyA == NULL) || (bodyB == NULL)) return;
1313 manifold->contactsCount = 0;
1315 // Transform circle center to polygon transform space
1316 Vector2 center = bodyA->position;
1317 center = MathMatVector2Product(MathMatTranspose(bodyB->shape.transform), MathVector2Subtract(center, bodyB->position));
1319 // Find edge with minimum penetration
1320 // It is the same concept as using support points in SolvePolygonToPolygon
1321 float separation = -PHYSAC_FLT_MAX;
1323 PhysicsVertexData vertexData = bodyB->shape.vertexData;
1325 for (unsigned int i = 0; i < vertexData.vertexCount; i++)
1327 float currentSeparation = MathVector2DotProduct(vertexData.normals[i], MathVector2Subtract(center, vertexData.positions[i]));
1329 if (currentSeparation > bodyA->shape.radius) return;
1331 if (currentSeparation > separation)
1333 separation = currentSeparation;
1338 // Grab face's vertices
1339 Vector2 v1 = vertexData.positions[faceNormal];
1340 int nextIndex = (((faceNormal + 1) < (int)vertexData.vertexCount) ? (faceNormal + 1) : 0);
1341 Vector2 v2 = vertexData.positions[nextIndex];
1343 // Check to see if center is within polygon
1344 if (separation < PHYSAC_EPSILON)
1346 manifold->contactsCount = 1;
1347 Vector2 normal = MathMatVector2Product(bodyB->shape.transform, vertexData.normals[faceNormal]);
1348 manifold->normal = CLITERAL(Vector2){ -normal.x, -normal.y };
1349 manifold->contacts[0] = CLITERAL(Vector2){ manifold->normal.x*bodyA->shape.radius + bodyA->position.x, manifold->normal.y*bodyA->shape.radius + bodyA->position.y };
1350 manifold->penetration = bodyA->shape.radius;
1354 // Determine which voronoi region of the edge center of circle lies within
1355 float dot1 = MathVector2DotProduct(MathVector2Subtract(center, v1), MathVector2Subtract(v2, v1));
1356 float dot2 = MathVector2DotProduct(MathVector2Subtract(center, v2), MathVector2Subtract(v1, v2));
1357 manifold->penetration = bodyA->shape.radius - separation;
1359 if (dot1 <= 0.0f) // Closest to v1
1361 if (MathVector2SqrDistance(center, v1) > bodyA->shape.radius*bodyA->shape.radius) return;
1363 manifold->contactsCount = 1;
1364 Vector2 normal = MathVector2Subtract(v1, center);
1365 normal = MathMatVector2Product(bodyB->shape.transform, normal);
1366 MathVector2Normalize(&normal);
1367 manifold->normal = normal;
1368 v1 = MathMatVector2Product(bodyB->shape.transform, v1);
1369 v1 = MathVector2Add(v1, bodyB->position);
1370 manifold->contacts[0] = v1;
1372 else if (dot2 <= 0.0f) // Closest to v2
1374 if (MathVector2SqrDistance(center, v2) > bodyA->shape.radius*bodyA->shape.radius) return;
1376 manifold->contactsCount = 1;
1377 Vector2 normal = MathVector2Subtract(v2, center);
1378 v2 = MathMatVector2Product(bodyB->shape.transform, v2);
1379 v2 = MathVector2Add(v2, bodyB->position);
1380 manifold->contacts[0] = v2;
1381 normal = MathMatVector2Product(bodyB->shape.transform, normal);
1382 MathVector2Normalize(&normal);
1383 manifold->normal = normal;
1385 else // Closest to face
1387 Vector2 normal = vertexData.normals[faceNormal];
1389 if (MathVector2DotProduct(MathVector2Subtract(center, v1), normal) > bodyA->shape.radius) return;
1391 normal = MathMatVector2Product(bodyB->shape.transform, normal);
1392 manifold->normal = CLITERAL(Vector2){ -normal.x, -normal.y };
1393 manifold->contacts[0] = CLITERAL(Vector2){ manifold->normal.x*bodyA->shape.radius + bodyA->position.x, manifold->normal.y*bodyA->shape.radius + bodyA->position.y };
1394 manifold->contactsCount = 1;
1398 // Solves collision between a polygon to a circle shape physics bodies
1399 static void SolvePolygonToCircle(PhysicsManifold manifold)
1401 PhysicsBody bodyA = manifold->bodyA;
1402 PhysicsBody bodyB = manifold->bodyB;
1404 if ((bodyA == NULL) || (bodyB == NULL)) return;
1406 manifold->bodyA = bodyB;
1407 manifold->bodyB = bodyA;
1408 SolveCircleToPolygon(manifold);
1410 manifold->normal.x *= -1.0f;
1411 manifold->normal.y *= -1.0f;
1414 // Solves collision between two polygons shape physics bodies
1415 static void SolvePolygonToPolygon(PhysicsManifold manifold)
1417 if ((manifold->bodyA == NULL) || (manifold->bodyB == NULL)) return;
1419 PhysicsShape bodyA = manifold->bodyA->shape;
1420 PhysicsShape bodyB = manifold->bodyB->shape;
1421 manifold->contactsCount = 0;
1423 // Check for separating axis with A shape's face planes
1425 float penetrationA = FindAxisLeastPenetration(&faceA, bodyA, bodyB);
1426 if (penetrationA >= 0.0f) return;
1428 // Check for separating axis with B shape's face planes
1430 float penetrationB = FindAxisLeastPenetration(&faceB, bodyB, bodyA);
1431 if (penetrationB >= 0.0f) return;
1433 int referenceIndex = 0;
1434 bool flip = false; // Always point from A shape to B shape
1436 PhysicsShape refPoly; // Reference
1437 PhysicsShape incPoly; // Incident
1439 // Determine which shape contains reference face
1440 // Checking bias range for penetration
1441 if (penetrationA >= (penetrationB*0.95f + penetrationA*0.01f))
1445 referenceIndex = faceA;
1451 referenceIndex = faceB;
1455 // World space incident face
1456 Vector2 incidentFace[2];
1457 FindIncidentFace(&incidentFace[0], &incidentFace[1], refPoly, incPoly, referenceIndex);
1459 // Setup reference face vertices
1460 PhysicsVertexData refData = refPoly.vertexData;
1461 Vector2 v1 = refData.positions[referenceIndex];
1462 referenceIndex = (((referenceIndex + 1) < (int)refData.vertexCount) ? (referenceIndex + 1) : 0);
1463 Vector2 v2 = refData.positions[referenceIndex];
1465 // Transform vertices to world space
1466 v1 = MathMatVector2Product(refPoly.transform, v1);
1467 v1 = MathVector2Add(v1, refPoly.body->position);
1468 v2 = MathMatVector2Product(refPoly.transform, v2);
1469 v2 = MathVector2Add(v2, refPoly.body->position);
1471 // Calculate reference face side normal in world space
1472 Vector2 sidePlaneNormal = MathVector2Subtract(v2, v1);
1473 MathVector2Normalize(&sidePlaneNormal);
1476 Vector2 refFaceNormal = { sidePlaneNormal.y, -sidePlaneNormal.x };
1477 float refC = MathVector2DotProduct(refFaceNormal, v1);
1478 float negSide = MathVector2DotProduct(sidePlaneNormal, v1)*-1;
1479 float posSide = MathVector2DotProduct(sidePlaneNormal, v2);
1481 // MathVector2Clip incident face to reference face side planes (due to floating point error, possible to not have required points
1482 if (MathVector2Clip(CLITERAL(Vector2){ -sidePlaneNormal.x, -sidePlaneNormal.y }, &incidentFace[0], &incidentFace[1], negSide) < 2) return;
1483 if (MathVector2Clip(sidePlaneNormal, &incidentFace[0], &incidentFace[1], posSide) < 2) return;
1485 // Flip normal if required
1486 manifold->normal = (flip ? CLITERAL(Vector2){ -refFaceNormal.x, -refFaceNormal.y } : refFaceNormal);
1488 // Keep points behind reference face
1489 int currentPoint = 0; // MathVector2Clipped points behind reference face
1490 float separation = MathVector2DotProduct(refFaceNormal, incidentFace[0]) - refC;
1491 if (separation <= 0.0f)
1493 manifold->contacts[currentPoint] = incidentFace[0];
1494 manifold->penetration = -separation;
1497 else manifold->penetration = 0.0f;
1499 separation = MathVector2DotProduct(refFaceNormal, incidentFace[1]) - refC;
1501 if (separation <= 0.0f)
1503 manifold->contacts[currentPoint] = incidentFace[1];
1504 manifold->penetration += -separation;
1507 // Calculate total penetration average
1508 manifold->penetration /= currentPoint;
1511 manifold->contactsCount = currentPoint;
1514 // Integrates physics forces into velocity
1515 static void IntegratePhysicsForces(PhysicsBody body)
1517 if ((body == NULL) || (body->inverseMass == 0.0f) || !body->enabled) return;
1519 body->velocity.x += (float)((body->force.x*body->inverseMass)*(deltaTime/2.0));
1520 body->velocity.y += (float)((body->force.y*body->inverseMass)*(deltaTime/2.0));
1522 if (body->useGravity)
1524 body->velocity.x += (float)(gravityForce.x*(deltaTime/1000/2.0));
1525 body->velocity.y += (float)(gravityForce.y*(deltaTime/1000/2.0));
1528 if (!body->freezeOrient) body->angularVelocity += (float)(body->torque*body->inverseInertia*(deltaTime/2.0));
1531 // Initializes physics manifolds to solve collisions
1532 static void InitializePhysicsManifolds(PhysicsManifold manifold)
1534 PhysicsBody bodyA = manifold->bodyA;
1535 PhysicsBody bodyB = manifold->bodyB;
1537 if ((bodyA == NULL) || (bodyB == NULL)) return;
1539 // Calculate average restitution, static and dynamic friction
1540 manifold->restitution = sqrtf(bodyA->restitution*bodyB->restitution);
1541 manifold->staticFriction = sqrtf(bodyA->staticFriction*bodyB->staticFriction);
1542 manifold->dynamicFriction = sqrtf(bodyA->dynamicFriction*bodyB->dynamicFriction);
1544 for (unsigned int i = 0; i < manifold->contactsCount; i++)
1546 // Caculate radius from center of mass to contact
1547 Vector2 radiusA = MathVector2Subtract(manifold->contacts[i], bodyA->position);
1548 Vector2 radiusB = MathVector2Subtract(manifold->contacts[i], bodyB->position);
1550 Vector2 crossA = MathVector2Product(radiusA, bodyA->angularVelocity);
1551 Vector2 crossB = MathVector2Product(radiusB, bodyB->angularVelocity);
1553 Vector2 radiusV = { 0.0f, 0.0f };
1554 radiusV.x = bodyB->velocity.x + crossB.x - bodyA->velocity.x - crossA.x;
1555 radiusV.y = bodyB->velocity.y + crossB.y - bodyA->velocity.y - crossA.y;
1557 // Determine if we should perform a resting collision or not;
1558 // The idea is if the only thing moving this object is gravity, then the collision should be performed without any restitution
1559 if (MathVector2SqrLen(radiusV) < (MathVector2SqrLen(CLITERAL(Vector2){ (float)(gravityForce.x*deltaTime/1000), (float)(gravityForce.y*deltaTime/1000) }) + PHYSAC_EPSILON)) manifold->restitution = 0;
1563 // Integrates physics collisions impulses to solve collisions
1564 static void IntegratePhysicsImpulses(PhysicsManifold manifold)
1566 PhysicsBody bodyA = manifold->bodyA;
1567 PhysicsBody bodyB = manifold->bodyB;
1569 if ((bodyA == NULL) || (bodyB == NULL)) return;
1571 // Early out and positional correct if both objects have infinite mass
1572 if (fabs(bodyA->inverseMass + bodyB->inverseMass) <= PHYSAC_EPSILON)
1574 bodyA->velocity = PHYSAC_VECTOR_ZERO;
1575 bodyB->velocity = PHYSAC_VECTOR_ZERO;
1579 for (unsigned int i = 0; i < manifold->contactsCount; i++)
1581 // Calculate radius from center of mass to contact
1582 Vector2 radiusA = MathVector2Subtract(manifold->contacts[i], bodyA->position);
1583 Vector2 radiusB = MathVector2Subtract(manifold->contacts[i], bodyB->position);
1585 // Calculate relative velocity
1586 Vector2 radiusV = { 0.0f, 0.0f };
1587 radiusV.x = bodyB->velocity.x + MathVector2Product(radiusB, bodyB->angularVelocity).x - bodyA->velocity.x - MathVector2Product(radiusA, bodyA->angularVelocity).x;
1588 radiusV.y = bodyB->velocity.y + MathVector2Product(radiusB, bodyB->angularVelocity).y - bodyA->velocity.y - MathVector2Product(radiusA, bodyA->angularVelocity).y;
1590 // Relative velocity along the normal
1591 float contactVelocity = MathVector2DotProduct(radiusV, manifold->normal);
1593 // Do not resolve if velocities are separating
1594 if (contactVelocity > 0.0f) return;
1596 float raCrossN = MathVector2CrossProduct(radiusA, manifold->normal);
1597 float rbCrossN = MathVector2CrossProduct(radiusB, manifold->normal);
1599 float inverseMassSum = bodyA->inverseMass + bodyB->inverseMass + (raCrossN*raCrossN)*bodyA->inverseInertia + (rbCrossN*rbCrossN)*bodyB->inverseInertia;
1601 // Calculate impulse scalar value
1602 float impulse = -(1.0f + manifold->restitution)*contactVelocity;
1603 impulse /= inverseMassSum;
1604 impulse /= (float)manifold->contactsCount;
1606 // Apply impulse to each physics body
1607 Vector2 impulseV = { manifold->normal.x*impulse, manifold->normal.y*impulse };
1611 bodyA->velocity.x += bodyA->inverseMass*(-impulseV.x);
1612 bodyA->velocity.y += bodyA->inverseMass*(-impulseV.y);
1613 if (!bodyA->freezeOrient) bodyA->angularVelocity += bodyA->inverseInertia*MathVector2CrossProduct(radiusA, CLITERAL(Vector2){ -impulseV.x, -impulseV.y });
1618 bodyB->velocity.x += bodyB->inverseMass*(impulseV.x);
1619 bodyB->velocity.y += bodyB->inverseMass*(impulseV.y);
1620 if (!bodyB->freezeOrient) bodyB->angularVelocity += bodyB->inverseInertia*MathVector2CrossProduct(radiusB, impulseV);
1623 // Apply friction impulse to each physics body
1624 radiusV.x = bodyB->velocity.x + MathVector2Product(radiusB, bodyB->angularVelocity).x - bodyA->velocity.x - MathVector2Product(radiusA, bodyA->angularVelocity).x;
1625 radiusV.y = bodyB->velocity.y + MathVector2Product(radiusB, bodyB->angularVelocity).y - bodyA->velocity.y - MathVector2Product(radiusA, bodyA->angularVelocity).y;
1627 Vector2 tangent = { radiusV.x - (manifold->normal.x*MathVector2DotProduct(radiusV, manifold->normal)), radiusV.y - (manifold->normal.y*MathVector2DotProduct(radiusV, manifold->normal)) };
1628 MathVector2Normalize(&tangent);
1630 // Calculate impulse tangent magnitude
1631 float impulseTangent = -MathVector2DotProduct(radiusV, tangent);
1632 impulseTangent /= inverseMassSum;
1633 impulseTangent /= (float)manifold->contactsCount;
1635 float absImpulseTangent = (float)fabs(impulseTangent);
1637 // Don't apply tiny friction impulses
1638 if (absImpulseTangent <= PHYSAC_EPSILON) return;
1640 // Apply coulumb's law
1641 Vector2 tangentImpulse = { 0.0f, 0.0f };
1642 if (absImpulseTangent < impulse*manifold->staticFriction) tangentImpulse = CLITERAL(Vector2){ tangent.x*impulseTangent, tangent.y*impulseTangent };
1643 else tangentImpulse = CLITERAL(Vector2){ tangent.x*-impulse*manifold->dynamicFriction, tangent.y*-impulse*manifold->dynamicFriction };
1645 // Apply friction impulse
1648 bodyA->velocity.x += bodyA->inverseMass*(-tangentImpulse.x);
1649 bodyA->velocity.y += bodyA->inverseMass*(-tangentImpulse.y);
1651 if (!bodyA->freezeOrient) bodyA->angularVelocity += bodyA->inverseInertia*MathVector2CrossProduct(radiusA, CLITERAL(Vector2){ -tangentImpulse.x, -tangentImpulse.y });
1656 bodyB->velocity.x += bodyB->inverseMass*(tangentImpulse.x);
1657 bodyB->velocity.y += bodyB->inverseMass*(tangentImpulse.y);
1659 if (!bodyB->freezeOrient) bodyB->angularVelocity += bodyB->inverseInertia*MathVector2CrossProduct(radiusB, tangentImpulse);
1664 // Integrates physics velocity into position and forces
1665 static void IntegratePhysicsVelocity(PhysicsBody body)
1667 if ((body == NULL) ||!body->enabled) return;
1669 body->position.x += (float)(body->velocity.x*deltaTime);
1670 body->position.y += (float)(body->velocity.y*deltaTime);
1672 if (!body->freezeOrient) body->orient += (float)(body->angularVelocity*deltaTime);
1673 body->shape.transform = MathMatFromRadians(body->orient);
1675 IntegratePhysicsForces(body);
1678 // Corrects physics bodies positions based on manifolds collision information
1679 static void CorrectPhysicsPositions(PhysicsManifold manifold)
1681 PhysicsBody bodyA = manifold->bodyA;
1682 PhysicsBody bodyB = manifold->bodyB;
1684 if ((bodyA == NULL) || (bodyB == NULL)) return;
1686 Vector2 correction = { 0.0f, 0.0f };
1687 correction.x = (PHYSAC_MAX(manifold->penetration - PHYSAC_PENETRATION_ALLOWANCE, 0.0f)/(bodyA->inverseMass + bodyB->inverseMass))*manifold->normal.x*PHYSAC_PENETRATION_CORRECTION;
1688 correction.y = (PHYSAC_MAX(manifold->penetration - PHYSAC_PENETRATION_ALLOWANCE, 0.0f)/(bodyA->inverseMass + bodyB->inverseMass))*manifold->normal.y*PHYSAC_PENETRATION_CORRECTION;
1692 bodyA->position.x -= correction.x*bodyA->inverseMass;
1693 bodyA->position.y -= correction.y*bodyA->inverseMass;
1698 bodyB->position.x += correction.x*bodyB->inverseMass;
1699 bodyB->position.y += correction.y*bodyB->inverseMass;
1703 // Returns the extreme point along a direction within a polygon
1704 static Vector2 GetSupport(PhysicsShape shape, Vector2 dir)
1706 float bestProjection = -PHYSAC_FLT_MAX;
1707 Vector2 bestVertex = { 0.0f, 0.0f };
1708 PhysicsVertexData data = shape.vertexData;
1710 for (unsigned int i = 0; i < data.vertexCount; i++)
1712 Vector2 vertex = data.positions[i];
1713 float projection = MathVector2DotProduct(vertex, dir);
1715 if (projection > bestProjection)
1717 bestVertex = vertex;
1718 bestProjection = projection;
1725 // Finds polygon shapes axis least penetration
1726 static float FindAxisLeastPenetration(int *faceIndex, PhysicsShape shapeA, PhysicsShape shapeB)
1728 float bestDistance = -PHYSAC_FLT_MAX;
1731 PhysicsVertexData dataA = shapeA.vertexData;
1732 //PhysicsVertexData dataB = shapeB.vertexData;
1734 for (unsigned int i = 0; i < dataA.vertexCount; i++)
1736 // Retrieve a face normal from A shape
1737 Vector2 normal = dataA.normals[i];
1738 Vector2 transNormal = MathMatVector2Product(shapeA.transform, normal);
1740 // Transform face normal into B shape's model space
1741 Matrix2x2 buT = MathMatTranspose(shapeB.transform);
1742 normal = MathMatVector2Product(buT, transNormal);
1744 // Retrieve support point from B shape along -n
1745 Vector2 support = GetSupport(shapeB, CLITERAL(Vector2){ -normal.x, -normal.y });
1747 // Retrieve vertex on face from A shape, transform into B shape's model space
1748 Vector2 vertex = dataA.positions[i];
1749 vertex = MathMatVector2Product(shapeA.transform, vertex);
1750 vertex = MathVector2Add(vertex, shapeA.body->position);
1751 vertex = MathVector2Subtract(vertex, shapeB.body->position);
1752 vertex = MathMatVector2Product(buT, vertex);
1754 // Compute penetration distance in B shape's model space
1755 float distance = MathVector2DotProduct(normal, MathVector2Subtract(support, vertex));
1757 // Store greatest distance
1758 if (distance > bestDistance)
1760 bestDistance = distance;
1765 *faceIndex = bestIndex;
1766 return bestDistance;
1769 // Finds two polygon shapes incident face
1770 static void FindIncidentFace(Vector2 *v0, Vector2 *v1, PhysicsShape ref, PhysicsShape inc, int index)
1772 PhysicsVertexData refData = ref.vertexData;
1773 PhysicsVertexData incData = inc.vertexData;
1775 Vector2 referenceNormal = refData.normals[index];
1777 // Calculate normal in incident's frame of reference
1778 referenceNormal = MathMatVector2Product(ref.transform, referenceNormal); // To world space
1779 referenceNormal = MathMatVector2Product(MathMatTranspose(inc.transform), referenceNormal); // To incident's model space
1781 // Find most anti-normal face on polygon
1782 int incidentFace = 0;
1783 float minDot = PHYSAC_FLT_MAX;
1785 for (unsigned int i = 0; i < incData.vertexCount; i++)
1787 float dot = MathVector2DotProduct(referenceNormal, incData.normals[i]);
1796 // Assign face vertices for incident face
1797 *v0 = MathMatVector2Product(inc.transform, incData.positions[incidentFace]);
1798 *v0 = MathVector2Add(*v0, inc.body->position);
1799 incidentFace = (((incidentFace + 1) < (int)incData.vertexCount) ? (incidentFace + 1) : 0);
1800 *v1 = MathMatVector2Product(inc.transform, incData.positions[incidentFace]);
1801 *v1 = MathVector2Add(*v1, inc.body->position);
1804 // Returns clipping value based on a normal and two faces
1805 static int MathVector2Clip(Vector2 normal, Vector2 *faceA, Vector2 *faceB, float clip)
1808 Vector2 out[2] = { *faceA, *faceB };
1810 // Retrieve distances from each endpoint to the line
1811 float distanceA = MathVector2DotProduct(normal, *faceA) - clip;
1812 float distanceB = MathVector2DotProduct(normal, *faceB) - clip;
1814 // If negative (behind plane)
1815 if (distanceA <= 0.0f) out[sp++] = *faceA;
1816 if (distanceB <= 0.0f) out[sp++] = *faceB;
1818 // If the points are on different sides of the plane
1819 if ((distanceA*distanceB) < 0.0f)
1821 // Push intersection point
1822 float alpha = distanceA/(distanceA - distanceB);
1824 Vector2 delta = MathVector2Subtract(*faceB, *faceA);
1827 out[sp] = MathVector2Add(out[sp], delta);
1831 // Assign the new converted values
1838 // Returns the barycenter of a triangle given by 3 points
1839 static Vector2 MathTriangleBarycenter(Vector2 v1, Vector2 v2, Vector2 v3)
1841 Vector2 result = { 0.0f, 0.0f };
1843 result.x = (v1.x + v2.x + v3.x)/3;
1844 result.y = (v1.y + v2.y + v3.y)/3;
1849 #if !defined(PHYSAC_AVOID_TIMMING_SYSTEM)
1850 // Initializes hi-resolution MONOTONIC timer
1851 static void InitTimer(void)
1854 QueryPerformanceFrequency((unsigned long long int *) &frequency);
1857 #if defined(__EMSCRIPTEN__) || defined(__linux__)
1858 struct timespec now;
1859 if (clock_gettime(CLOCK_MONOTONIC, &now) == 0) frequency = 1000000000;
1862 #if defined(__APPLE__)
1863 mach_timebase_info_data_t timebase;
1864 mach_timebase_info(&timebase);
1865 frequency = (timebase.denom*1e9)/timebase.numer;
1868 baseClockTicks = (double)GetClockTicks(); // Get MONOTONIC clock time offset
1869 startTime = GetCurrentTime(); // Get current time in milliseconds
1872 // Get hi-res MONOTONIC time measure in clock ticks
1873 static unsigned long long int GetClockTicks(void)
1875 unsigned long long int value = 0;
1878 QueryPerformanceCounter((unsigned long long int *) &value);
1881 #if defined(__linux__)
1882 struct timespec now;
1883 clock_gettime(CLOCK_MONOTONIC, &now);
1884 value = (unsigned long long int)now.tv_sec*(unsigned long long int)1000000000 + (unsigned long long int)now.tv_nsec;
1887 #if defined(__APPLE__)
1888 value = mach_absolute_time();
1894 // Get current time in milliseconds
1895 static double GetCurrentTime(void)
1897 return (double)(GetClockTicks() - baseClockTicks)/frequency*1000;
1899 #endif // !PHYSAC_AVOID_TIMMING_SYSTEM
1902 // Returns the cross product of a vector and a value
1903 static inline Vector2 MathVector2Product(Vector2 vector, float value)
1905 Vector2 result = { -value*vector.y, value*vector.x };
1909 // Returns the cross product of two vectors
1910 static inline float MathVector2CrossProduct(Vector2 v1, Vector2 v2)
1912 return (v1.x*v2.y - v1.y*v2.x);
1915 // Returns the len square root of a vector
1916 static inline float MathVector2SqrLen(Vector2 vector)
1918 return (vector.x*vector.x + vector.y*vector.y);
1921 // Returns the dot product of two vectors
1922 static inline float MathVector2DotProduct(Vector2 v1, Vector2 v2)
1924 return (v1.x*v2.x + v1.y*v2.y);
1927 // Returns the square root of distance between two vectors
1928 static inline float MathVector2SqrDistance(Vector2 v1, Vector2 v2)
1930 Vector2 dir = MathVector2Subtract(v1, v2);
1931 return MathVector2DotProduct(dir, dir);
1934 // Returns the normalized values of a vector
1935 static void MathVector2Normalize(Vector2 *vector)
1937 float length, ilength;
1939 Vector2 aux = *vector;
1940 length = sqrtf(aux.x*aux.x + aux.y*aux.y);
1942 if (length == 0) length = 1.0f;
1944 ilength = 1.0f/length;
1946 vector->x *= ilength;
1947 vector->y *= ilength;
1950 // Returns the sum of two given vectors
1951 static inline Vector2 MathVector2Add(Vector2 v1, Vector2 v2)
1953 Vector2 result = { v1.x + v2.x, v1.y + v2.y };
1957 // Returns the subtract of two given vectors
1958 static inline Vector2 MathVector2Subtract(Vector2 v1, Vector2 v2)
1960 Vector2 result = { v1.x - v2.x, v1.y - v2.y };
1964 // Creates a matrix 2x2 from a given radians value
1965 static Matrix2x2 MathMatFromRadians(float radians)
1967 float cos = cosf(radians);
1968 float sin = sinf(radians);
1970 Matrix2x2 result = { cos, -sin, sin, cos };
1974 // Returns the transpose of a given matrix 2x2
1975 static inline Matrix2x2 MathMatTranspose(Matrix2x2 matrix)
1977 Matrix2x2 result = { matrix.m00, matrix.m10, matrix.m01, matrix.m11 };
1981 // Multiplies a vector by a matrix 2x2
1982 static inline Vector2 MathMatVector2Product(Matrix2x2 matrix, Vector2 vector)
1984 Vector2 result = { matrix.m00*vector.x + matrix.m01*vector.y, matrix.m10*vector.x + matrix.m11*vector.y };
1988 #endif // PHYSAC_IMPLEMENTATION