Justin Stephens / Physac-MBED

MBED port of the Physacs library

physac.h@0:e39efa4f4f58, 21 months ago (annotated)

Committer:
jstephens78
Date:
Tue Nov 29 22:42:42 2022 +0000
Revision:
0:e39efa4f4f58
Child:
1:ebc0214989c0
Initial commit

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