diff --git a/src/physics_world.c b/src/physics_world.c new file mode 100644 index 0000000..0000963 --- /dev/null +++ b/src/physics_world.c @@ -0,0 +1,93 @@ +#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(); +} diff --git a/src/physics_world.h b/src/physics_world.h new file mode 100644 index 0000000..088b251 --- /dev/null +++ b/src/physics_world.h @@ -0,0 +1,14 @@ +#ifndef _fencer_physics_world_h +#define _fencer_physics_world_h + +#include "physics_entity.h" + +extern void physics_world_init(); +extern void physics_world_clean(); + +extern void physics_world_add_entity(PhysicsEntity entity); +extern void physics_world_remove_entity(PhysicsEntity entity); + +extern void physics_world_tick(); + +#endif // !_fencer_physics_world_h