94 lines
2.6 KiB
C
94 lines
2.6 KiB
C
#include "physics_world.h"
|
|
#include "debug.h"
|
|
#include "collision.h"
|
|
|
|
static PhysicsEntity* _world_bodies = NULL;
|
|
static size_t _world_bodies_cap = 0;
|
|
static size_t _world_bodies_len = 0;
|
|
|
|
void physics_world_init() {
|
|
_world_bodies = malloc(8 * sizeof(PhysicsEntity));
|
|
_world_bodies_cap = 0;
|
|
}
|
|
|
|
void physics_world_clean() {
|
|
free(_world_bodies);
|
|
_world_bodies_cap = 0;
|
|
_world_bodies_cap = 0;
|
|
}
|
|
|
|
void _physics_world_resize(size_t minimum) {
|
|
if(minimum < _world_bodies_cap) {
|
|
return;
|
|
}
|
|
|
|
size_t amount = _world_bodies_cap;
|
|
while(amount < minimum) {
|
|
amount *= 2;
|
|
}
|
|
|
|
PhysicsEntity* new = realloc(_world_bodies, amount * sizeof(PhysicsEntity));
|
|
ASSERT_RETURN(new == NULL, , "Failed to grow world bodies array");
|
|
|
|
_world_bodies = new;
|
|
_world_bodies_cap = amount;
|
|
}
|
|
|
|
void physics_world_add_entity(PhysicsEntity entity) {
|
|
_physics_world_resize(_world_bodies_len + 1);
|
|
|
|
_world_bodies[_world_bodies_len] = entity;
|
|
++_world_bodies_len;
|
|
}
|
|
|
|
static
|
|
void _internal_physics_world_remove_entity(PhysicsEntity* at) {
|
|
PhysicsEntity* from = at + 1;
|
|
memmove(at, from, (from - _world_bodies + _world_bodies_len) * sizeof(PhysicsEntity));
|
|
--_world_bodies_len;
|
|
}
|
|
|
|
void physics_world_remove_entity(PhysicsEntity entity) {
|
|
PhysicsEntity* end = _world_bodies + _world_bodies_len;
|
|
for(PhysicsEntity* itr = _world_bodies; itr < end; ++itr) {
|
|
if(itr->data == entity.data) {
|
|
_internal_physics_world_remove_entity(itr);
|
|
}
|
|
}
|
|
|
|
ASSERT_RETURN(0,, "Physics entity with data at %p is not registered in physics world", entity.data);
|
|
}
|
|
|
|
static
|
|
void _internal_physics_narrow_collision() {
|
|
PhysicsEntity* end = _world_bodies + _world_bodies_len;
|
|
PhysicsEntity* half_end = _world_bodies + _world_bodies_len/2;
|
|
Collision collision_left, collision_right;
|
|
|
|
for(PhysicsEntity* left = _world_bodies; left < end; ++left) {
|
|
for(PhysicsEntity* right = _world_bodies; right < half_end; ++right) {
|
|
collision_check(*left, *right, &collision_left, &collision_right);
|
|
|
|
left->tc->on_collision(left->data, collision_left);
|
|
right->tc->on_collision(right->data, collision_right);
|
|
}
|
|
}
|
|
}
|
|
|
|
static
|
|
void _internal_physics_move() {
|
|
PhysicsEntity* end = _world_bodies + _world_bodies_len;
|
|
RigidBody* body = NULL;
|
|
|
|
for(PhysicsEntity* entity = _world_bodies; entity < end; ++entity) {
|
|
body = entity->tc->get_rigidbody(entity->data);
|
|
rigidbody_apply_physics(body);
|
|
}
|
|
}
|
|
|
|
void physics_world_tick() {
|
|
// _internal_physics_broad_collision();
|
|
_internal_physics_narrow_collision();
|
|
_internal_physics_move();
|
|
}
|