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