Justin Stephens / Physac-MBED

MBED port of the Physacs library

physac.h@1:ebc0214989c0, 20 months ago (annotated)

Committer:
jstephens78
Date:
Mon Dec 05 19:27:37 2022 +0000
Revision:
1:ebc0214989c0
Parent:
0:e39efa4f4f58 
Update README, add MBED example

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jstephens78 0:e39efa4f4f58 1 /**********************************************************************************************
jstephens78 0:e39efa4f4f58 2 *
jstephens78 0:e39efa4f4f58 3 * Physac v1.1 - 2D Physics library for videogames
jstephens78 0:e39efa4f4f58 4 *
jstephens78 0:e39efa4f4f58 5 * DESCRIPTION:
jstephens78 0:e39efa4f4f58 6 *
jstephens78 0:e39efa4f4f58 7 * Physac is a small 2D physics library written in pure C. The engine uses a fixed time-step thread loop
jstephens78 0:e39efa4f4f58 8 * to simluate physics. A physics step contains the following phases: get collision information,
jstephens78 0:e39efa4f4f58 9 * apply dynamics, collision solving and position correction. It uses a very simple struct for physic
jstephens78 0:e39efa4f4f58 10 * bodies with a position vector to be used in any 3D rendering API.
jstephens78 0:e39efa4f4f58 11 *
jstephens78 0:e39efa4f4f58 12 * CONFIGURATION:
jstephens78 0:e39efa4f4f58 13 *
jstephens78 0:e39efa4f4f58 14 * #define PHYSAC_IMPLEMENTATION
jstephens78 0:e39efa4f4f58 15 * Generates the implementation of the library into the included file.
jstephens78 0:e39efa4f4f58 16 * If not defined, the library is in header only mode and can be included in other headers
jstephens78 0:e39efa4f4f58 17 * or source files without problems. But only ONE file should hold the implementation.
jstephens78 0:e39efa4f4f58 18 *
jstephens78 0:e39efa4f4f58 19 * #define PHYSAC_STATIC (defined by default)
jstephens78 0:e39efa4f4f58 20 * The generated implementation will stay private inside implementation file and all
jstephens78 0:e39efa4f4f58 21 * internal symbols and functions will only be visible inside that file.
jstephens78 0:e39efa4f4f58 22 *
jstephens78 0:e39efa4f4f58 23 * #define PHYSAC_NO_THREADS
jstephens78 0:e39efa4f4f58 24 * The generated implementation won't include pthread library and user must create a secondary thread to call PhysicsThread().
jstephens78 0:e39efa4f4f58 25 * It is so important that the thread where PhysicsThread() is called must not have v-sync or any other CPU limitation.
jstephens78 0:e39efa4f4f58 26 *
jstephens78 0:e39efa4f4f58 27 * #define PHYSAC_STANDALONE
jstephens78 0:e39efa4f4f58 28 * Avoid raylib.h header inclusion in this file. Data types defined on raylib are defined
jstephens78 0:e39efa4f4f58 29 * internally in the library and input management and drawing functions must be provided by
jstephens78 0:e39efa4f4f58 30 * the user (check library implementation for further details).
jstephens78 0:e39efa4f4f58 31 *
jstephens78 0:e39efa4f4f58 32 * #define PHYSAC_DEBUG
jstephens78 0:e39efa4f4f58 33 * Traces log messages when creating and destroying physics bodies and detects errors in physics
jstephens78 0:e39efa4f4f58 34 * calculations and reference exceptions; it is useful for debug purposes
jstephens78 0:e39efa4f4f58 35 *
jstephens78 0:e39efa4f4f58 36 * #define PHYSAC_MALLOC()
jstephens78 0:e39efa4f4f58 37 * #define PHYSAC_FREE()
jstephens78 0:e39efa4f4f58 38 * You can define your own malloc/free implementation replacing stdlib.h malloc()/free() functions.
jstephens78 0:e39efa4f4f58 39 * Otherwise it will include stdlib.h and use the C standard library malloc()/free() function.
jstephens78 0:e39efa4f4f58 40 *
jstephens78 0:e39efa4f4f58 41 *
jstephens78 0:e39efa4f4f58 42 * NOTE 1: Physac requires multi-threading, when InitPhysics() a second thread is created to manage physics calculations.
jstephens78 0:e39efa4f4f58 43 * NOTE 2: Physac requires static C library linkage to avoid dependency on MinGW DLL (-static -lpthread)
jstephens78 0:e39efa4f4f58 44 *
jstephens78 0:e39efa4f4f58 45 * Use the following code to compile:
jstephens78 0:e39efa4f4f58 46 * gcc -o $(NAME_PART).exe $(FILE_NAME) -s -static -lraylib -lpthread -lopengl32 -lgdi32 -lwinmm -std=c99
jstephens78 0:e39efa4f4f58 47 *
jstephens78 0:e39efa4f4f58 48 * VERY THANKS TO:
jstephens78 0:e39efa4f4f58 49 * - raysan5: helped with library design
jstephens78 0:e39efa4f4f58 50 * - ficoos: added support for Linux
jstephens78 0:e39efa4f4f58 51 * - R8D8: added support for Linux
jstephens78 0:e39efa4f4f58 52 * - jubalh: fixed implementation of time calculations
jstephens78 0:e39efa4f4f58 53 * - a3f: fixed implementation of time calculations
jstephens78 0:e39efa4f4f58 54 * - define-private-public: added support for OSX
jstephens78 0:e39efa4f4f58 55 * - pamarcos: fixed implementation of physics steps
jstephens78 0:e39efa4f4f58 56 * - noshbar: fixed some memory leaks
jstephens78 0:e39efa4f4f58 57 *
jstephens78 0:e39efa4f4f58 58 *
jstephens78 0:e39efa4f4f58 59 * LICENSE: zlib/libpng
jstephens78 0:e39efa4f4f58 60 *
jstephens78 0:e39efa4f4f58 61 * Copyright (c) 2016-2020 Victor Fisac (github: @victorfisac)
jstephens78 0:e39efa4f4f58 62 *
jstephens78 0:e39efa4f4f58 63 * This software is provided "as-is", without any express or implied warranty. In no event
jstephens78 0:e39efa4f4f58 64 * will the authors be held liable for any damages arising from the use of this software.
jstephens78 0:e39efa4f4f58 65 *
jstephens78 0:e39efa4f4f58 66 * Permission is granted to anyone to use this software for any purpose, including commercial
jstephens78 0:e39efa4f4f58 67 * applications, and to alter it and redistribute it freely, subject to the following restrictions:
jstephens78 0:e39efa4f4f58 68 *
jstephens78 0:e39efa4f4f58 69 * 1. The origin of this software must not be misrepresented; you must not claim that you
jstephens78 0:e39efa4f4f58 70 * wrote the original software. If you use this software in a product, an acknowledgment
jstephens78 0:e39efa4f4f58 71 * in the product documentation would be appreciated but is not required.
jstephens78 0:e39efa4f4f58 72 *
jstephens78 0:e39efa4f4f58 73 * 2. Altered source versions must be plainly marked as such, and must not be misrepresented
jstephens78 0:e39efa4f4f58 74 * as being the original software.
jstephens78 0:e39efa4f4f58 75 *
jstephens78 0:e39efa4f4f58 76 * 3. This notice may not be removed or altered from any source distribution.
jstephens78 0:e39efa4f4f58 77 *
jstephens78 0:e39efa4f4f58 78 **********************************************************************************************/
jstephens78 0:e39efa4f4f58 79
jstephens78 0:e39efa4f4f58 80 #if !defined(PHYSAC_H)
jstephens78 0:e39efa4f4f58 81 #define PHYSAC_H
jstephens78 0:e39efa4f4f58 82
jstephens78 0:e39efa4f4f58 83 // #define PHYSAC_STATIC
jstephens78 0:e39efa4f4f58 84 // #define PHYSAC_NO_THREADS
jstephens78 0:e39efa4f4f58 85 // #define PHYSAC_STANDALONE
jstephens78 0:e39efa4f4f58 86 // #define PHYSAC_DEBUG
jstephens78 0:e39efa4f4f58 87
jstephens78 0:e39efa4f4f58 88 #if defined(PHYSAC_STATIC)
jstephens78 0:e39efa4f4f58 89 #define PHYSACDEF static // Functions just visible to module including this file
jstephens78 0:e39efa4f4f58 90 #else
jstephens78 0:e39efa4f4f58 91 #if defined(__cplusplus)
jstephens78 0:e39efa4f4f58 92 #define PHYSACDEF extern "C" // Functions visible from other files (no name mangling of functions in C++)
jstephens78 0:e39efa4f4f58 93 #else
jstephens78 0:e39efa4f4f58 94 #define PHYSACDEF extern // Functions visible from other files
jstephens78 0:e39efa4f4f58 95 #endif
jstephens78 0:e39efa4f4f58 96 #endif
jstephens78 0:e39efa4f4f58 97
jstephens78 0:e39efa4f4f58 98 //----------------------------------------------------------------------------------
jstephens78 0:e39efa4f4f58 99 // Defines and Macros
jstephens78 0:e39efa4f4f58 100 //----------------------------------------------------------------------------------
jstephens78 1:ebc0214989c0 101 #define PHYSAC_MAX_BODIES 32
jstephens78 0:e39efa4f4f58 102 #define PHYSAC_MAX_MANIFOLDS 256
jstephens78 0:e39efa4f4f58 103 #define PHYSAC_MAX_VERTICES 4
jstephens78 1:ebc0214989c0 104 #define PHYSAC_CIRCLE_VERTICES 12
jstephens78 0:e39efa4f4f58 105
jstephens78 0:e39efa4f4f58 106 #define PHYSAC_COLLISION_ITERATIONS 100
jstephens78 0:e39efa4f4f58 107 #define PHYSAC_PENETRATION_ALLOWANCE 0.05f
jstephens78 0:e39efa4f4f58 108 #define PHYSAC_PENETRATION_CORRECTION 0.4f
jstephens78 0:e39efa4f4f58 109
jstephens78 0:e39efa4f4f58 110 #define PHYSAC_PI 3.14159265358979323846
jstephens78 0:e39efa4f4f58 111 #define PHYSAC_DEG2RAD (PHYSAC_PI/180.0f)
jstephens78 0:e39efa4f4f58 112
jstephens78 1:ebc0214989c0 113 #ifndef PHYSAC_MALLOC
jstephens78 0:e39efa4f4f58 114 #define PHYSAC_MALLOC(size) malloc(size)
jstephens78 1:ebc0214989c0 115 #endif
jstephens78 1:ebc0214989c0 116 #ifndef PHYSAC_FREE
jstephens78 0:e39efa4f4f58 117 #define PHYSAC_FREE(ptr) free(ptr)
jstephens78 1:ebc0214989c0 118 #endif
jstephens78 0:e39efa4f4f58 119
jstephens78 0:e39efa4f4f58 120 //----------------------------------------------------------------------------------
jstephens78 0:e39efa4f4f58 121 // Types and Structures Definition
jstephens78 0:e39efa4f4f58 122 // NOTE: Below types are required for PHYSAC_STANDALONE usage
jstephens78 0:e39efa4f4f58 123 //----------------------------------------------------------------------------------
jstephens78 0:e39efa4f4f58 124 #if defined(PHYSAC_STANDALONE)
jstephens78 0:e39efa4f4f58 125 // Vector2 type
jstephens78 0:e39efa4f4f58 126 typedef struct Vector2 {
jstephens78 0:e39efa4f4f58 127 float x;
jstephens78 0:e39efa4f4f58 128 float y;
jstephens78 0:e39efa4f4f58 129 } Vector2;
jstephens78 0:e39efa4f4f58 130
jstephens78 0:e39efa4f4f58 131 // Boolean type
jstephens78 0:e39efa4f4f58 132 #if !defined(_STDBOOL_H)
jstephens78 0:e39efa4f4f58 133 typedef enum { false, true } bool;
jstephens78 0:e39efa4f4f58 134 #define _STDBOOL_H
jstephens78 0:e39efa4f4f58 135 #endif
jstephens78 0:e39efa4f4f58 136 #endif
jstephens78 0:e39efa4f4f58 137
jstephens78 0:e39efa4f4f58 138 typedef enum PhysicsShapeType { PHYSICS_CIRCLE, PHYSICS_POLYGON } PhysicsShapeType;
jstephens78 0:e39efa4f4f58 139
jstephens78 0:e39efa4f4f58 140 // Previously defined to be used in PhysicsShape struct as circular dependencies
jstephens78 0:e39efa4f4f58 141 typedef struct PhysicsBodyData *PhysicsBody;
jstephens78 0:e39efa4f4f58 142
jstephens78 0:e39efa4f4f58 143 // Mat2 type (used for polygon shape rotation matrix)
jstephens78 0:e39efa4f4f58 144 typedef struct Mat2 {
jstephens78 0:e39efa4f4f58 145 float m00;
jstephens78 0:e39efa4f4f58 146 float m01;
jstephens78 0:e39efa4f4f58 147 float m10;
jstephens78 0:e39efa4f4f58 148 float m11;
jstephens78 0:e39efa4f4f58 149 } Mat2;
jstephens78 0:e39efa4f4f58 150
jstephens78 0:e39efa4f4f58 151 typedef struct PolygonData {
jstephens78 0:e39efa4f4f58 152 unsigned int vertexCount; // Current used vertex and normals count
jstephens78 0:e39efa4f4f58 153 Vector2 positions[PHYSAC_MAX_VERTICES]; // Polygon vertex positions vectors
jstephens78 0:e39efa4f4f58 154 Vector2 normals[PHYSAC_MAX_VERTICES]; // Polygon vertex normals vectors
jstephens78 0:e39efa4f4f58 155 } PolygonData;
jstephens78 0:e39efa4f4f58 156
jstephens78 0:e39efa4f4f58 157 typedef struct PhysicsShape {
jstephens78 0:e39efa4f4f58 158 PhysicsShapeType type; // Physics shape type (circle or polygon)
jstephens78 0:e39efa4f4f58 159 PhysicsBody body; // Shape physics body reference
jstephens78 0:e39efa4f4f58 160 float radius; // Circle shape radius (used for circle shapes)
jstephens78 0:e39efa4f4f58 161 Mat2 transform; // Vertices transform matrix 2x2
jstephens78 0:e39efa4f4f58 162 PolygonData vertexData; // Polygon shape vertices position and normals data (just used for polygon shapes)
jstephens78 0:e39efa4f4f58 163 } PhysicsShape;
jstephens78 0:e39efa4f4f58 164
jstephens78 0:e39efa4f4f58 165 typedef struct PhysicsBodyData {
jstephens78 0:e39efa4f4f58 166 unsigned int id; // Reference unique identifier
jstephens78 0:e39efa4f4f58 167 bool enabled; // Enabled dynamics state (collisions are calculated anyway)
jstephens78 0:e39efa4f4f58 168 Vector2 position; // Physics body shape pivot
jstephens78 0:e39efa4f4f58 169 Vector2 velocity; // Current linear velocity applied to position
jstephens78 0:e39efa4f4f58 170 Vector2 force; // Current linear force (reset to 0 every step)
jstephens78 0:e39efa4f4f58 171 float angularVelocity; // Current angular velocity applied to orient
jstephens78 0:e39efa4f4f58 172 float torque; // Current angular force (reset to 0 every step)
jstephens78 0:e39efa4f4f58 173 float orient; // Rotation in radians
jstephens78 0:e39efa4f4f58 174 float inertia; // Moment of inertia
jstephens78 0:e39efa4f4f58 175 float inverseInertia; // Inverse value of inertia
jstephens78 0:e39efa4f4f58 176 float mass; // Physics body mass
jstephens78 0:e39efa4f4f58 177 float inverseMass; // Inverse value of mass
jstephens78 0:e39efa4f4f58 178 float staticFriction; // Friction when the body has not movement (0 to 1)
jstephens78 0:e39efa4f4f58 179 float dynamicFriction; // Friction when the body has movement (0 to 1)
jstephens78 0:e39efa4f4f58 180 float restitution; // Restitution coefficient of the body (0 to 1)
jstephens78 0:e39efa4f4f58 181 bool useGravity; // Apply gravity force to dynamics
jstephens78 0:e39efa4f4f58 182 bool isGrounded; // Physics grounded on other body state
jstephens78 0:e39efa4f4f58 183 bool freezeOrient; // Physics rotation constraint
jstephens78 0:e39efa4f4f58 184 PhysicsShape shape; // Physics body shape information (type, radius, vertices, normals)
jstephens78 0:e39efa4f4f58 185 } PhysicsBodyData;
jstephens78 0:e39efa4f4f58 186
jstephens78 0:e39efa4f4f58 187 typedef struct PhysicsManifoldData {
jstephens78 0:e39efa4f4f58 188 unsigned int id; // Reference unique identifier
jstephens78 0:e39efa4f4f58 189 PhysicsBody bodyA; // Manifold first physics body reference
jstephens78 0:e39efa4f4f58 190 PhysicsBody bodyB; // Manifold second physics body reference
jstephens78 0:e39efa4f4f58 191 float penetration; // Depth of penetration from collision
jstephens78 0:e39efa4f4f58 192 Vector2 normal; // Normal direction vector from 'a' to 'b'
jstephens78 0:e39efa4f4f58 193 Vector2 contacts[2]; // Points of contact during collision
jstephens78 0:e39efa4f4f58 194 unsigned int contactsCount; // Current collision number of contacts
jstephens78 0:e39efa4f4f58 195 float restitution; // Mixed restitution during collision
jstephens78 0:e39efa4f4f58 196 float dynamicFriction; // Mixed dynamic friction during collision
jstephens78 0:e39efa4f4f58 197 float staticFriction; // Mixed static friction during collision
jstephens78 0:e39efa4f4f58 198 } PhysicsManifoldData, *PhysicsManifold;
jstephens78 0:e39efa4f4f58 199
jstephens78 0:e39efa4f4f58 200 #if defined(__cplusplus)
jstephens78 0:e39efa4f4f58 201 extern "C" { // Prevents name mangling of functions
jstephens78 0:e39efa4f4f58 202 #endif
jstephens78 0:e39efa4f4f58 203
jstephens78 0:e39efa4f4f58 204 //----------------------------------------------------------------------------------
jstephens78 0:e39efa4f4f58 205 // Module Functions Declaration
jstephens78 0:e39efa4f4f58 206 //----------------------------------------------------------------------------------
jstephens78 0:e39efa4f4f58 207 PHYSACDEF void InitPhysics(void); // Initializes physics values, pointers and creates physics loop thread
jstephens78 0:e39efa4f4f58 208 PHYSACDEF void RunPhysicsStep(void); // Run physics step, to be used if PHYSICS_NO_THREADS is set in your main loop
jstephens78 0:e39efa4f4f58 209 PHYSACDEF void SetPhysicsTimeStep(double delta); // Sets physics fixed time step in milliseconds. 1.666666 by default
jstephens78 0:e39efa4f4f58 210 PHYSACDEF bool IsPhysicsEnabled(void); // Returns true if physics thread is currently enabled
jstephens78 0:e39efa4f4f58 211 PHYSACDEF void SetPhysicsGravity(float x, float y); // Sets physics global gravity force
jstephens78 0:e39efa4f4f58 212 PHYSACDEF PhysicsBody CreatePhysicsBodyCircle(Vector2 pos, float radius, float density); // Creates a new circle physics body with generic parameters
jstephens78 0:e39efa4f4f58 213 PHYSACDEF PhysicsBody CreatePhysicsBodyRectangle(Vector2 pos, float width, float height, float density); // Creates a new rectangle physics body with generic parameters
jstephens78 0:e39efa4f4f58 214 PHYSACDEF PhysicsBody CreatePhysicsBodyPolygon(Vector2 pos, float radius, int sides, float density); // Creates a new polygon physics body with generic parameters
jstephens78 0:e39efa4f4f58 215 PHYSACDEF void PhysicsAddForce(PhysicsBody body, Vector2 force); // Adds a force to a physics body
jstephens78 0:e39efa4f4f58 216 PHYSACDEF void PhysicsAddTorque(PhysicsBody body, float amount); // Adds an angular force to a physics body
jstephens78 0:e39efa4f4f58 217 PHYSACDEF void PhysicsShatter(PhysicsBody body, Vector2 position, float force); // Shatters a polygon shape physics body to little physics bodies with explosion force
jstephens78 0:e39efa4f4f58 218 PHYSACDEF int GetPhysicsBodiesCount(void); // Returns the current amount of created physics bodies
jstephens78 0:e39efa4f4f58 219 PHYSACDEF PhysicsBody GetPhysicsBody(int index); // Returns a physics body of the bodies pool at a specific index
jstephens78 0:e39efa4f4f58 220 PHYSACDEF int GetPhysicsShapeType(int index); // Returns the physics body shape type (PHYSICS_CIRCLE or PHYSICS_POLYGON)
jstephens78 0:e39efa4f4f58 221 PHYSACDEF int GetPhysicsShapeVerticesCount(int index); // Returns the amount of vertices of a physics body shape
jstephens78 0:e39efa4f4f58 222 PHYSACDEF Vector2 GetPhysicsShapeVertex(PhysicsBody body, int vertex); // Returns transformed position of a body shape (body position + vertex transformed position)
jstephens78 0:e39efa4f4f58 223 PHYSACDEF void SetPhysicsBodyRotation(PhysicsBody body, float radians); // Sets physics body shape transform based on radians parameter
jstephens78 0:e39efa4f4f58 224 PHYSACDEF void DestroyPhysicsBody(PhysicsBody body); // Unitializes and destroy a physics body
jstephens78 0:e39efa4f4f58 225 PHYSACDEF void ClosePhysics(void); // Unitializes physics pointers and closes physics loop thread
jstephens78 0:e39efa4f4f58 226
jstephens78 0:e39efa4f4f58 227 #if defined(__cplusplus)
jstephens78 0:e39efa4f4f58 228 }
jstephens78 0:e39efa4f4f58 229 #endif
jstephens78 0:e39efa4f4f58 230
jstephens78 0:e39efa4f4f58 231 #endif // PHYSAC_H
jstephens78 0:e39efa4f4f58 232
jstephens78 0:e39efa4f4f58 233 /***********************************************************************************
jstephens78 0:e39efa4f4f58 234 *
jstephens78 0:e39efa4f4f58 235 * PHYSAC IMPLEMENTATION
jstephens78 0:e39efa4f4f58 236 *
jstephens78 0:e39efa4f4f58 237 ************************************************************************************/
jstephens78 0:e39efa4f4f58 238
jstephens78 0:e39efa4f4f58 239 #if defined(PHYSAC_IMPLEMENTATION)
jstephens78 0:e39efa4f4f58 240
jstephens78 0:e39efa4f4f58 241 #if !defined(PHYSAC_NO_THREADS)
jstephens78 0:e39efa4f4f58 242 #include <pthread.h> // Required for: pthread_t, pthread_create()
jstephens78 0:e39efa4f4f58 243 #endif
jstephens78 0:e39efa4f4f58 244
jstephens78 0:e39efa4f4f58 245 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 246 #include <stdio.h> // Required for: printf()
jstephens78 0:e39efa4f4f58 247 #endif
jstephens78 0:e39efa4f4f58 248
jstephens78 0:e39efa4f4f58 249 #include <stdlib.h> // Required for: malloc(), free(), srand(), rand()
jstephens78 0:e39efa4f4f58 250 #include <math.h> // Required for: cosf(), sinf(), fabs(), sqrtf()
jstephens78 0:e39efa4f4f58 251 #include <stdint.h> // Required for: uint64_t
jstephens78 0:e39efa4f4f58 252
jstephens78 0:e39efa4f4f58 253 #if !defined(PHYSAC_STANDALONE)
jstephens78 0:e39efa4f4f58 254 #include "raymath.h" // Required for: Vector2Add(), Vector2Subtract()
jstephens78 0:e39efa4f4f58 255 #endif
jstephens78 0:e39efa4f4f58 256
jstephens78 0:e39efa4f4f58 257 // Time management functionality
jstephens78 0:e39efa4f4f58 258 #include <time.h> // Required for: time(), clock_gettime()
jstephens78 0:e39efa4f4f58 259 #if defined(_WIN32)
jstephens78 0:e39efa4f4f58 260 // Functions required to query time on Windows
jstephens78 0:e39efa4f4f58 261 #if defined(__cplusplus)
jstephens78 0:e39efa4f4f58 262 extern "C" { // Prevents name mangling of functions
jstephens78 0:e39efa4f4f58 263 #endif
jstephens78 0:e39efa4f4f58 264 int __stdcall QueryPerformanceCounter(unsigned long long int* lpPerformanceCount);
jstephens78 0:e39efa4f4f58 265 int __stdcall QueryPerformanceFrequency(unsigned long long int* lpFrequency);
jstephens78 0:e39efa4f4f58 266 #if defined(__cplusplus)
jstephens78 0:e39efa4f4f58 267 }
jstephens78 0:e39efa4f4f58 268 #endif
jstephens78 0:e39efa4f4f58 269 #elif defined(__linux__)
jstephens78 0:e39efa4f4f58 270 #if _POSIX_C_SOURCE < 199309L
jstephens78 0:e39efa4f4f58 271 #undef _POSIX_C_SOURCE
jstephens78 0:e39efa4f4f58 272 #define _POSIX_C_SOURCE 199309L // Required for CLOCK_MONOTONIC if compiled with c99 without gnu ext.
jstephens78 0:e39efa4f4f58 273 #endif
jstephens78 0:e39efa4f4f58 274 #include <sys/time.h> // Required for: timespec
jstephens78 0:e39efa4f4f58 275 #elif defined(__APPLE__) // macOS also defines __MACH__
jstephens78 0:e39efa4f4f58 276 #include <mach/mach_time.h> // Required for: mach_absolute_time()
jstephens78 0:e39efa4f4f58 277 #endif
jstephens78 0:e39efa4f4f58 278
jstephens78 0:e39efa4f4f58 279 //----------------------------------------------------------------------------------
jstephens78 0:e39efa4f4f58 280 // Defines and Macros
jstephens78 0:e39efa4f4f58 281 //----------------------------------------------------------------------------------
jstephens78 0:e39efa4f4f58 282 #define min(a,b) (((a)<(b))?(a):(b))
jstephens78 0:e39efa4f4f58 283 #define max(a,b) (((a)>(b))?(a):(b))
jstephens78 0:e39efa4f4f58 284 #define PHYSAC_FLT_MAX 3.402823466e+38f
jstephens78 0:e39efa4f4f58 285 #define PHYSAC_EPSILON 0.000001f
jstephens78 0:e39efa4f4f58 286 #define PHYSAC_K 1.0f/3.0f
jstephens78 0:e39efa4f4f58 287 #define PHYSAC_VECTOR_ZERO (Vector2){ 0.0f, 0.0f }
jstephens78 0:e39efa4f4f58 288
jstephens78 0:e39efa4f4f58 289 //----------------------------------------------------------------------------------
jstephens78 0:e39efa4f4f58 290 // Global Variables Definition
jstephens78 0:e39efa4f4f58 291 //----------------------------------------------------------------------------------
jstephens78 0:e39efa4f4f58 292 #if !defined(PHYSAC_NO_THREADS)
jstephens78 0:e39efa4f4f58 293 static pthread_t physicsThreadId; // Physics thread id
jstephens78 0:e39efa4f4f58 294 #endif
jstephens78 0:e39efa4f4f58 295 static unsigned int usedMemory = 0; // Total allocated dynamic memory
jstephens78 0:e39efa4f4f58 296 static bool physicsThreadEnabled = false; // Physics thread enabled state
jstephens78 0:e39efa4f4f58 297 static double baseTime = 0.0; // Offset time for MONOTONIC clock
jstephens78 0:e39efa4f4f58 298 static double startTime = 0.0; // Start time in milliseconds
jstephens78 0:e39efa4f4f58 299 static double deltaTime = 1.0/60.0/10.0 * 1000; // Delta time used for physics steps, in milliseconds
jstephens78 0:e39efa4f4f58 300 static double currentTime = 0.0; // Current time in milliseconds
jstephens78 0:e39efa4f4f58 301 static uint64_t frequency = 0; // Hi-res clock frequency
jstephens78 0:e39efa4f4f58 302
jstephens78 0:e39efa4f4f58 303 static double accumulator = 0.0; // Physics time step delta time accumulator
jstephens78 0:e39efa4f4f58 304 static unsigned int stepsCount = 0; // Total physics steps processed
jstephens78 0:e39efa4f4f58 305 static Vector2 gravityForce = { 0.0f, 9.81f }; // Physics world gravity force
jstephens78 0:e39efa4f4f58 306 static PhysicsBody bodies[PHYSAC_MAX_BODIES]; // Physics bodies pointers array
jstephens78 0:e39efa4f4f58 307 static unsigned int physicsBodiesCount = 0; // Physics world current bodies counter
jstephens78 0:e39efa4f4f58 308 static PhysicsManifold contacts[PHYSAC_MAX_MANIFOLDS]; // Physics bodies pointers array
jstephens78 0:e39efa4f4f58 309 static unsigned int physicsManifoldsCount = 0; // Physics world current manifolds counter
jstephens78 0:e39efa4f4f58 310
jstephens78 0:e39efa4f4f58 311 //----------------------------------------------------------------------------------
jstephens78 0:e39efa4f4f58 312 // Module Internal Functions Declaration
jstephens78 0:e39efa4f4f58 313 //----------------------------------------------------------------------------------
jstephens78 0:e39efa4f4f58 314 static int FindAvailableBodyIndex(); // Finds a valid index for a new physics body initialization
jstephens78 0:e39efa4f4f58 315 static PolygonData CreateRandomPolygon(float radius, int sides); // Creates a random polygon shape with max vertex distance from polygon pivot
jstephens78 0:e39efa4f4f58 316 static PolygonData CreateRectanglePolygon(Vector2 pos, Vector2 size); // Creates a rectangle polygon shape based on a min and max positions
jstephens78 0:e39efa4f4f58 317 static void *PhysicsLoop(void *arg); // Physics loop thread function
jstephens78 0:e39efa4f4f58 318 static void PhysicsStep(void); // Physics steps calculations (dynamics, collisions and position corrections)
jstephens78 0:e39efa4f4f58 319 static int FindAvailableManifoldIndex(); // Finds a valid index for a new manifold initialization
jstephens78 0:e39efa4f4f58 320 static PhysicsManifold CreatePhysicsManifold(PhysicsBody a, PhysicsBody b); // Creates a new physics manifold to solve collision
jstephens78 0:e39efa4f4f58 321 static void DestroyPhysicsManifold(PhysicsManifold manifold); // Unitializes and destroys a physics manifold
jstephens78 0:e39efa4f4f58 322 static void SolvePhysicsManifold(PhysicsManifold manifold); // Solves a created physics manifold between two physics bodies
jstephens78 0:e39efa4f4f58 323 static void SolveCircleToCircle(PhysicsManifold manifold); // Solves collision between two circle shape physics bodies
jstephens78 0:e39efa4f4f58 324 static void SolveCircleToPolygon(PhysicsManifold manifold); // Solves collision between a circle to a polygon shape physics bodies
jstephens78 0:e39efa4f4f58 325 static void SolvePolygonToCircle(PhysicsManifold manifold); // Solves collision between a polygon to a circle shape physics bodies
jstephens78 0:e39efa4f4f58 326 static void SolveDifferentShapes(PhysicsManifold manifold, PhysicsBody bodyA, PhysicsBody bodyB); // Solve collision between two different types of shapes
jstephens78 0:e39efa4f4f58 327 static void SolvePolygonToPolygon(PhysicsManifold manifold); // Solves collision between two polygons shape physics bodies
jstephens78 0:e39efa4f4f58 328 static void IntegratePhysicsForces(PhysicsBody body); // Integrates physics forces into velocity
jstephens78 0:e39efa4f4f58 329 static void InitializePhysicsManifolds(PhysicsManifold manifold); // Initializes physics manifolds to solve collisions
jstephens78 0:e39efa4f4f58 330 static void IntegratePhysicsImpulses(PhysicsManifold manifold); // Integrates physics collisions impulses to solve collisions
jstephens78 0:e39efa4f4f58 331 static void IntegratePhysicsVelocity(PhysicsBody body); // Integrates physics velocity into position and forces
jstephens78 0:e39efa4f4f58 332 static void CorrectPhysicsPositions(PhysicsManifold manifold); // Corrects physics bodies positions based on manifolds collision information
jstephens78 0:e39efa4f4f58 333 static float FindAxisLeastPenetration(int *faceIndex, PhysicsShape shapeA, PhysicsShape shapeB); // Finds polygon shapes axis least penetration
jstephens78 0:e39efa4f4f58 334 static void FindIncidentFace(Vector2 *v0, Vector2 *v1, PhysicsShape ref, PhysicsShape inc, int index); // Finds two polygon shapes incident face
jstephens78 0:e39efa4f4f58 335 static int Clip(Vector2 normal, float clip, Vector2 *faceA, Vector2 *faceB); // Calculates clipping based on a normal and two faces
jstephens78 0:e39efa4f4f58 336 static bool BiasGreaterThan(float valueA, float valueB); // Check if values are between bias range
jstephens78 0:e39efa4f4f58 337 static Vector2 TriangleBarycenter(Vector2 v1, Vector2 v2, Vector2 v3); // Returns the barycenter of a triangle given by 3 points
jstephens78 0:e39efa4f4f58 338
jstephens78 0:e39efa4f4f58 339 static void InitTimer(void); // Initializes hi-resolution MONOTONIC timer
jstephens78 0:e39efa4f4f58 340 static uint64_t GetTimeCount(void); // Get hi-res MONOTONIC time measure in mseconds
jstephens78 0:e39efa4f4f58 341 static double GetCurrentTime(void); // Get current time measure in milliseconds
jstephens78 0:e39efa4f4f58 342
jstephens78 0:e39efa4f4f58 343 // Math functions
jstephens78 0:e39efa4f4f58 344 static Vector2 MathCross(float value, Vector2 vector); // Returns the cross product of a vector and a value
jstephens78 0:e39efa4f4f58 345 static float MathCrossVector2(Vector2 v1, Vector2 v2); // Returns the cross product of two vectors
jstephens78 0:e39efa4f4f58 346 static float MathLenSqr(Vector2 vector); // Returns the len square root of a vector
jstephens78 0:e39efa4f4f58 347 static float MathDot(Vector2 v1, Vector2 v2); // Returns the dot product of two vectors
jstephens78 0:e39efa4f4f58 348 static inline float DistSqr(Vector2 v1, Vector2 v2); // Returns the square root of distance between two vectors
jstephens78 0:e39efa4f4f58 349 static void MathNormalize(Vector2 *vector); // Returns the normalized values of a vector
jstephens78 0:e39efa4f4f58 350 #if defined(PHYSAC_STANDALONE)
jstephens78 0:e39efa4f4f58 351 static Vector2 Vector2Add(Vector2 v1, Vector2 v2); // Returns the sum of two given vectors
jstephens78 0:e39efa4f4f58 352 static Vector2 Vector2Subtract(Vector2 v1, Vector2 v2); // Returns the subtract of two given vectors
jstephens78 0:e39efa4f4f58 353 #endif
jstephens78 0:e39efa4f4f58 354
jstephens78 0:e39efa4f4f58 355 static Mat2 Mat2Radians(float radians); // Creates a matrix 2x2 from a given radians value
jstephens78 0:e39efa4f4f58 356 static void Mat2Set(Mat2 *matrix, float radians); // Set values from radians to a created matrix 2x2
jstephens78 0:e39efa4f4f58 357 static inline Mat2 Mat2Transpose(Mat2 matrix); // Returns the transpose of a given matrix 2x2
jstephens78 0:e39efa4f4f58 358 static inline Vector2 Mat2MultiplyVector2(Mat2 matrix, Vector2 vector); // Multiplies a vector by a matrix 2x2
jstephens78 0:e39efa4f4f58 359
jstephens78 0:e39efa4f4f58 360 //----------------------------------------------------------------------------------
jstephens78 0:e39efa4f4f58 361 // Module Functions Definition
jstephens78 0:e39efa4f4f58 362 //----------------------------------------------------------------------------------
jstephens78 0:e39efa4f4f58 363 // Initializes physics values, pointers and creates physics loop thread
jstephens78 0:e39efa4f4f58 364 PHYSACDEF void InitPhysics(void)
jstephens78 0:e39efa4f4f58 365 {
jstephens78 0:e39efa4f4f58 366 #if !defined(PHYSAC_NO_THREADS)
jstephens78 0:e39efa4f4f58 367 // NOTE: if defined, user will need to create a thread for PhysicsThread function manually
jstephens78 0:e39efa4f4f58 368 // Create physics thread using POSIXS thread libraries
jstephens78 0:e39efa4f4f58 369 pthread_create(&physicsThreadId, NULL, &PhysicsLoop, NULL);
jstephens78 0:e39efa4f4f58 370 #endif
jstephens78 0:e39efa4f4f58 371
jstephens78 0:e39efa4f4f58 372 // Initialize high resolution timer
jstephens78 0:e39efa4f4f58 373 InitTimer();
jstephens78 0:e39efa4f4f58 374
jstephens78 0:e39efa4f4f58 375 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 376 printf("[PHYSAC] physics module initialized successfully\n");
jstephens78 0:e39efa4f4f58 377 #endif
jstephens78 0:e39efa4f4f58 378
jstephens78 0:e39efa4f4f58 379 accumulator = 0.0;
jstephens78 0:e39efa4f4f58 380 }
jstephens78 0:e39efa4f4f58 381
jstephens78 0:e39efa4f4f58 382 // Returns true if physics thread is currently enabled
jstephens78 0:e39efa4f4f58 383 PHYSACDEF bool IsPhysicsEnabled(void)
jstephens78 0:e39efa4f4f58 384 {
jstephens78 0:e39efa4f4f58 385 return physicsThreadEnabled;
jstephens78 0:e39efa4f4f58 386 }
jstephens78 0:e39efa4f4f58 387
jstephens78 0:e39efa4f4f58 388 // Sets physics global gravity force
jstephens78 0:e39efa4f4f58 389 PHYSACDEF void SetPhysicsGravity(float x, float y)
jstephens78 0:e39efa4f4f58 390 {
jstephens78 0:e39efa4f4f58 391 gravityForce.x = x;
jstephens78 0:e39efa4f4f58 392 gravityForce.y = y;
jstephens78 0:e39efa4f4f58 393 }
jstephens78 0:e39efa4f4f58 394
jstephens78 0:e39efa4f4f58 395 // Creates a new circle physics body with generic parameters
jstephens78 0:e39efa4f4f58 396 PHYSACDEF PhysicsBody CreatePhysicsBodyCircle(Vector2 pos, float radius, float density)
jstephens78 0:e39efa4f4f58 397 {
jstephens78 0:e39efa4f4f58 398 PhysicsBody newBody = (PhysicsBody)PHYSAC_MALLOC(sizeof(PhysicsBodyData));
jstephens78 0:e39efa4f4f58 399 usedMemory += sizeof(PhysicsBodyData);
jstephens78 0:e39efa4f4f58 400
jstephens78 0:e39efa4f4f58 401 int newId = FindAvailableBodyIndex();
jstephens78 0:e39efa4f4f58 402 if (newId != -1)
jstephens78 0:e39efa4f4f58 403 {
jstephens78 0:e39efa4f4f58 404 // Initialize new body with generic values
jstephens78 0:e39efa4f4f58 405 newBody->id = newId;
jstephens78 0:e39efa4f4f58 406 newBody->enabled = true;
jstephens78 0:e39efa4f4f58 407 newBody->position = pos;
jstephens78 0:e39efa4f4f58 408 newBody->velocity = PHYSAC_VECTOR_ZERO;
jstephens78 0:e39efa4f4f58 409 newBody->force = PHYSAC_VECTOR_ZERO;
jstephens78 0:e39efa4f4f58 410 newBody->angularVelocity = 0.0f;
jstephens78 0:e39efa4f4f58 411 newBody->torque = 0.0f;
jstephens78 0:e39efa4f4f58 412 newBody->orient = 0.0f;
jstephens78 0:e39efa4f4f58 413 newBody->shape.type = PHYSICS_CIRCLE;
jstephens78 0:e39efa4f4f58 414 newBody->shape.body = newBody;
jstephens78 0:e39efa4f4f58 415 newBody->shape.radius = radius;
jstephens78 0:e39efa4f4f58 416 newBody->shape.transform = Mat2Radians(0.0f);
jstephens78 0:e39efa4f4f58 417 newBody->shape.vertexData = (PolygonData) { 0 };
jstephens78 0:e39efa4f4f58 418
jstephens78 0:e39efa4f4f58 419 newBody->mass = PHYSAC_PI*radius*radius*density;
jstephens78 0:e39efa4f4f58 420 newBody->inverseMass = ((newBody->mass != 0.0f) ? 1.0f/newBody->mass : 0.0f);
jstephens78 0:e39efa4f4f58 421 newBody->inertia = newBody->mass*radius*radius;
jstephens78 0:e39efa4f4f58 422 newBody->inverseInertia = ((newBody->inertia != 0.0f) ? 1.0f/newBody->inertia : 0.0f);
jstephens78 0:e39efa4f4f58 423 newBody->staticFriction = 0.4f;
jstephens78 0:e39efa4f4f58 424 newBody->dynamicFriction = 0.2f;
jstephens78 0:e39efa4f4f58 425 newBody->restitution = 0.0f;
jstephens78 0:e39efa4f4f58 426 newBody->useGravity = true;
jstephens78 0:e39efa4f4f58 427 newBody->isGrounded = false;
jstephens78 0:e39efa4f4f58 428 newBody->freezeOrient = false;
jstephens78 0:e39efa4f4f58 429
jstephens78 0:e39efa4f4f58 430 // Add new body to bodies pointers array and update bodies count
jstephens78 0:e39efa4f4f58 431 bodies[physicsBodiesCount] = newBody;
jstephens78 0:e39efa4f4f58 432 physicsBodiesCount++;
jstephens78 0:e39efa4f4f58 433
jstephens78 0:e39efa4f4f58 434 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 435 printf("[PHYSAC] created polygon physics body id %i\n", newBody->id);
jstephens78 0:e39efa4f4f58 436 #endif
jstephens78 0:e39efa4f4f58 437 }
jstephens78 0:e39efa4f4f58 438 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 439 else
jstephens78 0:e39efa4f4f58 440 printf("[PHYSAC] new physics body creation failed because there is any available id to use\n");
jstephens78 0:e39efa4f4f58 441 #endif
jstephens78 0:e39efa4f4f58 442
jstephens78 0:e39efa4f4f58 443 return newBody;
jstephens78 0:e39efa4f4f58 444 }
jstephens78 0:e39efa4f4f58 445
jstephens78 0:e39efa4f4f58 446 // Creates a new rectangle physics body with generic parameters
jstephens78 0:e39efa4f4f58 447 PHYSACDEF PhysicsBody CreatePhysicsBodyRectangle(Vector2 pos, float width, float height, float density)
jstephens78 0:e39efa4f4f58 448 {
jstephens78 0:e39efa4f4f58 449 PhysicsBody newBody = (PhysicsBody)PHYSAC_MALLOC(sizeof(PhysicsBodyData));
jstephens78 0:e39efa4f4f58 450 usedMemory += sizeof(PhysicsBodyData);
jstephens78 0:e39efa4f4f58 451
jstephens78 0:e39efa4f4f58 452 int newId = FindAvailableBodyIndex();
jstephens78 0:e39efa4f4f58 453 if (newId != -1)
jstephens78 0:e39efa4f4f58 454 {
jstephens78 0:e39efa4f4f58 455 // Initialize new body with generic values
jstephens78 0:e39efa4f4f58 456 newBody->id = newId;
jstephens78 0:e39efa4f4f58 457 newBody->enabled = true;
jstephens78 0:e39efa4f4f58 458 newBody->position = pos;
jstephens78 0:e39efa4f4f58 459 newBody->velocity = (Vector2){ 0.0f };
jstephens78 0:e39efa4f4f58 460 newBody->force = (Vector2){ 0.0f };
jstephens78 0:e39efa4f4f58 461 newBody->angularVelocity = 0.0f;
jstephens78 0:e39efa4f4f58 462 newBody->torque = 0.0f;
jstephens78 0:e39efa4f4f58 463 newBody->orient = 0.0f;
jstephens78 0:e39efa4f4f58 464 newBody->shape.type = PHYSICS_POLYGON;
jstephens78 0:e39efa4f4f58 465 newBody->shape.body = newBody;
jstephens78 0:e39efa4f4f58 466 newBody->shape.radius = 0.0f;
jstephens78 0:e39efa4f4f58 467 newBody->shape.transform = Mat2Radians(0.0f);
jstephens78 0:e39efa4f4f58 468 newBody->shape.vertexData = CreateRectanglePolygon(pos, (Vector2){ width, height });
jstephens78 0:e39efa4f4f58 469
jstephens78 0:e39efa4f4f58 470 // Calculate centroid and moment of inertia
jstephens78 0:e39efa4f4f58 471 Vector2 center = { 0.0f, 0.0f };
jstephens78 0:e39efa4f4f58 472 float area = 0.0f;
jstephens78 0:e39efa4f4f58 473 float inertia = 0.0f;
jstephens78 0:e39efa4f4f58 474
jstephens78 0:e39efa4f4f58 475 for (int i = 0; i < newBody->shape.vertexData.vertexCount; i++)
jstephens78 0:e39efa4f4f58 476 {
jstephens78 0:e39efa4f4f58 477 // Triangle vertices, third vertex implied as (0, 0)
jstephens78 0:e39efa4f4f58 478 Vector2 p1 = newBody->shape.vertexData.positions[i];
jstephens78 0:e39efa4f4f58 479 int nextIndex = (((i + 1) < newBody->shape.vertexData.vertexCount) ? (i + 1) : 0);
jstephens78 0:e39efa4f4f58 480 Vector2 p2 = newBody->shape.vertexData.positions[nextIndex];
jstephens78 0:e39efa4f4f58 481
jstephens78 0:e39efa4f4f58 482 float D = MathCrossVector2(p1, p2);
jstephens78 0:e39efa4f4f58 483 float triangleArea = D/2;
jstephens78 0:e39efa4f4f58 484
jstephens78 0:e39efa4f4f58 485 area += triangleArea;
jstephens78 0:e39efa4f4f58 486
jstephens78 0:e39efa4f4f58 487 // Use area to weight the centroid average, not just vertex position
jstephens78 0:e39efa4f4f58 488 center.x += triangleArea*PHYSAC_K*(p1.x + p2.x);
jstephens78 0:e39efa4f4f58 489 center.y += triangleArea*PHYSAC_K*(p1.y + p2.y);
jstephens78 0:e39efa4f4f58 490
jstephens78 0:e39efa4f4f58 491 float intx2 = p1.x*p1.x + p2.x*p1.x + p2.x*p2.x;
jstephens78 0:e39efa4f4f58 492 float inty2 = p1.y*p1.y + p2.y*p1.y + p2.y*p2.y;
jstephens78 0:e39efa4f4f58 493 inertia += (0.25f*PHYSAC_K*D)*(intx2 + inty2);
jstephens78 0:e39efa4f4f58 494 }
jstephens78 0:e39efa4f4f58 495
jstephens78 0:e39efa4f4f58 496 center.x *= 1.0f/area;
jstephens78 0:e39efa4f4f58 497 center.y *= 1.0f/area;
jstephens78 0:e39efa4f4f58 498
jstephens78 0:e39efa4f4f58 499 // Translate vertices to centroid (make the centroid (0, 0) for the polygon in model space)
jstephens78 0:e39efa4f4f58 500 // Note: this is not really necessary
jstephens78 0:e39efa4f4f58 501 for (int i = 0; i < newBody->shape.vertexData.vertexCount; i++)
jstephens78 0:e39efa4f4f58 502 {
jstephens78 0:e39efa4f4f58 503 newBody->shape.vertexData.positions[i].x -= center.x;
jstephens78 0:e39efa4f4f58 504 newBody->shape.vertexData.positions[i].y -= center.y;
jstephens78 0:e39efa4f4f58 505 }
jstephens78 0:e39efa4f4f58 506
jstephens78 0:e39efa4f4f58 507 newBody->mass = density*area;
jstephens78 0:e39efa4f4f58 508 newBody->inverseMass = ((newBody->mass != 0.0f) ? 1.0f/newBody->mass : 0.0f);
jstephens78 0:e39efa4f4f58 509 newBody->inertia = density*inertia;
jstephens78 0:e39efa4f4f58 510 newBody->inverseInertia = ((newBody->inertia != 0.0f) ? 1.0f/newBody->inertia : 0.0f);
jstephens78 0:e39efa4f4f58 511 newBody->staticFriction = 0.4f;
jstephens78 0:e39efa4f4f58 512 newBody->dynamicFriction = 0.2f;
jstephens78 0:e39efa4f4f58 513 newBody->restitution = 0.0f;
jstephens78 0:e39efa4f4f58 514 newBody->useGravity = true;
jstephens78 0:e39efa4f4f58 515 newBody->isGrounded = false;
jstephens78 0:e39efa4f4f58 516 newBody->freezeOrient = false;
jstephens78 0:e39efa4f4f58 517
jstephens78 0:e39efa4f4f58 518 // Add new body to bodies pointers array and update bodies count
jstephens78 0:e39efa4f4f58 519 bodies[physicsBodiesCount] = newBody;
jstephens78 0:e39efa4f4f58 520 physicsBodiesCount++;
jstephens78 0:e39efa4f4f58 521
jstephens78 0:e39efa4f4f58 522 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 523 printf("[PHYSAC] created polygon physics body id %i\n", newBody->id);
jstephens78 0:e39efa4f4f58 524 #endif
jstephens78 0:e39efa4f4f58 525 }
jstephens78 0:e39efa4f4f58 526 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 527 else
jstephens78 0:e39efa4f4f58 528 printf("[PHYSAC] new physics body creation failed because there is any available id to use\n");
jstephens78 0:e39efa4f4f58 529 #endif
jstephens78 0:e39efa4f4f58 530
jstephens78 0:e39efa4f4f58 531 return newBody;
jstephens78 0:e39efa4f4f58 532 }
jstephens78 0:e39efa4f4f58 533
jstephens78 0:e39efa4f4f58 534 // Creates a new polygon physics body with generic parameters
jstephens78 0:e39efa4f4f58 535 PHYSACDEF PhysicsBody CreatePhysicsBodyPolygon(Vector2 pos, float radius, int sides, float density)
jstephens78 0:e39efa4f4f58 536 {
jstephens78 0:e39efa4f4f58 537 PhysicsBody newBody = (PhysicsBody)PHYSAC_MALLOC(sizeof(PhysicsBodyData));
jstephens78 0:e39efa4f4f58 538 usedMemory += sizeof(PhysicsBodyData);
jstephens78 0:e39efa4f4f58 539
jstephens78 0:e39efa4f4f58 540 int newId = FindAvailableBodyIndex();
jstephens78 0:e39efa4f4f58 541 if (newId != -1)
jstephens78 0:e39efa4f4f58 542 {
jstephens78 0:e39efa4f4f58 543 // Initialize new body with generic values
jstephens78 0:e39efa4f4f58 544 newBody->id = newId;
jstephens78 0:e39efa4f4f58 545 newBody->enabled = true;
jstephens78 0:e39efa4f4f58 546 newBody->position = pos;
jstephens78 0:e39efa4f4f58 547 newBody->velocity = PHYSAC_VECTOR_ZERO;
jstephens78 0:e39efa4f4f58 548 newBody->force = PHYSAC_VECTOR_ZERO;
jstephens78 0:e39efa4f4f58 549 newBody->angularVelocity = 0.0f;
jstephens78 0:e39efa4f4f58 550 newBody->torque = 0.0f;
jstephens78 0:e39efa4f4f58 551 newBody->orient = 0.0f;
jstephens78 0:e39efa4f4f58 552 newBody->shape.type = PHYSICS_POLYGON;
jstephens78 0:e39efa4f4f58 553 newBody->shape.body = newBody;
jstephens78 0:e39efa4f4f58 554 newBody->shape.transform = Mat2Radians(0.0f);
jstephens78 0:e39efa4f4f58 555 newBody->shape.vertexData = CreateRandomPolygon(radius, sides);
jstephens78 0:e39efa4f4f58 556
jstephens78 0:e39efa4f4f58 557 // Calculate centroid and moment of inertia
jstephens78 0:e39efa4f4f58 558 Vector2 center = { 0.0f, 0.0f };
jstephens78 0:e39efa4f4f58 559 float area = 0.0f;
jstephens78 0:e39efa4f4f58 560 float inertia = 0.0f;
jstephens78 0:e39efa4f4f58 561
jstephens78 0:e39efa4f4f58 562 for (int i = 0; i < newBody->shape.vertexData.vertexCount; i++)
jstephens78 0:e39efa4f4f58 563 {
jstephens78 0:e39efa4f4f58 564 // Triangle vertices, third vertex implied as (0, 0)
jstephens78 0:e39efa4f4f58 565 Vector2 position1 = newBody->shape.vertexData.positions[i];
jstephens78 0:e39efa4f4f58 566 int nextIndex = (((i + 1) < newBody->shape.vertexData.vertexCount) ? (i + 1) : 0);
jstephens78 0:e39efa4f4f58 567 Vector2 position2 = newBody->shape.vertexData.positions[nextIndex];
jstephens78 0:e39efa4f4f58 568
jstephens78 0:e39efa4f4f58 569 float cross = MathCrossVector2(position1, position2);
jstephens78 0:e39efa4f4f58 570 float triangleArea = cross/2;
jstephens78 0:e39efa4f4f58 571
jstephens78 0:e39efa4f4f58 572 area += triangleArea;
jstephens78 0:e39efa4f4f58 573
jstephens78 0:e39efa4f4f58 574 // Use area to weight the centroid average, not just vertex position
jstephens78 0:e39efa4f4f58 575 center.x += triangleArea*PHYSAC_K*(position1.x + position2.x);
jstephens78 0:e39efa4f4f58 576 center.y += triangleArea*PHYSAC_K*(position1.y + position2.y);
jstephens78 0:e39efa4f4f58 577
jstephens78 0:e39efa4f4f58 578 float intx2 = position1.x*position1.x + position2.x*position1.x + position2.x*position2.x;
jstephens78 0:e39efa4f4f58 579 float inty2 = position1.y*position1.y + position2.y*position1.y + position2.y*position2.y;
jstephens78 0:e39efa4f4f58 580 inertia += (0.25f*PHYSAC_K*cross)*(intx2 + inty2);
jstephens78 0:e39efa4f4f58 581 }
jstephens78 0:e39efa4f4f58 582
jstephens78 0:e39efa4f4f58 583 center.x *= 1.0f/area;
jstephens78 0:e39efa4f4f58 584 center.y *= 1.0f/area;
jstephens78 0:e39efa4f4f58 585
jstephens78 0:e39efa4f4f58 586 // Translate vertices to centroid (make the centroid (0, 0) for the polygon in model space)
jstephens78 0:e39efa4f4f58 587 // Note: this is not really necessary
jstephens78 0:e39efa4f4f58 588 for (int i = 0; i < newBody->shape.vertexData.vertexCount; i++)
jstephens78 0:e39efa4f4f58 589 {
jstephens78 0:e39efa4f4f58 590 newBody->shape.vertexData.positions[i].x -= center.x;
jstephens78 0:e39efa4f4f58 591 newBody->shape.vertexData.positions[i].y -= center.y;
jstephens78 0:e39efa4f4f58 592 }
jstephens78 0:e39efa4f4f58 593
jstephens78 0:e39efa4f4f58 594 newBody->mass = density*area;
jstephens78 0:e39efa4f4f58 595 newBody->inverseMass = ((newBody->mass != 0.0f) ? 1.0f/newBody->mass : 0.0f);
jstephens78 0:e39efa4f4f58 596 newBody->inertia = density*inertia;
jstephens78 0:e39efa4f4f58 597 newBody->inverseInertia = ((newBody->inertia != 0.0f) ? 1.0f/newBody->inertia : 0.0f);
jstephens78 0:e39efa4f4f58 598 newBody->staticFriction = 0.4f;
jstephens78 0:e39efa4f4f58 599 newBody->dynamicFriction = 0.2f;
jstephens78 0:e39efa4f4f58 600 newBody->restitution = 0.0f;
jstephens78 0:e39efa4f4f58 601 newBody->useGravity = true;
jstephens78 0:e39efa4f4f58 602 newBody->isGrounded = false;
jstephens78 0:e39efa4f4f58 603 newBody->freezeOrient = false;
jstephens78 0:e39efa4f4f58 604
jstephens78 0:e39efa4f4f58 605 // Add new body to bodies pointers array and update bodies count
jstephens78 0:e39efa4f4f58 606 bodies[physicsBodiesCount] = newBody;
jstephens78 0:e39efa4f4f58 607 physicsBodiesCount++;
jstephens78 0:e39efa4f4f58 608
jstephens78 0:e39efa4f4f58 609 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 610 printf("[PHYSAC] created polygon physics body id %i\n", newBody->id);
jstephens78 0:e39efa4f4f58 611 #endif
jstephens78 0:e39efa4f4f58 612 }
jstephens78 0:e39efa4f4f58 613 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 614 else
jstephens78 0:e39efa4f4f58 615 printf("[PHYSAC] new physics body creation failed because there is any available id to use\n");
jstephens78 0:e39efa4f4f58 616 #endif
jstephens78 0:e39efa4f4f58 617
jstephens78 0:e39efa4f4f58 618 return newBody;
jstephens78 0:e39efa4f4f58 619 }
jstephens78 0:e39efa4f4f58 620
jstephens78 0:e39efa4f4f58 621 // Adds a force to a physics body
jstephens78 0:e39efa4f4f58 622 PHYSACDEF void PhysicsAddForce(PhysicsBody body, Vector2 force)
jstephens78 0:e39efa4f4f58 623 {
jstephens78 0:e39efa4f4f58 624 if (body != NULL)
jstephens78 0:e39efa4f4f58 625 body->force = Vector2Add(body->force, force);
jstephens78 0:e39efa4f4f58 626 }
jstephens78 0:e39efa4f4f58 627
jstephens78 0:e39efa4f4f58 628 // Adds an angular force to a physics body
jstephens78 0:e39efa4f4f58 629 PHYSACDEF void PhysicsAddTorque(PhysicsBody body, float amount)
jstephens78 0:e39efa4f4f58 630 {
jstephens78 0:e39efa4f4f58 631 if (body != NULL)
jstephens78 0:e39efa4f4f58 632 body->torque += amount;
jstephens78 0:e39efa4f4f58 633 }
jstephens78 0:e39efa4f4f58 634
jstephens78 0:e39efa4f4f58 635 // Shatters a polygon shape physics body to little physics bodies with explosion force
jstephens78 0:e39efa4f4f58 636 PHYSACDEF void PhysicsShatter(PhysicsBody body, Vector2 position, float force)
jstephens78 0:e39efa4f4f58 637 {
jstephens78 0:e39efa4f4f58 638 if (body != NULL)
jstephens78 0:e39efa4f4f58 639 {
jstephens78 0:e39efa4f4f58 640 if (body->shape.type == PHYSICS_POLYGON)
jstephens78 0:e39efa4f4f58 641 {
jstephens78 0:e39efa4f4f58 642 PolygonData vertexData = body->shape.vertexData;
jstephens78 0:e39efa4f4f58 643 bool collision = false;
jstephens78 0:e39efa4f4f58 644
jstephens78 0:e39efa4f4f58 645 for (int i = 0; i < vertexData.vertexCount; i++)
jstephens78 0:e39efa4f4f58 646 {
jstephens78 0:e39efa4f4f58 647 Vector2 positionA = body->position;
jstephens78 0:e39efa4f4f58 648 Vector2 positionB = Mat2MultiplyVector2(body->shape.transform, Vector2Add(body->position, vertexData.positions[i]));
jstephens78 0:e39efa4f4f58 649 int nextIndex = (((i + 1) < vertexData.vertexCount) ? (i + 1) : 0);
jstephens78 0:e39efa4f4f58 650 Vector2 positionC = Mat2MultiplyVector2(body->shape.transform, Vector2Add(body->position, vertexData.positions[nextIndex]));
jstephens78 0:e39efa4f4f58 651
jstephens78 0:e39efa4f4f58 652 // Check collision between each triangle
jstephens78 0:e39efa4f4f58 653 float alpha = ((positionB.y - positionC.y)*(position.x - positionC.x) + (positionC.x - positionB.x)*(position.y - positionC.y))/
jstephens78 0:e39efa4f4f58 654 ((positionB.y - positionC.y)*(positionA.x - positionC.x) + (positionC.x - positionB.x)*(positionA.y - positionC.y));
jstephens78 0:e39efa4f4f58 655
jstephens78 0:e39efa4f4f58 656 float beta = ((positionC.y - positionA.y)*(position.x - positionC.x) + (positionA.x - positionC.x)*(position.y - positionC.y))/
jstephens78 0:e39efa4f4f58 657 ((positionB.y - positionC.y)*(positionA.x - positionC.x) + (positionC.x - positionB.x)*(positionA.y - positionC.y));
jstephens78 0:e39efa4f4f58 658
jstephens78 0:e39efa4f4f58 659 float gamma = 1.0f - alpha - beta;
jstephens78 0:e39efa4f4f58 660
jstephens78 0:e39efa4f4f58 661 if ((alpha > 0.0f) && (beta > 0.0f) && (gamma > 0.0f))
jstephens78 0:e39efa4f4f58 662 {
jstephens78 0:e39efa4f4f58 663 collision = true;
jstephens78 0:e39efa4f4f58 664 break;
jstephens78 0:e39efa4f4f58 665 }
jstephens78 0:e39efa4f4f58 666 }
jstephens78 0:e39efa4f4f58 667
jstephens78 0:e39efa4f4f58 668 if (collision)
jstephens78 0:e39efa4f4f58 669 {
jstephens78 0:e39efa4f4f58 670 int count = vertexData.vertexCount;
jstephens78 0:e39efa4f4f58 671 Vector2 bodyPos = body->position;
jstephens78 0:e39efa4f4f58 672 Vector2 *vertices = (Vector2*)PHYSAC_MALLOC(sizeof(Vector2) * count);
jstephens78 0:e39efa4f4f58 673 Mat2 trans = body->shape.transform;
jstephens78 0:e39efa4f4f58 674
jstephens78 0:e39efa4f4f58 675 for (int i = 0; i < count; i++)
jstephens78 0:e39efa4f4f58 676 vertices[i] = vertexData.positions[i];
jstephens78 0:e39efa4f4f58 677
jstephens78 0:e39efa4f4f58 678 // Destroy shattered physics body
jstephens78 0:e39efa4f4f58 679 DestroyPhysicsBody(body);
jstephens78 0:e39efa4f4f58 680
jstephens78 0:e39efa4f4f58 681 for (int i = 0; i < count; i++)
jstephens78 0:e39efa4f4f58 682 {
jstephens78 0:e39efa4f4f58 683 int nextIndex = (((i + 1) < count) ? (i + 1) : 0);
jstephens78 0:e39efa4f4f58 684 Vector2 center = TriangleBarycenter(vertices[i], vertices[nextIndex], PHYSAC_VECTOR_ZERO);
jstephens78 0:e39efa4f4f58 685 center = Vector2Add(bodyPos, center);
jstephens78 0:e39efa4f4f58 686 Vector2 offset = Vector2Subtract(center, bodyPos);
jstephens78 0:e39efa4f4f58 687
jstephens78 0:e39efa4f4f58 688 PhysicsBody newBody = CreatePhysicsBodyPolygon(center, 10, 3, 10); // Create polygon physics body with relevant values
jstephens78 0:e39efa4f4f58 689
jstephens78 0:e39efa4f4f58 690 PolygonData newData = { 0 };
jstephens78 0:e39efa4f4f58 691 newData.vertexCount = 3;
jstephens78 0:e39efa4f4f58 692
jstephens78 0:e39efa4f4f58 693 newData.positions[0] = Vector2Subtract(vertices[i], offset);
jstephens78 0:e39efa4f4f58 694 newData.positions[1] = Vector2Subtract(vertices[nextIndex], offset);
jstephens78 0:e39efa4f4f58 695 newData.positions[2] = Vector2Subtract(position, center);
jstephens78 0:e39efa4f4f58 696
jstephens78 0:e39efa4f4f58 697 // Separate vertices to avoid unnecessary physics collisions
jstephens78 0:e39efa4f4f58 698 newData.positions[0].x *= 0.95f;
jstephens78 0:e39efa4f4f58 699 newData.positions[0].y *= 0.95f;
jstephens78 0:e39efa4f4f58 700 newData.positions[1].x *= 0.95f;
jstephens78 0:e39efa4f4f58 701 newData.positions[1].y *= 0.95f;
jstephens78 0:e39efa4f4f58 702 newData.positions[2].x *= 0.95f;
jstephens78 0:e39efa4f4f58 703 newData.positions[2].y *= 0.95f;
jstephens78 0:e39efa4f4f58 704
jstephens78 0:e39efa4f4f58 705 // Calculate polygon faces normals
jstephens78 0:e39efa4f4f58 706 for (int j = 0; j < newData.vertexCount; j++)
jstephens78 0:e39efa4f4f58 707 {
jstephens78 0:e39efa4f4f58 708 int nextVertex = (((j + 1) < newData.vertexCount) ? (j + 1) : 0);
jstephens78 0:e39efa4f4f58 709 Vector2 face = Vector2Subtract(newData.positions[nextVertex], newData.positions[j]);
jstephens78 0:e39efa4f4f58 710
jstephens78 0:e39efa4f4f58 711 newData.normals[j] = (Vector2){ face.y, -face.x };
jstephens78 0:e39efa4f4f58 712 MathNormalize(&newData.normals[j]);
jstephens78 0:e39efa4f4f58 713 }
jstephens78 0:e39efa4f4f58 714
jstephens78 0:e39efa4f4f58 715 // Apply computed vertex data to new physics body shape
jstephens78 0:e39efa4f4f58 716 newBody->shape.vertexData = newData;
jstephens78 0:e39efa4f4f58 717 newBody->shape.transform = trans;
jstephens78 0:e39efa4f4f58 718
jstephens78 0:e39efa4f4f58 719 // Calculate centroid and moment of inertia
jstephens78 0:e39efa4f4f58 720 center = PHYSAC_VECTOR_ZERO;
jstephens78 0:e39efa4f4f58 721 float area = 0.0f;
jstephens78 0:e39efa4f4f58 722 float inertia = 0.0f;
jstephens78 0:e39efa4f4f58 723
jstephens78 0:e39efa4f4f58 724 for (int j = 0; j < newBody->shape.vertexData.vertexCount; j++)
jstephens78 0:e39efa4f4f58 725 {
jstephens78 0:e39efa4f4f58 726 // Triangle vertices, third vertex implied as (0, 0)
jstephens78 0:e39efa4f4f58 727 Vector2 p1 = newBody->shape.vertexData.positions[j];
jstephens78 0:e39efa4f4f58 728 int nextVertex = (((j + 1) < newBody->shape.vertexData.vertexCount) ? (j + 1) : 0);
jstephens78 0:e39efa4f4f58 729 Vector2 p2 = newBody->shape.vertexData.positions[nextVertex];
jstephens78 0:e39efa4f4f58 730
jstephens78 0:e39efa4f4f58 731 float D = MathCrossVector2(p1, p2);
jstephens78 0:e39efa4f4f58 732 float triangleArea = D/2;
jstephens78 0:e39efa4f4f58 733
jstephens78 0:e39efa4f4f58 734 area += triangleArea;
jstephens78 0:e39efa4f4f58 735
jstephens78 0:e39efa4f4f58 736 // Use area to weight the centroid average, not just vertex position
jstephens78 0:e39efa4f4f58 737 center.x += triangleArea*PHYSAC_K*(p1.x + p2.x);
jstephens78 0:e39efa4f4f58 738 center.y += triangleArea*PHYSAC_K*(p1.y + p2.y);
jstephens78 0:e39efa4f4f58 739
jstephens78 0:e39efa4f4f58 740 float intx2 = p1.x*p1.x + p2.x*p1.x + p2.x*p2.x;
jstephens78 0:e39efa4f4f58 741 float inty2 = p1.y*p1.y + p2.y*p1.y + p2.y*p2.y;
jstephens78 0:e39efa4f4f58 742 inertia += (0.25f*PHYSAC_K*D)*(intx2 + inty2);
jstephens78 0:e39efa4f4f58 743 }
jstephens78 0:e39efa4f4f58 744
jstephens78 0:e39efa4f4f58 745 center.x *= 1.0f/area;
jstephens78 0:e39efa4f4f58 746 center.y *= 1.0f/area;
jstephens78 0:e39efa4f4f58 747
jstephens78 0:e39efa4f4f58 748 newBody->mass = area;
jstephens78 0:e39efa4f4f58 749 newBody->inverseMass = ((newBody->mass != 0.0f) ? 1.0f/newBody->mass : 0.0f);
jstephens78 0:e39efa4f4f58 750 newBody->inertia = inertia;
jstephens78 0:e39efa4f4f58 751 newBody->inverseInertia = ((newBody->inertia != 0.0f) ? 1.0f/newBody->inertia : 0.0f);
jstephens78 0:e39efa4f4f58 752
jstephens78 0:e39efa4f4f58 753 // Calculate explosion force direction
jstephens78 0:e39efa4f4f58 754 Vector2 pointA = newBody->position;
jstephens78 0:e39efa4f4f58 755 Vector2 pointB = Vector2Subtract(newData.positions[1], newData.positions[0]);
jstephens78 0:e39efa4f4f58 756 pointB.x /= 2.0f;
jstephens78 0:e39efa4f4f58 757 pointB.y /= 2.0f;
jstephens78 0:e39efa4f4f58 758 Vector2 forceDirection = Vector2Subtract(Vector2Add(pointA, Vector2Add(newData.positions[0], pointB)), newBody->position);
jstephens78 0:e39efa4f4f58 759 MathNormalize(&forceDirection);
jstephens78 0:e39efa4f4f58 760 forceDirection.x *= force;
jstephens78 0:e39efa4f4f58 761 forceDirection.y *= force;
jstephens78 0:e39efa4f4f58 762
jstephens78 0:e39efa4f4f58 763 // Apply force to new physics body
jstephens78 0:e39efa4f4f58 764 PhysicsAddForce(newBody, forceDirection);
jstephens78 0:e39efa4f4f58 765 }
jstephens78 0:e39efa4f4f58 766
jstephens78 0:e39efa4f4f58 767 PHYSAC_FREE(vertices);
jstephens78 0:e39efa4f4f58 768 }
jstephens78 0:e39efa4f4f58 769 }
jstephens78 0:e39efa4f4f58 770 }
jstephens78 0:e39efa4f4f58 771 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 772 else
jstephens78 0:e39efa4f4f58 773 printf("[PHYSAC] error when trying to shatter a null reference physics body");
jstephens78 0:e39efa4f4f58 774 #endif
jstephens78 0:e39efa4f4f58 775 }
jstephens78 0:e39efa4f4f58 776
jstephens78 0:e39efa4f4f58 777 // Returns the current amount of created physics bodies
jstephens78 0:e39efa4f4f58 778 PHYSACDEF int GetPhysicsBodiesCount(void)
jstephens78 0:e39efa4f4f58 779 {
jstephens78 0:e39efa4f4f58 780 return physicsBodiesCount;
jstephens78 0:e39efa4f4f58 781 }
jstephens78 0:e39efa4f4f58 782
jstephens78 0:e39efa4f4f58 783 // Returns a physics body of the bodies pool at a specific index
jstephens78 0:e39efa4f4f58 784 PHYSACDEF PhysicsBody GetPhysicsBody(int index)
jstephens78 0:e39efa4f4f58 785 {
jstephens78 0:e39efa4f4f58 786 if (index < physicsBodiesCount)
jstephens78 0:e39efa4f4f58 787 {
jstephens78 0:e39efa4f4f58 788 if (bodies[index] == NULL)
jstephens78 0:e39efa4f4f58 789 {
jstephens78 0:e39efa4f4f58 790 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 791 printf("[PHYSAC] error when trying to get a null reference physics body");
jstephens78 0:e39efa4f4f58 792 #endif
jstephens78 0:e39efa4f4f58 793 }
jstephens78 0:e39efa4f4f58 794 }
jstephens78 0:e39efa4f4f58 795 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 796 else
jstephens78 0:e39efa4f4f58 797 printf("[PHYSAC] physics body index is out of bounds");
jstephens78 0:e39efa4f4f58 798 #endif
jstephens78 0:e39efa4f4f58 799
jstephens78 0:e39efa4f4f58 800 return bodies[index];
jstephens78 0:e39efa4f4f58 801 }
jstephens78 0:e39efa4f4f58 802
jstephens78 0:e39efa4f4f58 803 // Returns the physics body shape type (PHYSICS_CIRCLE or PHYSICS_POLYGON)
jstephens78 0:e39efa4f4f58 804 PHYSACDEF int GetPhysicsShapeType(int index)
jstephens78 0:e39efa4f4f58 805 {
jstephens78 0:e39efa4f4f58 806 int result = -1;
jstephens78 0:e39efa4f4f58 807
jstephens78 0:e39efa4f4f58 808 if (index < physicsBodiesCount)
jstephens78 0:e39efa4f4f58 809 {
jstephens78 0:e39efa4f4f58 810 if (bodies[index] != NULL)
jstephens78 0:e39efa4f4f58 811 result = bodies[index]->shape.type;
jstephens78 0:e39efa4f4f58 812
jstephens78 0:e39efa4f4f58 813 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 814 else
jstephens78 0:e39efa4f4f58 815 printf("[PHYSAC] error when trying to get a null reference physics body");
jstephens78 0:e39efa4f4f58 816 #endif
jstephens78 0:e39efa4f4f58 817 }
jstephens78 0:e39efa4f4f58 818 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 819 else
jstephens78 0:e39efa4f4f58 820 printf("[PHYSAC] physics body index is out of bounds");
jstephens78 0:e39efa4f4f58 821 #endif
jstephens78 0:e39efa4f4f58 822
jstephens78 0:e39efa4f4f58 823 return result;
jstephens78 0:e39efa4f4f58 824 }
jstephens78 0:e39efa4f4f58 825
jstephens78 0:e39efa4f4f58 826 // Returns the amount of vertices of a physics body shape
jstephens78 0:e39efa4f4f58 827 PHYSACDEF int GetPhysicsShapeVerticesCount(int index)
jstephens78 0:e39efa4f4f58 828 {
jstephens78 0:e39efa4f4f58 829 int result = 0;
jstephens78 0:e39efa4f4f58 830
jstephens78 0:e39efa4f4f58 831 if (index < physicsBodiesCount)
jstephens78 0:e39efa4f4f58 832 {
jstephens78 0:e39efa4f4f58 833 if (bodies[index] != NULL)
jstephens78 0:e39efa4f4f58 834 {
jstephens78 0:e39efa4f4f58 835 switch (bodies[index]->shape.type)
jstephens78 0:e39efa4f4f58 836 {
jstephens78 0:e39efa4f4f58 837 case PHYSICS_CIRCLE: result = PHYSAC_CIRCLE_VERTICES; break;
jstephens78 0:e39efa4f4f58 838 case PHYSICS_POLYGON: result = bodies[index]->shape.vertexData.vertexCount; break;
jstephens78 0:e39efa4f4f58 839 default: break;
jstephens78 0:e39efa4f4f58 840 }
jstephens78 0:e39efa4f4f58 841 }
jstephens78 0:e39efa4f4f58 842 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 843 else
jstephens78 0:e39efa4f4f58 844 printf("[PHYSAC] error when trying to get a null reference physics body");
jstephens78 0:e39efa4f4f58 845 #endif
jstephens78 0:e39efa4f4f58 846 }
jstephens78 0:e39efa4f4f58 847 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 848 else
jstephens78 0:e39efa4f4f58 849 printf("[PHYSAC] physics body index is out of bounds");
jstephens78 0:e39efa4f4f58 850 #endif
jstephens78 0:e39efa4f4f58 851
jstephens78 0:e39efa4f4f58 852 return result;
jstephens78 0:e39efa4f4f58 853 }
jstephens78 0:e39efa4f4f58 854
jstephens78 0:e39efa4f4f58 855 // Returns transformed position of a body shape (body position + vertex transformed position)
jstephens78 0:e39efa4f4f58 856 PHYSACDEF Vector2 GetPhysicsShapeVertex(PhysicsBody body, int vertex)
jstephens78 0:e39efa4f4f58 857 {
jstephens78 0:e39efa4f4f58 858 Vector2 position = { 0.0f, 0.0f };
jstephens78 0:e39efa4f4f58 859
jstephens78 0:e39efa4f4f58 860 if (body != NULL)
jstephens78 0:e39efa4f4f58 861 {
jstephens78 0:e39efa4f4f58 862 switch (body->shape.type)
jstephens78 0:e39efa4f4f58 863 {
jstephens78 0:e39efa4f4f58 864 case PHYSICS_CIRCLE:
jstephens78 0:e39efa4f4f58 865 {
jstephens78 0:e39efa4f4f58 866 position.x = body->position.x + cosf(360.0f/PHYSAC_CIRCLE_VERTICES*vertex*PHYSAC_DEG2RAD)*body->shape.radius;
jstephens78 0:e39efa4f4f58 867 position.y = body->position.y + sinf(360.0f/PHYSAC_CIRCLE_VERTICES*vertex*PHYSAC_DEG2RAD)*body->shape.radius;
jstephens78 0:e39efa4f4f58 868 } break;
jstephens78 0:e39efa4f4f58 869 case PHYSICS_POLYGON:
jstephens78 0:e39efa4f4f58 870 {
jstephens78 0:e39efa4f4f58 871 PolygonData vertexData = body->shape.vertexData;
jstephens78 0:e39efa4f4f58 872 position = Vector2Add(body->position, Mat2MultiplyVector2(body->shape.transform, vertexData.positions[vertex]));
jstephens78 0:e39efa4f4f58 873 } break;
jstephens78 0:e39efa4f4f58 874 default: break;
jstephens78 0:e39efa4f4f58 875 }
jstephens78 0:e39efa4f4f58 876 }
jstephens78 0:e39efa4f4f58 877 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 878 else
jstephens78 0:e39efa4f4f58 879 printf("[PHYSAC] error when trying to get a null reference physics body");
jstephens78 0:e39efa4f4f58 880 #endif
jstephens78 0:e39efa4f4f58 881
jstephens78 0:e39efa4f4f58 882 return position;
jstephens78 0:e39efa4f4f58 883 }
jstephens78 0:e39efa4f4f58 884
jstephens78 0:e39efa4f4f58 885 // Sets physics body shape transform based on radians parameter
jstephens78 0:e39efa4f4f58 886 PHYSACDEF void SetPhysicsBodyRotation(PhysicsBody body, float radians)
jstephens78 0:e39efa4f4f58 887 {
jstephens78 0:e39efa4f4f58 888 if (body != NULL)
jstephens78 0:e39efa4f4f58 889 {
jstephens78 0:e39efa4f4f58 890 body->orient = radians;
jstephens78 0:e39efa4f4f58 891
jstephens78 0:e39efa4f4f58 892 if (body->shape.type == PHYSICS_POLYGON)
jstephens78 0:e39efa4f4f58 893 body->shape.transform = Mat2Radians(radians);
jstephens78 0:e39efa4f4f58 894 }
jstephens78 0:e39efa4f4f58 895 }
jstephens78 0:e39efa4f4f58 896
jstephens78 0:e39efa4f4f58 897 // Unitializes and destroys a physics body
jstephens78 0:e39efa4f4f58 898 PHYSACDEF void DestroyPhysicsBody(PhysicsBody body)
jstephens78 0:e39efa4f4f58 899 {
jstephens78 0:e39efa4f4f58 900 if (body != NULL)
jstephens78 0:e39efa4f4f58 901 {
jstephens78 0:e39efa4f4f58 902 int id = body->id;
jstephens78 0:e39efa4f4f58 903 int index = -1;
jstephens78 0:e39efa4f4f58 904
jstephens78 0:e39efa4f4f58 905 for (int i = 0; i < physicsBodiesCount; i++)
jstephens78 0:e39efa4f4f58 906 {
jstephens78 0:e39efa4f4f58 907 if (bodies[i]->id == id)
jstephens78 0:e39efa4f4f58 908 {
jstephens78 0:e39efa4f4f58 909 index = i;
jstephens78 0:e39efa4f4f58 910 break;
jstephens78 0:e39efa4f4f58 911 }
jstephens78 0:e39efa4f4f58 912 }
jstephens78 0:e39efa4f4f58 913
jstephens78 0:e39efa4f4f58 914 if (index == -1)
jstephens78 0:e39efa4f4f58 915 {
jstephens78 0:e39efa4f4f58 916 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 917 printf("[PHYSAC] Not possible to find body id %i in pointers array\n", id);
jstephens78 0:e39efa4f4f58 918 #endif
jstephens78 0:e39efa4f4f58 919 return;
jstephens78 0:e39efa4f4f58 920 }
jstephens78 0:e39efa4f4f58 921
jstephens78 0:e39efa4f4f58 922 // Free body allocated memory
jstephens78 0:e39efa4f4f58 923 PHYSAC_FREE(body);
jstephens78 0:e39efa4f4f58 924 usedMemory -= sizeof(PhysicsBodyData);
jstephens78 0:e39efa4f4f58 925 bodies[index] = NULL;
jstephens78 0:e39efa4f4f58 926
jstephens78 0:e39efa4f4f58 927 // Reorder physics bodies pointers array and its catched index
jstephens78 0:e39efa4f4f58 928 for (int i = index; i < physicsBodiesCount; i++)
jstephens78 0:e39efa4f4f58 929 {
jstephens78 0:e39efa4f4f58 930 if ((i + 1) < physicsBodiesCount)
jstephens78 0:e39efa4f4f58 931 bodies[i] = bodies[i + 1];
jstephens78 0:e39efa4f4f58 932 }
jstephens78 0:e39efa4f4f58 933
jstephens78 0:e39efa4f4f58 934 // Update physics bodies count
jstephens78 0:e39efa4f4f58 935 physicsBodiesCount--;
jstephens78 0:e39efa4f4f58 936
jstephens78 0:e39efa4f4f58 937 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 938 printf("[PHYSAC] destroyed physics body id %i\n", id);
jstephens78 0:e39efa4f4f58 939 #endif
jstephens78 0:e39efa4f4f58 940 }
jstephens78 0:e39efa4f4f58 941 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 942 else
jstephens78 0:e39efa4f4f58 943 printf("[PHYSAC] error trying to destroy a null referenced body\n");
jstephens78 0:e39efa4f4f58 944 #endif
jstephens78 0:e39efa4f4f58 945 }
jstephens78 0:e39efa4f4f58 946
jstephens78 0:e39efa4f4f58 947 // Unitializes physics pointers and exits physics loop thread
jstephens78 0:e39efa4f4f58 948 PHYSACDEF void ClosePhysics(void)
jstephens78 0:e39efa4f4f58 949 {
jstephens78 0:e39efa4f4f58 950 // Exit physics loop thread
jstephens78 0:e39efa4f4f58 951 physicsThreadEnabled = false;
jstephens78 0:e39efa4f4f58 952
jstephens78 0:e39efa4f4f58 953 #if !defined(PHYSAC_NO_THREADS)
jstephens78 0:e39efa4f4f58 954 pthread_join(physicsThreadId, NULL);
jstephens78 0:e39efa4f4f58 955 #endif
jstephens78 0:e39efa4f4f58 956
jstephens78 0:e39efa4f4f58 957 // Unitialize physics manifolds dynamic memory allocations
jstephens78 0:e39efa4f4f58 958 for (int i = physicsManifoldsCount - 1; i >= 0; i--)
jstephens78 0:e39efa4f4f58 959 DestroyPhysicsManifold(contacts[i]);
jstephens78 0:e39efa4f4f58 960
jstephens78 0:e39efa4f4f58 961 // Unitialize physics bodies dynamic memory allocations
jstephens78 0:e39efa4f4f58 962 for (int i = physicsBodiesCount - 1; i >= 0; i--)
jstephens78 0:e39efa4f4f58 963 DestroyPhysicsBody(bodies[i]);
jstephens78 0:e39efa4f4f58 964
jstephens78 0:e39efa4f4f58 965 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 966 if (physicsBodiesCount > 0 || usedMemory != 0)
jstephens78 0:e39efa4f4f58 967 printf("[PHYSAC] physics module closed with %i still allocated bodies [MEMORY: %i bytes]\n", physicsBodiesCount, usedMemory);
jstephens78 0:e39efa4f4f58 968 else if (physicsManifoldsCount > 0 || usedMemory != 0)
jstephens78 0:e39efa4f4f58 969 printf("[PHYSAC] physics module closed with %i still allocated manifolds [MEMORY: %i bytes]\n", physicsManifoldsCount, usedMemory);
jstephens78 0:e39efa4f4f58 970 else
jstephens78 0:e39efa4f4f58 971 printf("[PHYSAC] physics module closed successfully\n");
jstephens78 0:e39efa4f4f58 972 #endif
jstephens78 0:e39efa4f4f58 973 }
jstephens78 0:e39efa4f4f58 974
jstephens78 0:e39efa4f4f58 975 //----------------------------------------------------------------------------------
jstephens78 0:e39efa4f4f58 976 // Module Internal Functions Definition
jstephens78 0:e39efa4f4f58 977 //----------------------------------------------------------------------------------
jstephens78 0:e39efa4f4f58 978 // Finds a valid index for a new physics body initialization
jstephens78 0:e39efa4f4f58 979 static int FindAvailableBodyIndex()
jstephens78 0:e39efa4f4f58 980 {
jstephens78 0:e39efa4f4f58 981 int index = -1;
jstephens78 0:e39efa4f4f58 982 for (int i = 0; i < PHYSAC_MAX_BODIES; i++)
jstephens78 0:e39efa4f4f58 983 {
jstephens78 0:e39efa4f4f58 984 int currentId = i;
jstephens78 0:e39efa4f4f58 985
jstephens78 0:e39efa4f4f58 986 // Check if current id already exist in other physics body
jstephens78 0:e39efa4f4f58 987 for (int k = 0; k < physicsBodiesCount; k++)
jstephens78 0:e39efa4f4f58 988 {
jstephens78 0:e39efa4f4f58 989 if (bodies[k]->id == currentId)
jstephens78 0:e39efa4f4f58 990 {
jstephens78 0:e39efa4f4f58 991 currentId++;
jstephens78 0:e39efa4f4f58 992 break;
jstephens78 0:e39efa4f4f58 993 }
jstephens78 0:e39efa4f4f58 994 }
jstephens78 0:e39efa4f4f58 995
jstephens78 0:e39efa4f4f58 996 // If it is not used, use it as new physics body id
jstephens78 0:e39efa4f4f58 997 if (currentId == i)
jstephens78 0:e39efa4f4f58 998 {
jstephens78 0:e39efa4f4f58 999 index = i;
jstephens78 0:e39efa4f4f58 1000 break;
jstephens78 0:e39efa4f4f58 1001 }
jstephens78 0:e39efa4f4f58 1002 }
jstephens78 0:e39efa4f4f58 1003
jstephens78 0:e39efa4f4f58 1004 return index;
jstephens78 0:e39efa4f4f58 1005 }
jstephens78 0:e39efa4f4f58 1006
jstephens78 0:e39efa4f4f58 1007 // Creates a random polygon shape with max vertex distance from polygon pivot
jstephens78 0:e39efa4f4f58 1008 static PolygonData CreateRandomPolygon(float radius, int sides)
jstephens78 0:e39efa4f4f58 1009 {
jstephens78 0:e39efa4f4f58 1010 PolygonData data = { 0 };
jstephens78 0:e39efa4f4f58 1011 data.vertexCount = sides;
jstephens78 0:e39efa4f4f58 1012
jstephens78 0:e39efa4f4f58 1013 // Calculate polygon vertices positions
jstephens78 0:e39efa4f4f58 1014 for (int i = 0; i < data.vertexCount; i++)
jstephens78 0:e39efa4f4f58 1015 {
jstephens78 0:e39efa4f4f58 1016 data.positions[i].x = cosf(360.0f/sides*i*PHYSAC_DEG2RAD)*radius;
jstephens78 0:e39efa4f4f58 1017 data.positions[i].y = sinf(360.0f/sides*i*PHYSAC_DEG2RAD)*radius;
jstephens78 0:e39efa4f4f58 1018 }
jstephens78 0:e39efa4f4f58 1019
jstephens78 0:e39efa4f4f58 1020 // Calculate polygon faces normals
jstephens78 0:e39efa4f4f58 1021 for (int i = 0; i < data.vertexCount; i++)
jstephens78 0:e39efa4f4f58 1022 {
jstephens78 0:e39efa4f4f58 1023 int nextIndex = (((i + 1) < sides) ? (i + 1) : 0);
jstephens78 0:e39efa4f4f58 1024 Vector2 face = Vector2Subtract(data.positions[nextIndex], data.positions[i]);
jstephens78 0:e39efa4f4f58 1025
jstephens78 0:e39efa4f4f58 1026 data.normals[i] = (Vector2){ face.y, -face.x };
jstephens78 0:e39efa4f4f58 1027 MathNormalize(&data.normals[i]);
jstephens78 0:e39efa4f4f58 1028 }
jstephens78 0:e39efa4f4f58 1029
jstephens78 0:e39efa4f4f58 1030 return data;
jstephens78 0:e39efa4f4f58 1031 }
jstephens78 0:e39efa4f4f58 1032
jstephens78 0:e39efa4f4f58 1033 // Creates a rectangle polygon shape based on a min and max positions
jstephens78 0:e39efa4f4f58 1034 static PolygonData CreateRectanglePolygon(Vector2 pos, Vector2 size)
jstephens78 0:e39efa4f4f58 1035 {
jstephens78 0:e39efa4f4f58 1036 PolygonData data = { 0 };
jstephens78 0:e39efa4f4f58 1037 data.vertexCount = 4;
jstephens78 0:e39efa4f4f58 1038
jstephens78 0:e39efa4f4f58 1039 // Calculate polygon vertices positions
jstephens78 0:e39efa4f4f58 1040 data.positions[0] = (Vector2){ pos.x + size.x/2, pos.y - size.y/2 };
jstephens78 0:e39efa4f4f58 1041 data.positions[1] = (Vector2){ pos.x + size.x/2, pos.y + size.y/2 };
jstephens78 0:e39efa4f4f58 1042 data.positions[2] = (Vector2){ pos.x - size.x/2, pos.y + size.y/2 };
jstephens78 0:e39efa4f4f58 1043 data.positions[3] = (Vector2){ pos.x - size.x/2, pos.y - size.y/2 };
jstephens78 0:e39efa4f4f58 1044
jstephens78 0:e39efa4f4f58 1045 // Calculate polygon faces normals
jstephens78 0:e39efa4f4f58 1046 for (int i = 0; i < data.vertexCount; i++)
jstephens78 0:e39efa4f4f58 1047 {
jstephens78 0:e39efa4f4f58 1048 int nextIndex = (((i + 1) < data.vertexCount) ? (i + 1) : 0);
jstephens78 0:e39efa4f4f58 1049 Vector2 face = Vector2Subtract(data.positions[nextIndex], data.positions[i]);
jstephens78 0:e39efa4f4f58 1050
jstephens78 0:e39efa4f4f58 1051 data.normals[i] = (Vector2){ face.y, -face.x };
jstephens78 0:e39efa4f4f58 1052 MathNormalize(&data.normals[i]);
jstephens78 0:e39efa4f4f58 1053 }
jstephens78 0:e39efa4f4f58 1054
jstephens78 0:e39efa4f4f58 1055 return data;
jstephens78 0:e39efa4f4f58 1056 }
jstephens78 0:e39efa4f4f58 1057
jstephens78 0:e39efa4f4f58 1058 // Physics loop thread function
jstephens78 0:e39efa4f4f58 1059 static void *PhysicsLoop(void *arg)
jstephens78 0:e39efa4f4f58 1060 {
jstephens78 0:e39efa4f4f58 1061 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 1062 printf("[PHYSAC] physics thread created successfully\n");
jstephens78 0:e39efa4f4f58 1063 #endif
jstephens78 0:e39efa4f4f58 1064
jstephens78 0:e39efa4f4f58 1065 // Initialize physics loop thread values
jstephens78 0:e39efa4f4f58 1066 physicsThreadEnabled = true;
jstephens78 0:e39efa4f4f58 1067
jstephens78 0:e39efa4f4f58 1068 // Physics update loop
jstephens78 0:e39efa4f4f58 1069 while (physicsThreadEnabled)
jstephens78 0:e39efa4f4f58 1070 {
jstephens78 0:e39efa4f4f58 1071 RunPhysicsStep();
jstephens78 0:e39efa4f4f58 1072 }
jstephens78 0:e39efa4f4f58 1073
jstephens78 0:e39efa4f4f58 1074 return NULL;
jstephens78 0:e39efa4f4f58 1075 }
jstephens78 0:e39efa4f4f58 1076
jstephens78 0:e39efa4f4f58 1077 // Physics steps calculations (dynamics, collisions and position corrections)
jstephens78 0:e39efa4f4f58 1078 static void PhysicsStep(void)
jstephens78 0:e39efa4f4f58 1079 {
jstephens78 0:e39efa4f4f58 1080 // Update current steps count
jstephens78 0:e39efa4f4f58 1081 stepsCount++;
jstephens78 0:e39efa4f4f58 1082
jstephens78 0:e39efa4f4f58 1083 // Clear previous generated collisions information
jstephens78 0:e39efa4f4f58 1084 for (int i = physicsManifoldsCount - 1; i >= 0; i--)
jstephens78 0:e39efa4f4f58 1085 {
jstephens78 0:e39efa4f4f58 1086 PhysicsManifold manifold = contacts[i];
jstephens78 0:e39efa4f4f58 1087
jstephens78 0:e39efa4f4f58 1088 if (manifold != NULL)
jstephens78 0:e39efa4f4f58 1089 DestroyPhysicsManifold(manifold);
jstephens78 0:e39efa4f4f58 1090 }
jstephens78 0:e39efa4f4f58 1091
jstephens78 0:e39efa4f4f58 1092 // Reset physics bodies grounded state
jstephens78 0:e39efa4f4f58 1093 for (int i = 0; i < physicsBodiesCount; i++)
jstephens78 0:e39efa4f4f58 1094 {
jstephens78 0:e39efa4f4f58 1095 PhysicsBody body = bodies[i];
jstephens78 0:e39efa4f4f58 1096 body->isGrounded = false;
jstephens78 0:e39efa4f4f58 1097 }
jstephens78 0:e39efa4f4f58 1098
jstephens78 0:e39efa4f4f58 1099 // Generate new collision information
jstephens78 0:e39efa4f4f58 1100 for (int i = 0; i < physicsBodiesCount; i++)
jstephens78 0:e39efa4f4f58 1101 {
jstephens78 0:e39efa4f4f58 1102 PhysicsBody bodyA = bodies[i];
jstephens78 0:e39efa4f4f58 1103
jstephens78 0:e39efa4f4f58 1104 if (bodyA != NULL)
jstephens78 0:e39efa4f4f58 1105 {
jstephens78 0:e39efa4f4f58 1106 for (int j = i + 1; j < physicsBodiesCount; j++)
jstephens78 0:e39efa4f4f58 1107 {
jstephens78 0:e39efa4f4f58 1108 PhysicsBody bodyB = bodies[j];
jstephens78 0:e39efa4f4f58 1109
jstephens78 0:e39efa4f4f58 1110 if (bodyB != NULL)
jstephens78 0:e39efa4f4f58 1111 {
jstephens78 0:e39efa4f4f58 1112 if ((bodyA->inverseMass == 0) && (bodyB->inverseMass == 0))
jstephens78 0:e39efa4f4f58 1113 continue;
jstephens78 0:e39efa4f4f58 1114
jstephens78 0:e39efa4f4f58 1115 PhysicsManifold manifold = CreatePhysicsManifold(bodyA, bodyB);
jstephens78 0:e39efa4f4f58 1116 SolvePhysicsManifold(manifold);
jstephens78 0:e39efa4f4f58 1117
jstephens78 0:e39efa4f4f58 1118 if (manifold->contactsCount > 0)
jstephens78 0:e39efa4f4f58 1119 {
jstephens78 0:e39efa4f4f58 1120 // Create a new manifold with same information as previously solved manifold and add it to the manifolds pool last slot
jstephens78 0:e39efa4f4f58 1121 PhysicsManifold newManifold = CreatePhysicsManifold(bodyA, bodyB);
jstephens78 0:e39efa4f4f58 1122 newManifold->penetration = manifold->penetration;
jstephens78 0:e39efa4f4f58 1123 newManifold->normal = manifold->normal;
jstephens78 0:e39efa4f4f58 1124 newManifold->contacts[0] = manifold->contacts[0];
jstephens78 0:e39efa4f4f58 1125 newManifold->contacts[1] = manifold->contacts[1];
jstephens78 0:e39efa4f4f58 1126 newManifold->contactsCount = manifold->contactsCount;
jstephens78 0:e39efa4f4f58 1127 newManifold->restitution = manifold->restitution;
jstephens78 0:e39efa4f4f58 1128 newManifold->dynamicFriction = manifold->dynamicFriction;
jstephens78 0:e39efa4f4f58 1129 newManifold->staticFriction = manifold->staticFriction;
jstephens78 0:e39efa4f4f58 1130 }
jstephens78 0:e39efa4f4f58 1131 }
jstephens78 0:e39efa4f4f58 1132 }
jstephens78 0:e39efa4f4f58 1133 }
jstephens78 0:e39efa4f4f58 1134 }
jstephens78 0:e39efa4f4f58 1135
jstephens78 0:e39efa4f4f58 1136 // Integrate forces to physics bodies
jstephens78 0:e39efa4f4f58 1137 for (int i = 0; i < physicsBodiesCount; i++)
jstephens78 0:e39efa4f4f58 1138 {
jstephens78 0:e39efa4f4f58 1139 PhysicsBody body = bodies[i];
jstephens78 0:e39efa4f4f58 1140
jstephens78 0:e39efa4f4f58 1141 if (body != NULL)
jstephens78 0:e39efa4f4f58 1142 IntegratePhysicsForces(body);
jstephens78 0:e39efa4f4f58 1143 }
jstephens78 0:e39efa4f4f58 1144
jstephens78 0:e39efa4f4f58 1145 // Initialize physics manifolds to solve collisions
jstephens78 0:e39efa4f4f58 1146 for (int i = 0; i < physicsManifoldsCount; i++)
jstephens78 0:e39efa4f4f58 1147 {
jstephens78 0:e39efa4f4f58 1148 PhysicsManifold manifold = contacts[i];
jstephens78 0:e39efa4f4f58 1149
jstephens78 0:e39efa4f4f58 1150 if (manifold != NULL)
jstephens78 0:e39efa4f4f58 1151 InitializePhysicsManifolds(manifold);
jstephens78 0:e39efa4f4f58 1152 }
jstephens78 0:e39efa4f4f58 1153
jstephens78 0:e39efa4f4f58 1154 // Integrate physics collisions impulses to solve collisions
jstephens78 0:e39efa4f4f58 1155 for (int i = 0; i < PHYSAC_COLLISION_ITERATIONS; i++)
jstephens78 0:e39efa4f4f58 1156 {
jstephens78 0:e39efa4f4f58 1157 for (int j = 0; j < physicsManifoldsCount; j++)
jstephens78 0:e39efa4f4f58 1158 {
jstephens78 0:e39efa4f4f58 1159 PhysicsManifold manifold = contacts[i];
jstephens78 0:e39efa4f4f58 1160
jstephens78 0:e39efa4f4f58 1161 if (manifold != NULL)
jstephens78 0:e39efa4f4f58 1162 IntegratePhysicsImpulses(manifold);
jstephens78 0:e39efa4f4f58 1163 }
jstephens78 0:e39efa4f4f58 1164 }
jstephens78 0:e39efa4f4f58 1165
jstephens78 0:e39efa4f4f58 1166 // Integrate velocity to physics bodies
jstephens78 0:e39efa4f4f58 1167 for (int i = 0; i < physicsBodiesCount; i++)
jstephens78 0:e39efa4f4f58 1168 {
jstephens78 0:e39efa4f4f58 1169 PhysicsBody body = bodies[i];
jstephens78 0:e39efa4f4f58 1170
jstephens78 0:e39efa4f4f58 1171 if (body != NULL)
jstephens78 0:e39efa4f4f58 1172 IntegratePhysicsVelocity(body);
jstephens78 0:e39efa4f4f58 1173 }
jstephens78 0:e39efa4f4f58 1174
jstephens78 0:e39efa4f4f58 1175 // Correct physics bodies positions based on manifolds collision information
jstephens78 0:e39efa4f4f58 1176 for (int i = 0; i < physicsManifoldsCount; i++)
jstephens78 0:e39efa4f4f58 1177 {
jstephens78 0:e39efa4f4f58 1178 PhysicsManifold manifold = contacts[i];
jstephens78 0:e39efa4f4f58 1179
jstephens78 0:e39efa4f4f58 1180 if (manifold != NULL)
jstephens78 0:e39efa4f4f58 1181 CorrectPhysicsPositions(manifold);
jstephens78 0:e39efa4f4f58 1182 }
jstephens78 0:e39efa4f4f58 1183
jstephens78 0:e39efa4f4f58 1184 // Clear physics bodies forces
jstephens78 0:e39efa4f4f58 1185 for (int i = 0; i < physicsBodiesCount; i++)
jstephens78 0:e39efa4f4f58 1186 {
jstephens78 0:e39efa4f4f58 1187 PhysicsBody body = bodies[i];
jstephens78 0:e39efa4f4f58 1188
jstephens78 0:e39efa4f4f58 1189 if (body != NULL)
jstephens78 0:e39efa4f4f58 1190 {
jstephens78 0:e39efa4f4f58 1191 body->force = PHYSAC_VECTOR_ZERO;
jstephens78 0:e39efa4f4f58 1192 body->torque = 0.0f;
jstephens78 0:e39efa4f4f58 1193 }
jstephens78 0:e39efa4f4f58 1194 }
jstephens78 0:e39efa4f4f58 1195 }
jstephens78 0:e39efa4f4f58 1196
jstephens78 0:e39efa4f4f58 1197 // Wrapper to ensure PhysicsStep is run with at a fixed time step
jstephens78 0:e39efa4f4f58 1198 PHYSACDEF void RunPhysicsStep(void)
jstephens78 0:e39efa4f4f58 1199 {
jstephens78 0:e39efa4f4f58 1200 // Calculate current time
jstephens78 0:e39efa4f4f58 1201 currentTime = GetCurrentTime();
jstephens78 0:e39efa4f4f58 1202
jstephens78 0:e39efa4f4f58 1203 // Calculate current delta time
jstephens78 0:e39efa4f4f58 1204 const double delta = currentTime - startTime;
jstephens78 0:e39efa4f4f58 1205
jstephens78 0:e39efa4f4f58 1206 // Store the time elapsed since the last frame began
jstephens78 0:e39efa4f4f58 1207 accumulator += delta;
jstephens78 0:e39efa4f4f58 1208
jstephens78 0:e39efa4f4f58 1209 // Fixed time stepping loop
jstephens78 0:e39efa4f4f58 1210 while (accumulator >= deltaTime)
jstephens78 0:e39efa4f4f58 1211 {
jstephens78 0:e39efa4f4f58 1212 PhysicsStep();
jstephens78 0:e39efa4f4f58 1213 accumulator -= deltaTime;
jstephens78 0:e39efa4f4f58 1214 }
jstephens78 0:e39efa4f4f58 1215
jstephens78 0:e39efa4f4f58 1216 // Record the starting of this frame
jstephens78 0:e39efa4f4f58 1217 startTime = currentTime;
jstephens78 0:e39efa4f4f58 1218 }
jstephens78 0:e39efa4f4f58 1219
jstephens78 0:e39efa4f4f58 1220 PHYSACDEF void SetPhysicsTimeStep(double delta)
jstephens78 0:e39efa4f4f58 1221 {
jstephens78 0:e39efa4f4f58 1222 deltaTime = delta;
jstephens78 0:e39efa4f4f58 1223 }
jstephens78 0:e39efa4f4f58 1224
jstephens78 0:e39efa4f4f58 1225 // Finds a valid index for a new manifold initialization
jstephens78 0:e39efa4f4f58 1226 static int FindAvailableManifoldIndex()
jstephens78 0:e39efa4f4f58 1227 {
jstephens78 0:e39efa4f4f58 1228 int index = -1;
jstephens78 0:e39efa4f4f58 1229 for (int i = 0; i < PHYSAC_MAX_MANIFOLDS; i++)
jstephens78 0:e39efa4f4f58 1230 {
jstephens78 0:e39efa4f4f58 1231 int currentId = i;
jstephens78 0:e39efa4f4f58 1232
jstephens78 0:e39efa4f4f58 1233 // Check if current id already exist in other physics body
jstephens78 0:e39efa4f4f58 1234 for (int k = 0; k < physicsManifoldsCount; k++)
jstephens78 0:e39efa4f4f58 1235 {
jstephens78 0:e39efa4f4f58 1236 if (contacts[k]->id == currentId)
jstephens78 0:e39efa4f4f58 1237 {
jstephens78 0:e39efa4f4f58 1238 currentId++;
jstephens78 0:e39efa4f4f58 1239 break;
jstephens78 0:e39efa4f4f58 1240 }
jstephens78 0:e39efa4f4f58 1241 }
jstephens78 0:e39efa4f4f58 1242
jstephens78 0:e39efa4f4f58 1243 // If it is not used, use it as new physics body id
jstephens78 0:e39efa4f4f58 1244 if (currentId == i)
jstephens78 0:e39efa4f4f58 1245 {
jstephens78 0:e39efa4f4f58 1246 index = i;
jstephens78 0:e39efa4f4f58 1247 break;
jstephens78 0:e39efa4f4f58 1248 }
jstephens78 0:e39efa4f4f58 1249 }
jstephens78 0:e39efa4f4f58 1250
jstephens78 0:e39efa4f4f58 1251 return index;
jstephens78 0:e39efa4f4f58 1252 }
jstephens78 0:e39efa4f4f58 1253
jstephens78 0:e39efa4f4f58 1254 // Creates a new physics manifold to solve collision
jstephens78 0:e39efa4f4f58 1255 static PhysicsManifold CreatePhysicsManifold(PhysicsBody a, PhysicsBody b)
jstephens78 0:e39efa4f4f58 1256 {
jstephens78 0:e39efa4f4f58 1257 PhysicsManifold newManifold = (PhysicsManifold)PHYSAC_MALLOC(sizeof(PhysicsManifoldData));
jstephens78 0:e39efa4f4f58 1258 usedMemory += sizeof(PhysicsManifoldData);
jstephens78 0:e39efa4f4f58 1259
jstephens78 0:e39efa4f4f58 1260 int newId = FindAvailableManifoldIndex();
jstephens78 0:e39efa4f4f58 1261 if (newId != -1)
jstephens78 0:e39efa4f4f58 1262 {
jstephens78 0:e39efa4f4f58 1263 // Initialize new manifold with generic values
jstephens78 0:e39efa4f4f58 1264 newManifold->id = newId;
jstephens78 0:e39efa4f4f58 1265 newManifold->bodyA = a;
jstephens78 0:e39efa4f4f58 1266 newManifold->bodyB = b;
jstephens78 0:e39efa4f4f58 1267 newManifold->penetration = 0;
jstephens78 0:e39efa4f4f58 1268 newManifold->normal = PHYSAC_VECTOR_ZERO;
jstephens78 0:e39efa4f4f58 1269 newManifold->contacts[0] = PHYSAC_VECTOR_ZERO;
jstephens78 0:e39efa4f4f58 1270 newManifold->contacts[1] = PHYSAC_VECTOR_ZERO;
jstephens78 0:e39efa4f4f58 1271 newManifold->contactsCount = 0;
jstephens78 0:e39efa4f4f58 1272 newManifold->restitution = 0.0f;
jstephens78 0:e39efa4f4f58 1273 newManifold->dynamicFriction = 0.0f;
jstephens78 0:e39efa4f4f58 1274 newManifold->staticFriction = 0.0f;
jstephens78 0:e39efa4f4f58 1275
jstephens78 0:e39efa4f4f58 1276 // Add new body to bodies pointers array and update bodies count
jstephens78 0:e39efa4f4f58 1277 contacts[physicsManifoldsCount] = newManifold;
jstephens78 0:e39efa4f4f58 1278 physicsManifoldsCount++;
jstephens78 0:e39efa4f4f58 1279 }
jstephens78 0:e39efa4f4f58 1280 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 1281 else
jstephens78 0:e39efa4f4f58 1282 printf("[PHYSAC] new physics manifold creation failed because there is any available id to use\n");
jstephens78 0:e39efa4f4f58 1283 #endif
jstephens78 0:e39efa4f4f58 1284
jstephens78 0:e39efa4f4f58 1285 return newManifold;
jstephens78 0:e39efa4f4f58 1286 }
jstephens78 0:e39efa4f4f58 1287
jstephens78 0:e39efa4f4f58 1288 // Unitializes and destroys a physics manifold
jstephens78 0:e39efa4f4f58 1289 static void DestroyPhysicsManifold(PhysicsManifold manifold)
jstephens78 0:e39efa4f4f58 1290 {
jstephens78 0:e39efa4f4f58 1291 if (manifold != NULL)
jstephens78 0:e39efa4f4f58 1292 {
jstephens78 0:e39efa4f4f58 1293 int id = manifold->id;
jstephens78 0:e39efa4f4f58 1294 int index = -1;
jstephens78 0:e39efa4f4f58 1295
jstephens78 0:e39efa4f4f58 1296 for (int i = 0; i < physicsManifoldsCount; i++)
jstephens78 0:e39efa4f4f58 1297 {
jstephens78 0:e39efa4f4f58 1298 if (contacts[i]->id == id)
jstephens78 0:e39efa4f4f58 1299 {
jstephens78 0:e39efa4f4f58 1300 index = i;
jstephens78 0:e39efa4f4f58 1301 break;
jstephens78 0:e39efa4f4f58 1302 }
jstephens78 0:e39efa4f4f58 1303 }
jstephens78 0:e39efa4f4f58 1304
jstephens78 0:e39efa4f4f58 1305 if (index == -1)
jstephens78 0:e39efa4f4f58 1306 {
jstephens78 0:e39efa4f4f58 1307 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 1308 printf("[PHYSAC] Not possible to manifold id %i in pointers array\n", id);
jstephens78 0:e39efa4f4f58 1309 #endif
jstephens78 0:e39efa4f4f58 1310 return;
jstephens78 0:e39efa4f4f58 1311 }
jstephens78 0:e39efa4f4f58 1312
jstephens78 0:e39efa4f4f58 1313 // Free manifold allocated memory
jstephens78 0:e39efa4f4f58 1314 PHYSAC_FREE(manifold);
jstephens78 0:e39efa4f4f58 1315 usedMemory -= sizeof(PhysicsManifoldData);
jstephens78 0:e39efa4f4f58 1316 contacts[index] = NULL;
jstephens78 0:e39efa4f4f58 1317
jstephens78 0:e39efa4f4f58 1318 // Reorder physics manifolds pointers array and its catched index
jstephens78 0:e39efa4f4f58 1319 for (int i = index; i < physicsManifoldsCount; i++)
jstephens78 0:e39efa4f4f58 1320 {
jstephens78 0:e39efa4f4f58 1321 if ((i + 1) < physicsManifoldsCount)
jstephens78 0:e39efa4f4f58 1322 contacts[i] = contacts[i + 1];
jstephens78 0:e39efa4f4f58 1323 }
jstephens78 0:e39efa4f4f58 1324
jstephens78 0:e39efa4f4f58 1325 // Update physics manifolds count
jstephens78 0:e39efa4f4f58 1326 physicsManifoldsCount--;
jstephens78 0:e39efa4f4f58 1327 }
jstephens78 0:e39efa4f4f58 1328 #if defined(PHYSAC_DEBUG)
jstephens78 0:e39efa4f4f58 1329 else
jstephens78 0:e39efa4f4f58 1330 printf("[PHYSAC] error trying to destroy a null referenced manifold\n");
jstephens78 0:e39efa4f4f58 1331 #endif
jstephens78 0:e39efa4f4f58 1332 }
jstephens78 0:e39efa4f4f58 1333
jstephens78 0:e39efa4f4f58 1334 // Solves a created physics manifold between two physics bodies
jstephens78 0:e39efa4f4f58 1335 static void SolvePhysicsManifold(PhysicsManifold manifold)
jstephens78 0:e39efa4f4f58 1336 {
jstephens78 0:e39efa4f4f58 1337 switch (manifold->bodyA->shape.type)
jstephens78 0:e39efa4f4f58 1338 {
jstephens78 0:e39efa4f4f58 1339 case PHYSICS_CIRCLE:
jstephens78 0:e39efa4f4f58 1340 {
jstephens78 0:e39efa4f4f58 1341 switch (manifold->bodyB->shape.type)
jstephens78 0:e39efa4f4f58 1342 {
jstephens78 0:e39efa4f4f58 1343 case PHYSICS_CIRCLE: SolveCircleToCircle(manifold); break;
jstephens78 0:e39efa4f4f58 1344 case PHYSICS_POLYGON: SolveCircleToPolygon(manifold); break;
jstephens78 0:e39efa4f4f58 1345 default: break;
jstephens78 0:e39efa4f4f58 1346 }
jstephens78 0:e39efa4f4f58 1347 } break;
jstephens78 0:e39efa4f4f58 1348 case PHYSICS_POLYGON:
jstephens78 0:e39efa4f4f58 1349 {
jstephens78 0:e39efa4f4f58 1350 switch (manifold->bodyB->shape.type)
jstephens78 0:e39efa4f4f58 1351 {
jstephens78 0:e39efa4f4f58 1352 case PHYSICS_CIRCLE: SolvePolygonToCircle(manifold); break;
jstephens78 0:e39efa4f4f58 1353 case PHYSICS_POLYGON: SolvePolygonToPolygon(manifold); break;
jstephens78 0:e39efa4f4f58 1354 default: break;
jstephens78 0:e39efa4f4f58 1355 }
jstephens78 0:e39efa4f4f58 1356 } break;
jstephens78 0:e39efa4f4f58 1357 default: break;
jstephens78 0:e39efa4f4f58 1358 }
jstephens78 0:e39efa4f4f58 1359
jstephens78 0:e39efa4f4f58 1360 // Update physics body grounded state if normal direction is down and grounded state is not set yet in previous manifolds
jstephens78 0:e39efa4f4f58 1361 if (!manifold->bodyB->isGrounded)
jstephens78 0:e39efa4f4f58 1362 manifold->bodyB->isGrounded = (manifold->normal.y < 0);
jstephens78 0:e39efa4f4f58 1363 }
jstephens78 0:e39efa4f4f58 1364
jstephens78 0:e39efa4f4f58 1365 // Solves collision between two circle shape physics bodies
jstephens78 0:e39efa4f4f58 1366 static void SolveCircleToCircle(PhysicsManifold manifold)
jstephens78 0:e39efa4f4f58 1367 {
jstephens78 0:e39efa4f4f58 1368 PhysicsBody bodyA = manifold->bodyA;
jstephens78 0:e39efa4f4f58 1369 PhysicsBody bodyB = manifold->bodyB;
jstephens78 0:e39efa4f4f58 1370
jstephens78 0:e39efa4f4f58 1371 if ((bodyA == NULL) || (bodyB == NULL))
jstephens78 0:e39efa4f4f58 1372 return;
jstephens78 0:e39efa4f4f58 1373
jstephens78 0:e39efa4f4f58 1374 // Calculate translational vector, which is normal
jstephens78 0:e39efa4f4f58 1375 Vector2 normal = Vector2Subtract(bodyB->position, bodyA->position);
jstephens78 0:e39efa4f4f58 1376
jstephens78 0:e39efa4f4f58 1377 float distSqr = MathLenSqr(normal);
jstephens78 0:e39efa4f4f58 1378 float radius = bodyA->shape.radius + bodyB->shape.radius;
jstephens78 0:e39efa4f4f58 1379
jstephens78 0:e39efa4f4f58 1380 // Check if circles are not in contact
jstephens78 0:e39efa4f4f58 1381 if (distSqr >= radius*radius)
jstephens78 0:e39efa4f4f58 1382 {
jstephens78 0:e39efa4f4f58 1383 manifold->contactsCount = 0;
jstephens78 0:e39efa4f4f58 1384 return;
jstephens78 0:e39efa4f4f58 1385 }
jstephens78 0:e39efa4f4f58 1386
jstephens78 0:e39efa4f4f58 1387 float distance = sqrtf(distSqr);
jstephens78 0:e39efa4f4f58 1388 manifold->contactsCount = 1;
jstephens78 0:e39efa4f4f58 1389
jstephens78 0:e39efa4f4f58 1390 if (distance == 0.0f)
jstephens78 0:e39efa4f4f58 1391 {
jstephens78 0:e39efa4f4f58 1392 manifold->penetration = bodyA->shape.radius;
jstephens78 0:e39efa4f4f58 1393 manifold->normal = (Vector2){ 1.0f, 0.0f };
jstephens78 0:e39efa4f4f58 1394 manifold->contacts[0] = bodyA->position;
jstephens78 0:e39efa4f4f58 1395 }
jstephens78 0:e39efa4f4f58 1396 else
jstephens78 0:e39efa4f4f58 1397 {
jstephens78 0:e39efa4f4f58 1398 manifold->penetration = radius - distance;
jstephens78 0:e39efa4f4f58 1399 manifold->normal = (Vector2){ normal.x/distance, normal.y/distance }; // Faster than using MathNormalize() due to sqrt is already performed
jstephens78 0:e39efa4f4f58 1400 manifold->contacts[0] = (Vector2){ manifold->normal.x*bodyA->shape.radius + bodyA->position.x, manifold->normal.y*bodyA->shape.radius + bodyA->position.y };
jstephens78 0:e39efa4f4f58 1401 }
jstephens78 0:e39efa4f4f58 1402
jstephens78 0:e39efa4f4f58 1403 // Update physics body grounded state if normal direction is down
jstephens78 0:e39efa4f4f58 1404 if (!bodyA->isGrounded)
jstephens78 0:e39efa4f4f58 1405 bodyA->isGrounded = (manifold->normal.y < 0);
jstephens78 0:e39efa4f4f58 1406 }
jstephens78 0:e39efa4f4f58 1407
jstephens78 0:e39efa4f4f58 1408 // Solves collision between a circle to a polygon shape physics bodies
jstephens78 0:e39efa4f4f58 1409 static void SolveCircleToPolygon(PhysicsManifold manifold)
jstephens78 0:e39efa4f4f58 1410 {
jstephens78 0:e39efa4f4f58 1411 PhysicsBody bodyA = manifold->bodyA;
jstephens78 0:e39efa4f4f58 1412 PhysicsBody bodyB = manifold->bodyB;
jstephens78 0:e39efa4f4f58 1413
jstephens78 0:e39efa4f4f58 1414 if ((bodyA == NULL) || (bodyB == NULL))
jstephens78 0:e39efa4f4f58 1415 return;
jstephens78 0:e39efa4f4f58 1416
jstephens78 0:e39efa4f4f58 1417 SolveDifferentShapes(manifold, bodyA, bodyB);
jstephens78 0:e39efa4f4f58 1418 }
jstephens78 0:e39efa4f4f58 1419
jstephens78 0:e39efa4f4f58 1420 // Solves collision between a circle to a polygon shape physics bodies
jstephens78 0:e39efa4f4f58 1421 static void SolvePolygonToCircle(PhysicsManifold manifold)
jstephens78 0:e39efa4f4f58 1422 {
jstephens78 0:e39efa4f4f58 1423 PhysicsBody bodyA = manifold->bodyA;
jstephens78 0:e39efa4f4f58 1424 PhysicsBody bodyB = manifold->bodyB;
jstephens78 0:e39efa4f4f58 1425
jstephens78 0:e39efa4f4f58 1426 if ((bodyA == NULL) || (bodyB == NULL))
jstephens78 0:e39efa4f4f58 1427 return;
jstephens78 0:e39efa4f4f58 1428
jstephens78 0:e39efa4f4f58 1429 SolveDifferentShapes(manifold, bodyB, bodyA);
jstephens78 0:e39efa4f4f58 1430
jstephens78 0:e39efa4f4f58 1431 manifold->normal.x *= -1.0f;
jstephens78 0:e39efa4f4f58 1432 manifold->normal.y *= -1.0f;
jstephens78 0:e39efa4f4f58 1433 }
jstephens78 0:e39efa4f4f58 1434
jstephens78 0:e39efa4f4f58 1435 // Solve collision between two different types of shapes
jstephens78 0:e39efa4f4f58 1436 static void SolveDifferentShapes(PhysicsManifold manifold, PhysicsBody bodyA, PhysicsBody bodyB)
jstephens78 0:e39efa4f4f58 1437 {
jstephens78 0:e39efa4f4f58 1438 manifold->contactsCount = 0;
jstephens78 0:e39efa4f4f58 1439
jstephens78 0:e39efa4f4f58 1440 // Transform circle center to polygon transform space
jstephens78 0:e39efa4f4f58 1441 Vector2 center = bodyA->position;
jstephens78 0:e39efa4f4f58 1442 center = Mat2MultiplyVector2(Mat2Transpose(bodyB->shape.transform), Vector2Subtract(center, bodyB->position));
jstephens78 0:e39efa4f4f58 1443
jstephens78 0:e39efa4f4f58 1444 // Find edge with minimum penetration
jstephens78 0:e39efa4f4f58 1445 // It is the same concept as using support points in SolvePolygonToPolygon
jstephens78 0:e39efa4f4f58 1446 float separation = -PHYSAC_FLT_MAX;
jstephens78 0:e39efa4f4f58 1447 int faceNormal = 0;
jstephens78 0:e39efa4f4f58 1448 PolygonData vertexData = bodyB->shape.vertexData;
jstephens78 0:e39efa4f4f58 1449
jstephens78 0:e39efa4f4f58 1450 for (int i = 0; i < vertexData.vertexCount; i++)
jstephens78 0:e39efa4f4f58 1451 {
jstephens78 0:e39efa4f4f58 1452 float currentSeparation = MathDot(vertexData.normals[i], Vector2Subtract(center, vertexData.positions[i]));
jstephens78 0:e39efa4f4f58 1453
jstephens78 0:e39efa4f4f58 1454 if (currentSeparation > bodyA->shape.radius)
jstephens78 0:e39efa4f4f58 1455 return;
jstephens78 0:e39efa4f4f58 1456
jstephens78 0:e39efa4f4f58 1457 if (currentSeparation > separation)
jstephens78 0:e39efa4f4f58 1458 {
jstephens78 0:e39efa4f4f58 1459 separation = currentSeparation;
jstephens78 0:e39efa4f4f58 1460 faceNormal = i;
jstephens78 0:e39efa4f4f58 1461 }
jstephens78 0:e39efa4f4f58 1462 }
jstephens78 0:e39efa4f4f58 1463
jstephens78 0:e39efa4f4f58 1464 // Grab face's vertices
jstephens78 0:e39efa4f4f58 1465 Vector2 v1 = vertexData.positions[faceNormal];
jstephens78 0:e39efa4f4f58 1466 int nextIndex = (((faceNormal + 1) < vertexData.vertexCount) ? (faceNormal + 1) : 0);
jstephens78 0:e39efa4f4f58 1467 Vector2 v2 = vertexData.positions[nextIndex];
jstephens78 0:e39efa4f4f58 1468
jstephens78 0:e39efa4f4f58 1469 // Check to see if center is within polygon
jstephens78 0:e39efa4f4f58 1470 if (separation < PHYSAC_EPSILON)
jstephens78 0:e39efa4f4f58 1471 {
jstephens78 0:e39efa4f4f58 1472 manifold->contactsCount = 1;
jstephens78 0:e39efa4f4f58 1473 Vector2 normal = Mat2MultiplyVector2(bodyB->shape.transform, vertexData.normals[faceNormal]);
jstephens78 0:e39efa4f4f58 1474 manifold->normal = (Vector2){ -normal.x, -normal.y };
jstephens78 0:e39efa4f4f58 1475 manifold->contacts[0] = (Vector2){ manifold->normal.x*bodyA->shape.radius + bodyA->position.x, manifold->normal.y*bodyA->shape.radius + bodyA->position.y };
jstephens78 0:e39efa4f4f58 1476 manifold->penetration = bodyA->shape.radius;
jstephens78 0:e39efa4f4f58 1477 return;
jstephens78 0:e39efa4f4f58 1478 }
jstephens78 0:e39efa4f4f58 1479
jstephens78 0:e39efa4f4f58 1480 // Determine which voronoi region of the edge center of circle lies within
jstephens78 0:e39efa4f4f58 1481 float dot1 = MathDot(Vector2Subtract(center, v1), Vector2Subtract(v2, v1));
jstephens78 0:e39efa4f4f58 1482 float dot2 = MathDot(Vector2Subtract(center, v2), Vector2Subtract(v1, v2));
jstephens78 0:e39efa4f4f58 1483 manifold->penetration = bodyA->shape.radius - separation;
jstephens78 0:e39efa4f4f58 1484
jstephens78 0:e39efa4f4f58 1485 if (dot1 <= 0.0f) // Closest to v1
jstephens78 0:e39efa4f4f58 1486 {
jstephens78 0:e39efa4f4f58 1487 if (DistSqr(center, v1) > bodyA->shape.radius*bodyA->shape.radius)
jstephens78 0:e39efa4f4f58 1488 return;
jstephens78 0:e39efa4f4f58 1489
jstephens78 0:e39efa4f4f58 1490 manifold->contactsCount = 1;
jstephens78 0:e39efa4f4f58 1491 Vector2 normal = Vector2Subtract(v1, center);
jstephens78 0:e39efa4f4f58 1492 normal = Mat2MultiplyVector2(bodyB->shape.transform, normal);
jstephens78 0:e39efa4f4f58 1493 MathNormalize(&normal);
jstephens78 0:e39efa4f4f58 1494 manifold->normal = normal;
jstephens78 0:e39efa4f4f58 1495 v1 = Mat2MultiplyVector2(bodyB->shape.transform, v1);
jstephens78 0:e39efa4f4f58 1496 v1 = Vector2Add(v1, bodyB->position);
jstephens78 0:e39efa4f4f58 1497 manifold->contacts[0] = v1;
jstephens78 0:e39efa4f4f58 1498 }
jstephens78 0:e39efa4f4f58 1499 else if (dot2 <= 0.0f) // Closest to v2
jstephens78 0:e39efa4f4f58 1500 {
jstephens78 0:e39efa4f4f58 1501 if (DistSqr(center, v2) > bodyA->shape.radius*bodyA->shape.radius)
jstephens78 0:e39efa4f4f58 1502 return;
jstephens78 0:e39efa4f4f58 1503
jstephens78 0:e39efa4f4f58 1504 manifold->contactsCount = 1;
jstephens78 0:e39efa4f4f58 1505 Vector2 normal = Vector2Subtract(v2, center);
jstephens78 0:e39efa4f4f58 1506 v2 = Mat2MultiplyVector2(bodyB->shape.transform, v2);
jstephens78 0:e39efa4f4f58 1507 v2 = Vector2Add(v2, bodyB->position);
jstephens78 0:e39efa4f4f58 1508 manifold->contacts[0] = v2;
jstephens78 0:e39efa4f4f58 1509 normal = Mat2MultiplyVector2(bodyB->shape.transform, normal);
jstephens78 0:e39efa4f4f58 1510 MathNormalize(&normal);
jstephens78 0:e39efa4f4f58 1511 manifold->normal = normal;
jstephens78 0:e39efa4f4f58 1512 }
jstephens78 0:e39efa4f4f58 1513 else // Closest to face
jstephens78 0:e39efa4f4f58 1514 {
jstephens78 0:e39efa4f4f58 1515 Vector2 normal = vertexData.normals[faceNormal];
jstephens78 0:e39efa4f4f58 1516
jstephens78 0:e39efa4f4f58 1517 if (MathDot(Vector2Subtract(center, v1), normal) > bodyA->shape.radius)
jstephens78 0:e39efa4f4f58 1518 return;
jstephens78 0:e39efa4f4f58 1519
jstephens78 0:e39efa4f4f58 1520 normal = Mat2MultiplyVector2(bodyB->shape.transform, normal);
jstephens78 0:e39efa4f4f58 1521 manifold->normal = (Vector2){ -normal.x, -normal.y };
jstephens78 0:e39efa4f4f58 1522 manifold->contacts[0] = (Vector2){ manifold->normal.x*bodyA->shape.radius + bodyA->position.x, manifold->normal.y*bodyA->shape.radius + bodyA->position.y };
jstephens78 0:e39efa4f4f58 1523 manifold->contactsCount = 1;
jstephens78 0:e39efa4f4f58 1524 }
jstephens78 0:e39efa4f4f58 1525 }
jstephens78 0:e39efa4f4f58 1526
jstephens78 0:e39efa4f4f58 1527 // Solves collision between two polygons shape physics bodies
jstephens78 0:e39efa4f4f58 1528 static void SolvePolygonToPolygon(PhysicsManifold manifold)
jstephens78 0:e39efa4f4f58 1529 {
jstephens78 0:e39efa4f4f58 1530 if ((manifold->bodyA == NULL) || (manifold->bodyB == NULL))
jstephens78 0:e39efa4f4f58 1531 return;
jstephens78 0:e39efa4f4f58 1532
jstephens78 0:e39efa4f4f58 1533 PhysicsShape bodyA = manifold->bodyA->shape;
jstephens78 0:e39efa4f4f58 1534 PhysicsShape bodyB = manifold->bodyB->shape;
jstephens78 0:e39efa4f4f58 1535 manifold->contactsCount = 0;
jstephens78 0:e39efa4f4f58 1536
jstephens78 0:e39efa4f4f58 1537 // Check for separating axis with A shape's face planes
jstephens78 0:e39efa4f4f58 1538 int faceA = 0;
jstephens78 0:e39efa4f4f58 1539 float penetrationA = FindAxisLeastPenetration(&faceA, bodyA, bodyB);
jstephens78 0:e39efa4f4f58 1540
jstephens78 0:e39efa4f4f58 1541 if (penetrationA >= 0.0f)
jstephens78 0:e39efa4f4f58 1542 return;
jstephens78 0:e39efa4f4f58 1543
jstephens78 0:e39efa4f4f58 1544 // Check for separating axis with B shape's face planes
jstephens78 0:e39efa4f4f58 1545 int faceB = 0;
jstephens78 0:e39efa4f4f58 1546 float penetrationB = FindAxisLeastPenetration(&faceB, bodyB, bodyA);
jstephens78 0:e39efa4f4f58 1547
jstephens78 0:e39efa4f4f58 1548 if (penetrationB >= 0.0f)
jstephens78 0:e39efa4f4f58 1549 return;
jstephens78 0:e39efa4f4f58 1550
jstephens78 0:e39efa4f4f58 1551 int referenceIndex = 0;
jstephens78 0:e39efa4f4f58 1552 bool flip = false; // Always point from A shape to B shape
jstephens78 0:e39efa4f4f58 1553
jstephens78 0:e39efa4f4f58 1554 PhysicsShape refPoly; // Reference
jstephens78 0:e39efa4f4f58 1555 PhysicsShape incPoly; // Incident
jstephens78 0:e39efa4f4f58 1556
jstephens78 0:e39efa4f4f58 1557 // Determine which shape contains reference face
jstephens78 0:e39efa4f4f58 1558 if (BiasGreaterThan(penetrationA, penetrationB))
jstephens78 0:e39efa4f4f58 1559 {
jstephens78 0:e39efa4f4f58 1560 refPoly = bodyA;
jstephens78 0:e39efa4f4f58 1561 incPoly = bodyB;
jstephens78 0:e39efa4f4f58 1562 referenceIndex = faceA;
jstephens78 0:e39efa4f4f58 1563 }
jstephens78 0:e39efa4f4f58 1564 else
jstephens78 0:e39efa4f4f58 1565 {
jstephens78 0:e39efa4f4f58 1566 refPoly = bodyB;
jstephens78 0:e39efa4f4f58 1567 incPoly = bodyA;
jstephens78 0:e39efa4f4f58 1568 referenceIndex = faceB;
jstephens78 0:e39efa4f4f58 1569 flip = true;
jstephens78 0:e39efa4f4f58 1570 }
jstephens78 0:e39efa4f4f58 1571
jstephens78 0:e39efa4f4f58 1572 // World space incident face
jstephens78 0:e39efa4f4f58 1573 Vector2 incidentFace[2];
jstephens78 0:e39efa4f4f58 1574 FindIncidentFace(&incidentFace[0], &incidentFace[1], refPoly, incPoly, referenceIndex);
jstephens78 0:e39efa4f4f58 1575
jstephens78 0:e39efa4f4f58 1576 // Setup reference face vertices
jstephens78 0:e39efa4f4f58 1577 PolygonData refData = refPoly.vertexData;
jstephens78 0:e39efa4f4f58 1578 Vector2 v1 = refData.positions[referenceIndex];
jstephens78 0:e39efa4f4f58 1579 referenceIndex = (((referenceIndex + 1) < refData.vertexCount) ? (referenceIndex + 1) : 0);
jstephens78 0:e39efa4f4f58 1580 Vector2 v2 = refData.positions[referenceIndex];
jstephens78 0:e39efa4f4f58 1581
jstephens78 0:e39efa4f4f58 1582 // Transform vertices to world space
jstephens78 0:e39efa4f4f58 1583 v1 = Mat2MultiplyVector2(refPoly.transform, v1);
jstephens78 0:e39efa4f4f58 1584 v1 = Vector2Add(v1, refPoly.body->position);
jstephens78 0:e39efa4f4f58 1585 v2 = Mat2MultiplyVector2(refPoly.transform, v2);
jstephens78 0:e39efa4f4f58 1586 v2 = Vector2Add(v2, refPoly.body->position);
jstephens78 0:e39efa4f4f58 1587
jstephens78 0:e39efa4f4f58 1588 // Calculate reference face side normal in world space
jstephens78 0:e39efa4f4f58 1589 Vector2 sidePlaneNormal = Vector2Subtract(v2, v1);
jstephens78 0:e39efa4f4f58 1590 MathNormalize(&sidePlaneNormal);
jstephens78 0:e39efa4f4f58 1591
jstephens78 0:e39efa4f4f58 1592 // Orthogonalize
jstephens78 0:e39efa4f4f58 1593 Vector2 refFaceNormal = { sidePlaneNormal.y, -sidePlaneNormal.x };
jstephens78 0:e39efa4f4f58 1594 float refC = MathDot(refFaceNormal, v1);
jstephens78 0:e39efa4f4f58 1595 float negSide = MathDot(sidePlaneNormal, v1)*-1;
jstephens78 0:e39efa4f4f58 1596 float posSide = MathDot(sidePlaneNormal, v2);
jstephens78 0:e39efa4f4f58 1597
jstephens78 0:e39efa4f4f58 1598 // Clip incident face to reference face side planes (due to floating point error, possible to not have required points
jstephens78 0:e39efa4f4f58 1599 if (Clip((Vector2){ -sidePlaneNormal.x, -sidePlaneNormal.y }, negSide, &incidentFace[0], &incidentFace[1]) < 2)
jstephens78 0:e39efa4f4f58 1600 return;
jstephens78 0:e39efa4f4f58 1601
jstephens78 0:e39efa4f4f58 1602 if (Clip(sidePlaneNormal, posSide, &incidentFace[0], &incidentFace[1]) < 2)
jstephens78 0:e39efa4f4f58 1603 return;
jstephens78 0:e39efa4f4f58 1604
jstephens78 0:e39efa4f4f58 1605 // Flip normal if required
jstephens78 0:e39efa4f4f58 1606 manifold->normal = (flip ? (Vector2){ -refFaceNormal.x, -refFaceNormal.y } : refFaceNormal);
jstephens78 0:e39efa4f4f58 1607
jstephens78 0:e39efa4f4f58 1608 // Keep points behind reference face
jstephens78 0:e39efa4f4f58 1609 int currentPoint = 0; // Clipped points behind reference face
jstephens78 0:e39efa4f4f58 1610 float separation = MathDot(refFaceNormal, incidentFace[0]) - refC;
jstephens78 0:e39efa4f4f58 1611
jstephens78 0:e39efa4f4f58 1612 if (separation <= 0.0f)
jstephens78 0:e39efa4f4f58 1613 {
jstephens78 0:e39efa4f4f58 1614 manifold->contacts[currentPoint] = incidentFace[0];
jstephens78 0:e39efa4f4f58 1615 manifold->penetration = -separation;
jstephens78 0:e39efa4f4f58 1616 currentPoint++;
jstephens78 0:e39efa4f4f58 1617 }
jstephens78 0:e39efa4f4f58 1618 else
jstephens78 0:e39efa4f4f58 1619 manifold->penetration = 0.0f;
jstephens78 0:e39efa4f4f58 1620
jstephens78 0:e39efa4f4f58 1621 separation = MathDot(refFaceNormal, incidentFace[1]) - refC;
jstephens78 0:e39efa4f4f58 1622
jstephens78 0:e39efa4f4f58 1623 if (separation <= 0.0f)
jstephens78 0:e39efa4f4f58 1624 {
jstephens78 0:e39efa4f4f58 1625 manifold->contacts[currentPoint] = incidentFace[1];
jstephens78 0:e39efa4f4f58 1626 manifold->penetration += -separation;
jstephens78 0:e39efa4f4f58 1627 currentPoint++;
jstephens78 0:e39efa4f4f58 1628
jstephens78 0:e39efa4f4f58 1629 // Calculate total penetration average
jstephens78 0:e39efa4f4f58 1630 manifold->penetration /= currentPoint;
jstephens78 0:e39efa4f4f58 1631 }
jstephens78 0:e39efa4f4f58 1632
jstephens78 0:e39efa4f4f58 1633 manifold->contactsCount = currentPoint;
jstephens78 0:e39efa4f4f58 1634 }
jstephens78 0:e39efa4f4f58 1635
jstephens78 0:e39efa4f4f58 1636 // Integrates physics forces into velocity
jstephens78 0:e39efa4f4f58 1637 static void IntegratePhysicsForces(PhysicsBody body)
jstephens78 0:e39efa4f4f58 1638 {
jstephens78 0:e39efa4f4f58 1639 if ((body == NULL) || (body->inverseMass == 0.0f) || !body->enabled)
jstephens78 0:e39efa4f4f58 1640 return;
jstephens78 0:e39efa4f4f58 1641
jstephens78 0:e39efa4f4f58 1642 body->velocity.x += (body->force.x*body->inverseMass)*(deltaTime/2.0);
jstephens78 0:e39efa4f4f58 1643 body->velocity.y += (body->force.y*body->inverseMass)*(deltaTime/2.0);
jstephens78 0:e39efa4f4f58 1644
jstephens78 0:e39efa4f4f58 1645 if (body->useGravity)
jstephens78 0:e39efa4f4f58 1646 {
jstephens78 0:e39efa4f4f58 1647 body->velocity.x += gravityForce.x*(deltaTime/1000/2.0);
jstephens78 0:e39efa4f4f58 1648 body->velocity.y += gravityForce.y*(deltaTime/1000/2.0);
jstephens78 0:e39efa4f4f58 1649 }
jstephens78 0:e39efa4f4f58 1650
jstephens78 0:e39efa4f4f58 1651 if (!body->freezeOrient)
jstephens78 0:e39efa4f4f58 1652 body->angularVelocity += body->torque*body->inverseInertia*(deltaTime/2.0);
jstephens78 0:e39efa4f4f58 1653 }
jstephens78 0:e39efa4f4f58 1654
jstephens78 0:e39efa4f4f58 1655 // Initializes physics manifolds to solve collisions
jstephens78 0:e39efa4f4f58 1656 static void InitializePhysicsManifolds(PhysicsManifold manifold)
jstephens78 0:e39efa4f4f58 1657 {
jstephens78 0:e39efa4f4f58 1658 PhysicsBody bodyA = manifold->bodyA;
jstephens78 0:e39efa4f4f58 1659 PhysicsBody bodyB = manifold->bodyB;
jstephens78 0:e39efa4f4f58 1660
jstephens78 0:e39efa4f4f58 1661 if ((bodyA == NULL) || (bodyB == NULL))
jstephens78 0:e39efa4f4f58 1662 return;
jstephens78 0:e39efa4f4f58 1663
jstephens78 0:e39efa4f4f58 1664 // Calculate average restitution, static and dynamic friction
jstephens78 0:e39efa4f4f58 1665 manifold->restitution = sqrtf(bodyA->restitution*bodyB->restitution);
jstephens78 0:e39efa4f4f58 1666 manifold->staticFriction = sqrtf(bodyA->staticFriction*bodyB->staticFriction);
jstephens78 0:e39efa4f4f58 1667 manifold->dynamicFriction = sqrtf(bodyA->dynamicFriction*bodyB->dynamicFriction);
jstephens78 0:e39efa4f4f58 1668
jstephens78 0:e39efa4f4f58 1669 for (int i = 0; i < manifold->contactsCount; i++)
jstephens78 0:e39efa4f4f58 1670 {
jstephens78 0:e39efa4f4f58 1671 // Caculate radius from center of mass to contact
jstephens78 0:e39efa4f4f58 1672 Vector2 radiusA = Vector2Subtract(manifold->contacts[i], bodyA->position);
jstephens78 0:e39efa4f4f58 1673 Vector2 radiusB = Vector2Subtract(manifold->contacts[i], bodyB->position);
jstephens78 0:e39efa4f4f58 1674
jstephens78 0:e39efa4f4f58 1675 Vector2 crossA = MathCross(bodyA->angularVelocity, radiusA);
jstephens78 0:e39efa4f4f58 1676 Vector2 crossB = MathCross(bodyB->angularVelocity, radiusB);
jstephens78 0:e39efa4f4f58 1677
jstephens78 0:e39efa4f4f58 1678 Vector2 radiusV = { 0.0f, 0.0f };
jstephens78 0:e39efa4f4f58 1679 radiusV.x = bodyB->velocity.x + crossB.x - bodyA->velocity.x - crossA.x;
jstephens78 0:e39efa4f4f58 1680 radiusV.y = bodyB->velocity.y + crossB.y - bodyA->velocity.y - crossA.y;
jstephens78 0:e39efa4f4f58 1681
jstephens78 0:e39efa4f4f58 1682 // Determine if we should perform a resting collision or not;
jstephens78 0:e39efa4f4f58 1683 // The idea is if the only thing moving this object is gravity, then the collision should be performed without any restitution
jstephens78 0:e39efa4f4f58 1684 if (MathLenSqr(radiusV) < (MathLenSqr((Vector2){ gravityForce.x*deltaTime/1000, gravityForce.y*deltaTime/1000 }) + PHYSAC_EPSILON))
jstephens78 0:e39efa4f4f58 1685 manifold->restitution = 0;
jstephens78 0:e39efa4f4f58 1686 }
jstephens78 0:e39efa4f4f58 1687 }
jstephens78 0:e39efa4f4f58 1688
jstephens78 0:e39efa4f4f58 1689 // Integrates physics collisions impulses to solve collisions
jstephens78 0:e39efa4f4f58 1690 static void IntegratePhysicsImpulses(PhysicsManifold manifold)
jstephens78 0:e39efa4f4f58 1691 {
jstephens78 0:e39efa4f4f58 1692 PhysicsBody bodyA = manifold->bodyA;
jstephens78 0:e39efa4f4f58 1693 PhysicsBody bodyB = manifold->bodyB;
jstephens78 0:e39efa4f4f58 1694
jstephens78 0:e39efa4f4f58 1695 if ((bodyA == NULL) || (bodyB == NULL))
jstephens78 0:e39efa4f4f58 1696 return;
jstephens78 0:e39efa4f4f58 1697
jstephens78 0:e39efa4f4f58 1698 // Early out and positional correct if both objects have infinite mass
jstephens78 0:e39efa4f4f58 1699 if (fabs(bodyA->inverseMass + bodyB->inverseMass) <= PHYSAC_EPSILON)
jstephens78 0:e39efa4f4f58 1700 {
jstephens78 0:e39efa4f4f58 1701 bodyA->velocity = PHYSAC_VECTOR_ZERO;
jstephens78 0:e39efa4f4f58 1702 bodyB->velocity = PHYSAC_VECTOR_ZERO;
jstephens78 0:e39efa4f4f58 1703 return;
jstephens78 0:e39efa4f4f58 1704 }
jstephens78 0:e39efa4f4f58 1705
jstephens78 0:e39efa4f4f58 1706 for (int i = 0; i < manifold->contactsCount; i++)
jstephens78 0:e39efa4f4f58 1707 {
jstephens78 0:e39efa4f4f58 1708 // Calculate radius from center of mass to contact
jstephens78 0:e39efa4f4f58 1709 Vector2 radiusA = Vector2Subtract(manifold->contacts[i], bodyA->position);
jstephens78 0:e39efa4f4f58 1710 Vector2 radiusB = Vector2Subtract(manifold->contacts[i], bodyB->position);
jstephens78 0:e39efa4f4f58 1711
jstephens78 0:e39efa4f4f58 1712 // Calculate relative velocity
jstephens78 0:e39efa4f4f58 1713 Vector2 radiusV = { 0.0f, 0.0f };
jstephens78 0:e39efa4f4f58 1714 radiusV.x = bodyB->velocity.x + MathCross(bodyB->angularVelocity, radiusB).x - bodyA->velocity.x - MathCross(bodyA->angularVelocity, radiusA).x;
jstephens78 0:e39efa4f4f58 1715 radiusV.y = bodyB->velocity.y + MathCross(bodyB->angularVelocity, radiusB).y - bodyA->velocity.y - MathCross(bodyA->angularVelocity, radiusA).y;
jstephens78 0:e39efa4f4f58 1716
jstephens78 0:e39efa4f4f58 1717 // Relative velocity along the normal
jstephens78 0:e39efa4f4f58 1718 float contactVelocity = MathDot(radiusV, manifold->normal);
jstephens78 0:e39efa4f4f58 1719
jstephens78 0:e39efa4f4f58 1720 // Do not resolve if velocities are separating
jstephens78 0:e39efa4f4f58 1721 if (contactVelocity > 0.0f)
jstephens78 0:e39efa4f4f58 1722 return;
jstephens78 0:e39efa4f4f58 1723
jstephens78 0:e39efa4f4f58 1724 float raCrossN = MathCrossVector2(radiusA, manifold->normal);
jstephens78 0:e39efa4f4f58 1725 float rbCrossN = MathCrossVector2(radiusB, manifold->normal);
jstephens78 0:e39efa4f4f58 1726
jstephens78 0:e39efa4f4f58 1727 float inverseMassSum = bodyA->inverseMass + bodyB->inverseMass + (raCrossN*raCrossN)*bodyA->inverseInertia + (rbCrossN*rbCrossN)*bodyB->inverseInertia;
jstephens78 0:e39efa4f4f58 1728
jstephens78 0:e39efa4f4f58 1729 // Calculate impulse scalar value
jstephens78 0:e39efa4f4f58 1730 float impulse = -(1.0f + manifold->restitution)*contactVelocity;
jstephens78 0:e39efa4f4f58 1731 impulse /= inverseMassSum;
jstephens78 0:e39efa4f4f58 1732 impulse /= (float)manifold->contactsCount;
jstephens78 0:e39efa4f4f58 1733
jstephens78 0:e39efa4f4f58 1734 // Apply impulse to each physics body
jstephens78 0:e39efa4f4f58 1735 Vector2 impulseV = { manifold->normal.x*impulse, manifold->normal.y*impulse };
jstephens78 0:e39efa4f4f58 1736
jstephens78 0:e39efa4f4f58 1737 if (bodyA->enabled)
jstephens78 0:e39efa4f4f58 1738 {
jstephens78 0:e39efa4f4f58 1739 bodyA->velocity.x += bodyA->inverseMass*(-impulseV.x);
jstephens78 0:e39efa4f4f58 1740 bodyA->velocity.y += bodyA->inverseMass*(-impulseV.y);
jstephens78 0:e39efa4f4f58 1741
jstephens78 0:e39efa4f4f58 1742 if (!bodyA->freezeOrient)
jstephens78 0:e39efa4f4f58 1743 bodyA->angularVelocity += bodyA->inverseInertia*MathCrossVector2(radiusA, (Vector2){ -impulseV.x, -impulseV.y });
jstephens78 0:e39efa4f4f58 1744 }
jstephens78 0:e39efa4f4f58 1745
jstephens78 0:e39efa4f4f58 1746 if (bodyB->enabled)
jstephens78 0:e39efa4f4f58 1747 {
jstephens78 0:e39efa4f4f58 1748 bodyB->velocity.x += bodyB->inverseMass*(impulseV.x);
jstephens78 0:e39efa4f4f58 1749 bodyB->velocity.y += bodyB->inverseMass*(impulseV.y);
jstephens78 0:e39efa4f4f58 1750
jstephens78 0:e39efa4f4f58 1751 if (!bodyB->freezeOrient)
jstephens78 0:e39efa4f4f58 1752 bodyB->angularVelocity += bodyB->inverseInertia*MathCrossVector2(radiusB, impulseV);
jstephens78 0:e39efa4f4f58 1753 }
jstephens78 0:e39efa4f4f58 1754
jstephens78 0:e39efa4f4f58 1755 // Apply friction impulse to each physics body
jstephens78 0:e39efa4f4f58 1756 radiusV.x = bodyB->velocity.x + MathCross(bodyB->angularVelocity, radiusB).x - bodyA->velocity.x - MathCross(bodyA->angularVelocity, radiusA).x;
jstephens78 0:e39efa4f4f58 1757 radiusV.y = bodyB->velocity.y + MathCross(bodyB->angularVelocity, radiusB).y - bodyA->velocity.y - MathCross(bodyA->angularVelocity, radiusA).y;
jstephens78 0:e39efa4f4f58 1758
jstephens78 0:e39efa4f4f58 1759 Vector2 tangent = { radiusV.x - (manifold->normal.x*MathDot(radiusV, manifold->normal)), radiusV.y - (manifold->normal.y*MathDot(radiusV, manifold->normal)) };
jstephens78 0:e39efa4f4f58 1760 MathNormalize(&tangent);
jstephens78 0:e39efa4f4f58 1761
jstephens78 0:e39efa4f4f58 1762 // Calculate impulse tangent magnitude
jstephens78 0:e39efa4f4f58 1763 float impulseTangent = -MathDot(radiusV, tangent);
jstephens78 0:e39efa4f4f58 1764 impulseTangent /= inverseMassSum;
jstephens78 0:e39efa4f4f58 1765 impulseTangent /= (float)manifold->contactsCount;
jstephens78 0:e39efa4f4f58 1766
jstephens78 0:e39efa4f4f58 1767 float absImpulseTangent = fabs(impulseTangent);
jstephens78 0:e39efa4f4f58 1768
jstephens78 0:e39efa4f4f58 1769 // Don't apply tiny friction impulses
jstephens78 0:e39efa4f4f58 1770 if (absImpulseTangent <= PHYSAC_EPSILON)
jstephens78 0:e39efa4f4f58 1771 return;
jstephens78 0:e39efa4f4f58 1772
jstephens78 0:e39efa4f4f58 1773 // Apply coulumb's law
jstephens78 0:e39efa4f4f58 1774 Vector2 tangentImpulse = { 0.0f, 0.0f };
jstephens78 0:e39efa4f4f58 1775 if (absImpulseTangent < impulse*manifold->staticFriction)
jstephens78 0:e39efa4f4f58 1776 tangentImpulse = (Vector2){ tangent.x*impulseTangent, tangent.y*impulseTangent };
jstephens78 0:e39efa4f4f58 1777 else
jstephens78 0:e39efa4f4f58 1778 tangentImpulse = (Vector2){ tangent.x*-impulse*manifold->dynamicFriction, tangent.y*-impulse*manifold->dynamicFriction };
jstephens78 0:e39efa4f4f58 1779
jstephens78 0:e39efa4f4f58 1780 // Apply friction impulse
jstephens78 0:e39efa4f4f58 1781 if (bodyA->enabled)
jstephens78 0:e39efa4f4f58 1782 {
jstephens78 0:e39efa4f4f58 1783 bodyA->velocity.x += bodyA->inverseMass*(-tangentImpulse.x);
jstephens78 0:e39efa4f4f58 1784 bodyA->velocity.y += bodyA->inverseMass*(-tangentImpulse.y);
jstephens78 0:e39efa4f4f58 1785
jstephens78 0:e39efa4f4f58 1786 if (!bodyA->freezeOrient)
jstephens78 0:e39efa4f4f58 1787 bodyA->angularVelocity += bodyA->inverseInertia*MathCrossVector2(radiusA, (Vector2){ -tangentImpulse.x, -tangentImpulse.y });
jstephens78 0:e39efa4f4f58 1788 }
jstephens78 0:e39efa4f4f58 1789
jstephens78 0:e39efa4f4f58 1790 if (bodyB->enabled)
jstephens78 0:e39efa4f4f58 1791 {
jstephens78 0:e39efa4f4f58 1792 bodyB->velocity.x += bodyB->inverseMass*(tangentImpulse.x);
jstephens78 0:e39efa4f4f58 1793 bodyB->velocity.y += bodyB->inverseMass*(tangentImpulse.y);
jstephens78 0:e39efa4f4f58 1794
jstephens78 0:e39efa4f4f58 1795 if (!bodyB->freezeOrient)
jstephens78 0:e39efa4f4f58 1796 bodyB->angularVelocity += bodyB->inverseInertia*MathCrossVector2(radiusB, tangentImpulse);
jstephens78 0:e39efa4f4f58 1797 }
jstephens78 0:e39efa4f4f58 1798 }
jstephens78 0:e39efa4f4f58 1799 }
jstephens78 0:e39efa4f4f58 1800
jstephens78 0:e39efa4f4f58 1801 // Integrates physics velocity into position and forces
jstephens78 0:e39efa4f4f58 1802 static void IntegratePhysicsVelocity(PhysicsBody body)
jstephens78 0:e39efa4f4f58 1803 {
jstephens78 0:e39efa4f4f58 1804 if ((body == NULL) ||!body->enabled)
jstephens78 0:e39efa4f4f58 1805 return;
jstephens78 0:e39efa4f4f58 1806
jstephens78 0:e39efa4f4f58 1807 body->position.x += body->velocity.x*deltaTime;
jstephens78 0:e39efa4f4f58 1808 body->position.y += body->velocity.y*deltaTime;
jstephens78 0:e39efa4f4f58 1809
jstephens78 0:e39efa4f4f58 1810 if (!body->freezeOrient)
jstephens78 0:e39efa4f4f58 1811 body->orient += body->angularVelocity*deltaTime;
jstephens78 0:e39efa4f4f58 1812
jstephens78 0:e39efa4f4f58 1813 Mat2Set(&body->shape.transform, body->orient);
jstephens78 0:e39efa4f4f58 1814
jstephens78 0:e39efa4f4f58 1815 IntegratePhysicsForces(body);
jstephens78 0:e39efa4f4f58 1816 }
jstephens78 0:e39efa4f4f58 1817
jstephens78 0:e39efa4f4f58 1818 // Corrects physics bodies positions based on manifolds collision information
jstephens78 0:e39efa4f4f58 1819 static void CorrectPhysicsPositions(PhysicsManifold manifold)
jstephens78 0:e39efa4f4f58 1820 {
jstephens78 0:e39efa4f4f58 1821 PhysicsBody bodyA = manifold->bodyA;
jstephens78 0:e39efa4f4f58 1822 PhysicsBody bodyB = manifold->bodyB;
jstephens78 0:e39efa4f4f58 1823
jstephens78 0:e39efa4f4f58 1824 if ((bodyA == NULL) || (bodyB == NULL))
jstephens78 0:e39efa4f4f58 1825 return;
jstephens78 0:e39efa4f4f58 1826
jstephens78 0:e39efa4f4f58 1827 Vector2 correction = { 0.0f, 0.0f };
jstephens78 0:e39efa4f4f58 1828 correction.x = (max(manifold->penetration - PHYSAC_PENETRATION_ALLOWANCE, 0.0f)/(bodyA->inverseMass + bodyB->inverseMass))*manifold->normal.x*PHYSAC_PENETRATION_CORRECTION;
jstephens78 0:e39efa4f4f58 1829 correction.y = (max(manifold->penetration - PHYSAC_PENETRATION_ALLOWANCE, 0.0f)/(bodyA->inverseMass + bodyB->inverseMass))*manifold->normal.y*PHYSAC_PENETRATION_CORRECTION;
jstephens78 0:e39efa4f4f58 1830
jstephens78 0:e39efa4f4f58 1831 if (bodyA->enabled)
jstephens78 0:e39efa4f4f58 1832 {
jstephens78 0:e39efa4f4f58 1833 bodyA->position.x -= correction.x*bodyA->inverseMass;
jstephens78 0:e39efa4f4f58 1834 bodyA->position.y -= correction.y*bodyA->inverseMass;
jstephens78 0:e39efa4f4f58 1835 }
jstephens78 0:e39efa4f4f58 1836
jstephens78 0:e39efa4f4f58 1837 if (bodyB->enabled)
jstephens78 0:e39efa4f4f58 1838 {
jstephens78 0:e39efa4f4f58 1839 bodyB->position.x += correction.x*bodyB->inverseMass;
jstephens78 0:e39efa4f4f58 1840 bodyB->position.y += correction.y*bodyB->inverseMass;
jstephens78 0:e39efa4f4f58 1841 }
jstephens78 0:e39efa4f4f58 1842 }
jstephens78 0:e39efa4f4f58 1843
jstephens78 0:e39efa4f4f58 1844 // Returns the extreme point along a direction within a polygon
jstephens78 0:e39efa4f4f58 1845 static Vector2 GetSupport(PhysicsShape shape, Vector2 dir)
jstephens78 0:e39efa4f4f58 1846 {
jstephens78 0:e39efa4f4f58 1847 float bestProjection = -PHYSAC_FLT_MAX;
jstephens78 0:e39efa4f4f58 1848 Vector2 bestVertex = { 0.0f, 0.0f };
jstephens78 0:e39efa4f4f58 1849 PolygonData data = shape.vertexData;
jstephens78 0:e39efa4f4f58 1850
jstephens78 0:e39efa4f4f58 1851 for (int i = 0; i < data.vertexCount; i++)
jstephens78 0:e39efa4f4f58 1852 {
jstephens78 0:e39efa4f4f58 1853 Vector2 vertex = data.positions[i];
jstephens78 0:e39efa4f4f58 1854 float projection = MathDot(vertex, dir);
jstephens78 0:e39efa4f4f58 1855
jstephens78 0:e39efa4f4f58 1856 if (projection > bestProjection)
jstephens78 0:e39efa4f4f58 1857 {
jstephens78 0:e39efa4f4f58 1858 bestVertex = vertex;
jstephens78 0:e39efa4f4f58 1859 bestProjection = projection;
jstephens78 0:e39efa4f4f58 1860 }
jstephens78 0:e39efa4f4f58 1861 }
jstephens78 0:e39efa4f4f58 1862
jstephens78 0:e39efa4f4f58 1863 return bestVertex;
jstephens78 0:e39efa4f4f58 1864 }
jstephens78 0:e39efa4f4f58 1865
jstephens78 0:e39efa4f4f58 1866 // Finds polygon shapes axis least penetration
jstephens78 0:e39efa4f4f58 1867 static float FindAxisLeastPenetration(int *faceIndex, PhysicsShape shapeA, PhysicsShape shapeB)
jstephens78 0:e39efa4f4f58 1868 {
jstephens78 0:e39efa4f4f58 1869 float bestDistance = -PHYSAC_FLT_MAX;
jstephens78 0:e39efa4f4f58 1870 int bestIndex = 0;
jstephens78 0:e39efa4f4f58 1871
jstephens78 0:e39efa4f4f58 1872 PolygonData dataA = shapeA.vertexData;
jstephens78 0:e39efa4f4f58 1873
jstephens78 0:e39efa4f4f58 1874 for (int i = 0; i < dataA.vertexCount; i++)
jstephens78 0:e39efa4f4f58 1875 {
jstephens78 0:e39efa4f4f58 1876 // Retrieve a face normal from A shape
jstephens78 0:e39efa4f4f58 1877 Vector2 normal = dataA.normals[i];
jstephens78 0:e39efa4f4f58 1878 Vector2 transNormal = Mat2MultiplyVector2(shapeA.transform, normal);
jstephens78 0:e39efa4f4f58 1879
jstephens78 0:e39efa4f4f58 1880 // Transform face normal into B shape's model space
jstephens78 0:e39efa4f4f58 1881 Mat2 buT = Mat2Transpose(shapeB.transform);
jstephens78 0:e39efa4f4f58 1882 normal = Mat2MultiplyVector2(buT, transNormal);
jstephens78 0:e39efa4f4f58 1883
jstephens78 0:e39efa4f4f58 1884 // Retrieve support point from B shape along -n
jstephens78 0:e39efa4f4f58 1885 Vector2 support = GetSupport(shapeB, (Vector2){ -normal.x, -normal.y });
jstephens78 0:e39efa4f4f58 1886
jstephens78 0:e39efa4f4f58 1887 // Retrieve vertex on face from A shape, transform into B shape's model space
jstephens78 0:e39efa4f4f58 1888 Vector2 vertex = dataA.positions[i];
jstephens78 0:e39efa4f4f58 1889 vertex = Mat2MultiplyVector2(shapeA.transform, vertex);
jstephens78 0:e39efa4f4f58 1890 vertex = Vector2Add(vertex, shapeA.body->position);
jstephens78 0:e39efa4f4f58 1891 vertex = Vector2Subtract(vertex, shapeB.body->position);
jstephens78 0:e39efa4f4f58 1892 vertex = Mat2MultiplyVector2(buT, vertex);
jstephens78 0:e39efa4f4f58 1893
jstephens78 0:e39efa4f4f58 1894 // Compute penetration distance in B shape's model space
jstephens78 0:e39efa4f4f58 1895 float distance = MathDot(normal, Vector2Subtract(support, vertex));
jstephens78 0:e39efa4f4f58 1896
jstephens78 0:e39efa4f4f58 1897 // Store greatest distance
jstephens78 0:e39efa4f4f58 1898 if (distance > bestDistance)
jstephens78 0:e39efa4f4f58 1899 {
jstephens78 0:e39efa4f4f58 1900 bestDistance = distance;
jstephens78 0:e39efa4f4f58 1901 bestIndex = i;
jstephens78 0:e39efa4f4f58 1902 }
jstephens78 0:e39efa4f4f58 1903 }
jstephens78 0:e39efa4f4f58 1904
jstephens78 0:e39efa4f4f58 1905 *faceIndex = bestIndex;
jstephens78 0:e39efa4f4f58 1906 return bestDistance;
jstephens78 0:e39efa4f4f58 1907 }
jstephens78 0:e39efa4f4f58 1908
jstephens78 0:e39efa4f4f58 1909 // Finds two polygon shapes incident face
jstephens78 0:e39efa4f4f58 1910 static void FindIncidentFace(Vector2 *v0, Vector2 *v1, PhysicsShape ref, PhysicsShape inc, int index)
jstephens78 0:e39efa4f4f58 1911 {
jstephens78 0:e39efa4f4f58 1912 PolygonData refData = ref.vertexData;
jstephens78 0:e39efa4f4f58 1913 PolygonData incData = inc.vertexData;
jstephens78 0:e39efa4f4f58 1914
jstephens78 0:e39efa4f4f58 1915 Vector2 referenceNormal = refData.normals[index];
jstephens78 0:e39efa4f4f58 1916
jstephens78 0:e39efa4f4f58 1917 // Calculate normal in incident's frame of reference
jstephens78 0:e39efa4f4f58 1918 referenceNormal = Mat2MultiplyVector2(ref.transform, referenceNormal); // To world space
jstephens78 0:e39efa4f4f58 1919 referenceNormal = Mat2MultiplyVector2(Mat2Transpose(inc.transform), referenceNormal); // To incident's model space
jstephens78 0:e39efa4f4f58 1920
jstephens78 0:e39efa4f4f58 1921 // Find most anti-normal face on polygon
jstephens78 0:e39efa4f4f58 1922 int incidentFace = 0;
jstephens78 0:e39efa4f4f58 1923 float minDot = PHYSAC_FLT_MAX;
jstephens78 0:e39efa4f4f58 1924
jstephens78 0:e39efa4f4f58 1925 for (int i = 0; i < incData.vertexCount; i++)
jstephens78 0:e39efa4f4f58 1926 {
jstephens78 0:e39efa4f4f58 1927 float dot = MathDot(referenceNormal, incData.normals[i]);
jstephens78 0:e39efa4f4f58 1928
jstephens78 0:e39efa4f4f58 1929 if (dot < minDot)
jstephens78 0:e39efa4f4f58 1930 {
jstephens78 0:e39efa4f4f58 1931 minDot = dot;
jstephens78 0:e39efa4f4f58 1932 incidentFace = i;
jstephens78 0:e39efa4f4f58 1933 }
jstephens78 0:e39efa4f4f58 1934 }
jstephens78 0:e39efa4f4f58 1935
jstephens78 0:e39efa4f4f58 1936 // Assign face vertices for incident face
jstephens78 0:e39efa4f4f58 1937 *v0 = Mat2MultiplyVector2(inc.transform, incData.positions[incidentFace]);
jstephens78 0:e39efa4f4f58 1938 *v0 = Vector2Add(*v0, inc.body->position);
jstephens78 0:e39efa4f4f58 1939 incidentFace = (((incidentFace + 1) < incData.vertexCount) ? (incidentFace + 1) : 0);
jstephens78 0:e39efa4f4f58 1940 *v1 = Mat2MultiplyVector2(inc.transform, incData.positions[incidentFace]);
jstephens78 0:e39efa4f4f58 1941 *v1 = Vector2Add(*v1, inc.body->position);
jstephens78 0:e39efa4f4f58 1942 }
jstephens78 0:e39efa4f4f58 1943
jstephens78 0:e39efa4f4f58 1944 // Calculates clipping based on a normal and two faces
jstephens78 0:e39efa4f4f58 1945 static int Clip(Vector2 normal, float clip, Vector2 *faceA, Vector2 *faceB)
jstephens78 0:e39efa4f4f58 1946 {
jstephens78 0:e39efa4f4f58 1947 int sp = 0;
jstephens78 0:e39efa4f4f58 1948 Vector2 out[2] = { *faceA, *faceB };
jstephens78 0:e39efa4f4f58 1949
jstephens78 0:e39efa4f4f58 1950 // Retrieve distances from each endpoint to the line
jstephens78 0:e39efa4f4f58 1951 float distanceA = MathDot(normal, *faceA) - clip;
jstephens78 0:e39efa4f4f58 1952 float distanceB = MathDot(normal, *faceB) - clip;
jstephens78 0:e39efa4f4f58 1953
jstephens78 0:e39efa4f4f58 1954 // If negative (behind plane)
jstephens78 0:e39efa4f4f58 1955 if (distanceA <= 0.0f)
jstephens78 0:e39efa4f4f58 1956 out[sp++] = *faceA;
jstephens78 0:e39efa4f4f58 1957
jstephens78 0:e39efa4f4f58 1958 if (distanceB <= 0.0f)
jstephens78 0:e39efa4f4f58 1959 out[sp++] = *faceB;
jstephens78 0:e39efa4f4f58 1960
jstephens78 0:e39efa4f4f58 1961 // If the points are on different sides of the plane
jstephens78 0:e39efa4f4f58 1962 if ((distanceA*distanceB) < 0.0f)
jstephens78 0:e39efa4f4f58 1963 {
jstephens78 0:e39efa4f4f58 1964 // Push intersection point
jstephens78 0:e39efa4f4f58 1965 float alpha = distanceA/(distanceA - distanceB);
jstephens78 0:e39efa4f4f58 1966 out[sp] = *faceA;
jstephens78 0:e39efa4f4f58 1967 Vector2 delta = Vector2Subtract(*faceB, *faceA);
jstephens78 0:e39efa4f4f58 1968 delta.x *= alpha;
jstephens78 0:e39efa4f4f58 1969 delta.y *= alpha;
jstephens78 0:e39efa4f4f58 1970 out[sp] = Vector2Add(out[sp], delta);
jstephens78 0:e39efa4f4f58 1971 sp++;
jstephens78 0:e39efa4f4f58 1972 }
jstephens78 0:e39efa4f4f58 1973
jstephens78 0:e39efa4f4f58 1974 // Assign the new converted values
jstephens78 0:e39efa4f4f58 1975 *faceA = out[0];
jstephens78 0:e39efa4f4f58 1976 *faceB = out[1];
jstephens78 0:e39efa4f4f58 1977
jstephens78 0:e39efa4f4f58 1978 return sp;
jstephens78 0:e39efa4f4f58 1979 }
jstephens78 0:e39efa4f4f58 1980
jstephens78 0:e39efa4f4f58 1981 // Check if values are between bias range
jstephens78 0:e39efa4f4f58 1982 static bool BiasGreaterThan(float valueA, float valueB)
jstephens78 0:e39efa4f4f58 1983 {
jstephens78 0:e39efa4f4f58 1984 return (valueA >= (valueB*0.95f + valueA*0.01f));
jstephens78 0:e39efa4f4f58 1985 }
jstephens78 0:e39efa4f4f58 1986
jstephens78 0:e39efa4f4f58 1987 // Returns the barycenter of a triangle given by 3 points
jstephens78 0:e39efa4f4f58 1988 static Vector2 TriangleBarycenter(Vector2 v1, Vector2 v2, Vector2 v3)
jstephens78 0:e39efa4f4f58 1989 {
jstephens78 0:e39efa4f4f58 1990 Vector2 result = { 0.0f, 0.0f };
jstephens78 0:e39efa4f4f58 1991
jstephens78 0:e39efa4f4f58 1992 result.x = (v1.x + v2.x + v3.x)/3;
jstephens78 0:e39efa4f4f58 1993 result.y = (v1.y + v2.y + v3.y)/3;
jstephens78 0:e39efa4f4f58 1994
jstephens78 0:e39efa4f4f58 1995 return result;
jstephens78 0:e39efa4f4f58 1996 }
jstephens78 0:e39efa4f4f58 1997
jstephens78 0:e39efa4f4f58 1998 // Initializes hi-resolution MONOTONIC timer
jstephens78 0:e39efa4f4f58 1999 static void InitTimer(void)
jstephens78 0:e39efa4f4f58 2000 {
jstephens78 0:e39efa4f4f58 2001 srand(time(NULL)); // Initialize random seed
jstephens78 0:e39efa4f4f58 2002
jstephens78 0:e39efa4f4f58 2003 #if defined(_WIN32)
jstephens78 0:e39efa4f4f58 2004 QueryPerformanceFrequency((unsigned long long int *) &frequency);
jstephens78 0:e39efa4f4f58 2005 #endif
jstephens78 0:e39efa4f4f58 2006
jstephens78 0:e39efa4f4f58 2007 #if defined(__linux__)
jstephens78 0:e39efa4f4f58 2008 struct timespec now;
jstephens78 0:e39efa4f4f58 2009 if (clock_gettime(CLOCK_MONOTONIC, &now) == 0)
jstephens78 0:e39efa4f4f58 2010 frequency = 1000000000;
jstephens78 0:e39efa4f4f58 2011 #endif
jstephens78 0:e39efa4f4f58 2012
jstephens78 0:e39efa4f4f58 2013 #if defined(__APPLE__)
jstephens78 0:e39efa4f4f58 2014 mach_timebase_info_data_t timebase;
jstephens78 0:e39efa4f4f58 2015 mach_timebase_info(&timebase);
jstephens78 0:e39efa4f4f58 2016 frequency = (timebase.denom*1e9)/timebase.numer;
jstephens78 0:e39efa4f4f58 2017 #endif
jstephens78 0:e39efa4f4f58 2018
jstephens78 0:e39efa4f4f58 2019 baseTime = GetTimeCount(); // Get MONOTONIC clock time offset
jstephens78 0:e39efa4f4f58 2020 startTime = GetCurrentTime(); // Get current time
jstephens78 0:e39efa4f4f58 2021 }
jstephens78 0:e39efa4f4f58 2022
jstephens78 0:e39efa4f4f58 2023 // Get hi-res MONOTONIC time measure in seconds
jstephens78 0:e39efa4f4f58 2024 static uint64_t GetTimeCount(void)
jstephens78 0:e39efa4f4f58 2025 {
jstephens78 0:e39efa4f4f58 2026 uint64_t value = 0;
jstephens78 0:e39efa4f4f58 2027
jstephens78 0:e39efa4f4f58 2028 #if defined(_WIN32)
jstephens78 0:e39efa4f4f58 2029 QueryPerformanceCounter((unsigned long long int *) &value);
jstephens78 0:e39efa4f4f58 2030 #endif
jstephens78 0:e39efa4f4f58 2031
jstephens78 0:e39efa4f4f58 2032 #if defined(__linux__)
jstephens78 0:e39efa4f4f58 2033 struct timespec now;
jstephens78 0:e39efa4f4f58 2034 clock_gettime(CLOCK_MONOTONIC, &now);
jstephens78 0:e39efa4f4f58 2035 value = (uint64_t)now.tv_sec*(uint64_t)1000000000 + (uint64_t)now.tv_nsec;
jstephens78 0:e39efa4f4f58 2036 #endif
jstephens78 0:e39efa4f4f58 2037
jstephens78 0:e39efa4f4f58 2038 #if defined(__APPLE__)
jstephens78 0:e39efa4f4f58 2039 value = mach_absolute_time();
jstephens78 0:e39efa4f4f58 2040 #endif
jstephens78 0:e39efa4f4f58 2041
jstephens78 0:e39efa4f4f58 2042 return value;
jstephens78 0:e39efa4f4f58 2043 }
jstephens78 0:e39efa4f4f58 2044
jstephens78 0:e39efa4f4f58 2045 // Get current time in milliseconds
jstephens78 0:e39efa4f4f58 2046 static double GetCurrentTime(void)
jstephens78 0:e39efa4f4f58 2047 {
jstephens78 0:e39efa4f4f58 2048 return (double)(GetTimeCount() - baseTime)/frequency*1000;
jstephens78 0:e39efa4f4f58 2049 }
jstephens78 0:e39efa4f4f58 2050
jstephens78 0:e39efa4f4f58 2051 // Returns the cross product of a vector and a value
jstephens78 0:e39efa4f4f58 2052 static inline Vector2 MathCross(float value, Vector2 vector)
jstephens78 0:e39efa4f4f58 2053 {
jstephens78 0:e39efa4f4f58 2054 return (Vector2){ -value*vector.y, value*vector.x };
jstephens78 0:e39efa4f4f58 2055 }
jstephens78 0:e39efa4f4f58 2056
jstephens78 0:e39efa4f4f58 2057 // Returns the cross product of two vectors
jstephens78 0:e39efa4f4f58 2058 static inline float MathCrossVector2(Vector2 v1, Vector2 v2)
jstephens78 0:e39efa4f4f58 2059 {
jstephens78 0:e39efa4f4f58 2060 return (v1.x*v2.y - v1.y*v2.x);
jstephens78 0:e39efa4f4f58 2061 }
jstephens78 0:e39efa4f4f58 2062
jstephens78 0:e39efa4f4f58 2063 // Returns the len square root of a vector
jstephens78 0:e39efa4f4f58 2064 static inline float MathLenSqr(Vector2 vector)
jstephens78 0:e39efa4f4f58 2065 {
jstephens78 0:e39efa4f4f58 2066 return (vector.x*vector.x + vector.y*vector.y);
jstephens78 0:e39efa4f4f58 2067 }
jstephens78 0:e39efa4f4f58 2068
jstephens78 0:e39efa4f4f58 2069 // Returns the dot product of two vectors
jstephens78 0:e39efa4f4f58 2070 static inline float MathDot(Vector2 v1, Vector2 v2)
jstephens78 0:e39efa4f4f58 2071 {
jstephens78 0:e39efa4f4f58 2072 return (v1.x*v2.x + v1.y*v2.y);
jstephens78 0:e39efa4f4f58 2073 }
jstephens78 0:e39efa4f4f58 2074
jstephens78 0:e39efa4f4f58 2075 // Returns the square root of distance between two vectors
jstephens78 0:e39efa4f4f58 2076 static inline float DistSqr(Vector2 v1, Vector2 v2)
jstephens78 0:e39efa4f4f58 2077 {
jstephens78 0:e39efa4f4f58 2078 Vector2 dir = Vector2Subtract(v1, v2);
jstephens78 0:e39efa4f4f58 2079 return MathDot(dir, dir);
jstephens78 0:e39efa4f4f58 2080 }
jstephens78 0:e39efa4f4f58 2081
jstephens78 0:e39efa4f4f58 2082 // Returns the normalized values of a vector
jstephens78 0:e39efa4f4f58 2083 static void MathNormalize(Vector2 *vector)
jstephens78 0:e39efa4f4f58 2084 {
jstephens78 0:e39efa4f4f58 2085 float length, ilength;
jstephens78 0:e39efa4f4f58 2086
jstephens78 0:e39efa4f4f58 2087 Vector2 aux = *vector;
jstephens78 0:e39efa4f4f58 2088 length = sqrtf(aux.x*aux.x + aux.y*aux.y);
jstephens78 0:e39efa4f4f58 2089
jstephens78 0:e39efa4f4f58 2090 if (length == 0)
jstephens78 0:e39efa4f4f58 2091 length = 1.0f;
jstephens78 0:e39efa4f4f58 2092
jstephens78 0:e39efa4f4f58 2093 ilength = 1.0f/length;
jstephens78 0:e39efa4f4f58 2094
jstephens78 0:e39efa4f4f58 2095 vector->x *= ilength;
jstephens78 0:e39efa4f4f58 2096 vector->y *= ilength;
jstephens78 0:e39efa4f4f58 2097 }
jstephens78 0:e39efa4f4f58 2098
jstephens78 0:e39efa4f4f58 2099 #if defined(PHYSAC_STANDALONE)
jstephens78 0:e39efa4f4f58 2100 // Returns the sum of two given vectors
jstephens78 0:e39efa4f4f58 2101 static inline Vector2 Vector2Add(Vector2 v1, Vector2 v2)
jstephens78 0:e39efa4f4f58 2102 {
jstephens78 0:e39efa4f4f58 2103 return (Vector2){ v1.x + v2.x, v1.y + v2.y };
jstephens78 0:e39efa4f4f58 2104 }
jstephens78 0:e39efa4f4f58 2105
jstephens78 0:e39efa4f4f58 2106 // Returns the subtract of two given vectors
jstephens78 0:e39efa4f4f58 2107 static inline Vector2 Vector2Subtract(Vector2 v1, Vector2 v2)
jstephens78 0:e39efa4f4f58 2108 {
jstephens78 0:e39efa4f4f58 2109 return (Vector2){ v1.x - v2.x, v1.y - v2.y };
jstephens78 0:e39efa4f4f58 2110 }
jstephens78 0:e39efa4f4f58 2111 #endif
jstephens78 0:e39efa4f4f58 2112
jstephens78 0:e39efa4f4f58 2113 // Creates a matrix 2x2 from a given radians value
jstephens78 0:e39efa4f4f58 2114 static Mat2 Mat2Radians(float radians)
jstephens78 0:e39efa4f4f58 2115 {
jstephens78 0:e39efa4f4f58 2116 float c = cosf(radians);
jstephens78 0:e39efa4f4f58 2117 float s = sinf(radians);
jstephens78 0:e39efa4f4f58 2118
jstephens78 0:e39efa4f4f58 2119 return (Mat2){ c, -s, s, c };
jstephens78 0:e39efa4f4f58 2120 }
jstephens78 0:e39efa4f4f58 2121
jstephens78 0:e39efa4f4f58 2122 // Set values from radians to a created matrix 2x2
jstephens78 0:e39efa4f4f58 2123 static void Mat2Set(Mat2 *matrix, float radians)
jstephens78 0:e39efa4f4f58 2124 {
jstephens78 0:e39efa4f4f58 2125 float cos = cosf(radians);
jstephens78 0:e39efa4f4f58 2126 float sin = sinf(radians);
jstephens78 0:e39efa4f4f58 2127
jstephens78 0:e39efa4f4f58 2128 matrix->m00 = cos;
jstephens78 0:e39efa4f4f58 2129 matrix->m01 = -sin;
jstephens78 0:e39efa4f4f58 2130 matrix->m10 = sin;
jstephens78 0:e39efa4f4f58 2131 matrix->m11 = cos;
jstephens78 0:e39efa4f4f58 2132 }
jstephens78 0:e39efa4f4f58 2133
jstephens78 0:e39efa4f4f58 2134 // Returns the transpose of a given matrix 2x2
jstephens78 0:e39efa4f4f58 2135 static inline Mat2 Mat2Transpose(Mat2 matrix)
jstephens78 0:e39efa4f4f58 2136 {
jstephens78 0:e39efa4f4f58 2137 return (Mat2){ matrix.m00, matrix.m10, matrix.m01, matrix.m11 };
jstephens78 0:e39efa4f4f58 2138 }
jstephens78 0:e39efa4f4f58 2139
jstephens78 0:e39efa4f4f58 2140 // Multiplies a vector by a matrix 2x2
jstephens78 0:e39efa4f4f58 2141 static inline Vector2 Mat2MultiplyVector2(Mat2 matrix, Vector2 vector)
jstephens78 0:e39efa4f4f58 2142 {
jstephens78 0:e39efa4f4f58 2143 return (Vector2){ matrix.m00*vector.x + matrix.m01*vector.y, matrix.m10*vector.x + matrix.m11*vector.y };
jstephens78 0:e39efa4f4f58 2144 }
jstephens78 0:e39efa4f4f58 2145
jstephens78 0:e39efa4f4f58 2146 #endif // PHYSAC_IMPLEMENTATION