From f373fada26a29f63299a174e94c46f9f2d239e34 Mon Sep 17 00:00:00 2001 From: Sara Date: Sun, 22 Oct 2023 11:25:44 +0200 Subject: [PATCH] added custom collision solvers --- src/collision.c | 1 + src/physics_entity.c | 52 ++++++++++++++++++++++++++++++++ src/physics_entity.h | 17 +++++++++-- src/physics_world.c | 5 ++- src/player.c | 22 ++++++++++++++ src/player.h | 4 ++- src/rigidbody.c | 72 +++++--------------------------------------- src/rigidbody.h | 12 ++++++-- src/tilemap.c | 2 ++ src/tilemap.h | 5 ++- 10 files changed, 116 insertions(+), 76 deletions(-) diff --git a/src/collision.c b/src/collision.c index dfe646c..c555a91 100644 --- a/src/collision.c +++ b/src/collision.c @@ -2,6 +2,7 @@ #include "debug.h" #include "player.h" #include "vmath.h" +#include "rigidbody.h" // ===================================================== // Shape overlap test using the separating axis theorem diff --git a/src/physics_entity.c b/src/physics_entity.c index 102b9f7..b28be3f 100644 --- a/src/physics_entity.c +++ b/src/physics_entity.c @@ -3,6 +3,8 @@ #include "rigidbody.h" #include "shape.h" #include "render.h" +#include "debug.h" +#include "program.h" void physics_entity_debug_draw(PhysicsEntity self) { RigidBody* body = self.tc->get_rigidbody(self.data); @@ -23,3 +25,53 @@ void physics_entity_debug_draw(PhysicsEntity self) { SDL_SetRenderDrawColor(g_renderer, 0, 255, 255, 255); SDL_RenderDrawLine(g_renderer, lhs.x, lhs.y, rhs.x, rhs.y); } +static inline +Vector _internal_calculate_contact_force(RigidBody* self, Contact* contact) { + Collision hit = contact->hit; + + const Vector velocity = hit.velocity; + const Vector normal = hit.normal; + + const float elasticity = 4.0; + // const float damping = 1.0;//4.f; + + return vmulff(normal, elasticity * -vdotf(normal, velocity)); +} + +static inline +void _internal_default_contact_solver(RigidBody* body, Contact* contact, Vector* solve_forces) { + Collision hit = contact->hit; + if(vdotf(hit.normal, rigidbody_get_velocity(body)) > 0.0) + return; + + float normalmag = vmagnitudef(contact->hit.normal); + ASSERT_RETURN(fabsf(normalmag - 1.f) < 0.99999f,, "Normal of collision not one"); + + Vector force = _internal_calculate_contact_force(body, contact); + float dot = vdotf(vnormalizedf(*solve_forces), force); + if (veqf(*solve_forces, ZeroVector) || dot * dot > vsqrmagnitudef(*solve_forces) || dot <= 0.0f) { + *solve_forces = vaddf(*solve_forces, force); + rigidbody_add_impulse(body, force, 1); + } + + ASSERT_RETURN(!visnanf(force),, "Force contains NaN (1)"); +} + +void default_contact_solver(PhysicsEntity self, List* contacts) { + Vector solve_forces = ZeroVector; + list_foreach(Contact, contact, contacts) { + _internal_default_contact_solver(self.tc->get_rigidbody(self.data), contact, &solve_forces); + } +} + +void physics_entity_handle_contacts(PhysicsEntity self) { + RigidBody* body = self.tc->get_rigidbody(self.data); + + ASSERT_RETURN(!visnanf(rigidbody_get_velocity(body)),, "Velocity is NaN (0)"); + + rigidbody_collect_contacts(body); + if(rigidbody_get_contacts(body)->len > 0) + self.tc->collision_solver(self.data, rigidbody_get_contacts(body)); + + ASSERT_RETURN(!visnanf(rigidbody_get_velocity(body)),, "Velocity is NaN (1)"); +} diff --git a/src/physics_entity.h b/src/physics_entity.h index e5626c8..f85ab57 100644 --- a/src/physics_entity.h +++ b/src/physics_entity.h @@ -3,16 +3,17 @@ #include "vmath.h" #include "typeclass_helpers.h" - -#include "rigidbody.h" +#include "list.h" #include "shape.h" typedef struct Collision Collision; +typedef struct RigidBody RigidBody; typedef struct { RigidBody* (*const get_rigidbody)(void* self); Shape* (*const get_shape)(void* self); void(*const on_collision)(void* self, Collision collision); + void(*const collision_solver)(void* self, List* collisions); } IPhysicsEntity; typedef struct { @@ -22,17 +23,27 @@ typedef struct { } PhysicsEntity; extern void physics_entity_debug_draw(PhysicsEntity self); +extern void default_contact_solver(PhysicsEntity self, List* contacts); +extern void physics_entity_handle_contacts(PhysicsEntity self); -#define impl_PhysicsEntity_for(T, get_rigidbody_f, get_shape_f, on_collision_f)\ + +#define impl_PhysicsEntity_default_solver(__Name, T)\ +void __Name(T* self, List* contacts) {\ + default_contact_solver(T##_as_PhysicsEntity(self), contacts);\ +} + +#define impl_PhysicsEntity_for(T, get_rigidbody_f, get_shape_f, on_collision_f, collision_solver_f)\ static inline PhysicsEntity T##_as_PhysicsEntity(T* x) {\ TC_FN_TYPECHECK(Transformable, T##_as_Transformable, T*);\ TC_FN_TYPECHECK(RigidBody*, get_rigidbody_f, T*);\ TC_FN_TYPECHECK(Shape*, get_shape_f, T*);\ TC_FN_TYPECHECK(void, on_collision_f, T*, Collision);\ + TC_FN_TYPECHECK(void, collision_solver_f, T*, List*);\ static IPhysicsEntity const tc = {\ .get_rigidbody = (RigidBody*(*const)(void*)) get_rigidbody_f,\ .get_shape = (Shape*(*const)(void*)) get_shape_f,\ .on_collision = (void(*const)(void*,Collision)) on_collision_f,\ + .collision_solver = (void(*const)(void*,List*)) collision_solver_f,\ };\ Transformable transformable = T##_as_Transformable(x);\ return (PhysicsEntity){.data = x, .tc = &tc, .transformable = transformable.tc};\ diff --git a/src/physics_world.c b/src/physics_world.c index 53a9676..1b7ced8 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -167,11 +167,10 @@ void _internal_physics_tilemap_collision() { static void _internal_physics_apply() { 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); + physics_entity_handle_contacts(*entity); + rigidbody_integrate_forces(entity->tc->get_rigidbody(entity->data)); } } diff --git a/src/player.c b/src/player.c index b4a43a5..2383f29 100644 --- a/src/player.c +++ b/src/player.c @@ -1,6 +1,7 @@ #include "player.h" #include "assets.h" #include "debug.h" +#include "physics_entity.h" #include "program.h" #include "rigidbody.h" #include "input.h" @@ -23,6 +24,27 @@ void player_spawn(Player* self, Vector at) { rigidbody_get_transform(self->rigidbody)->position = at; } +void player_collision_solver(Player* self, List* contacts) { + RigidBody* body = self->rigidbody; + Collision hit; + const Vector vel = vaddf(rigidbody_get_velocity(body), vmulff(rigidbody_get_force(body), delta_time())); + Vector avrg = ZeroVector; + int n = 0; + list_foreach(Contact, contact, contacts) { + hit = contact->hit; + Vector norm = vnormalizedf(hit.penetration_vector); + LOG_INFO("n %f %f", norm.x, norm.y); + float dot = fmaxf(0, vdotf(norm, hit.velocity)); + avrg = vaddf(vmulff(avrg, (float)n), vsubf(vel, vmulff(norm, dot))); + ++n; + avrg = vmulff(avrg, 1.0f / (float)n); + } + LOG_INFO("vel %f %f", vel.x, vel.y); + LOG_INFO("for %f %f", avrg.x, avrg.y); + // rigidbody_add_impulse(body, vinvf(avrg), 0); + default_contact_solver(Player_as_PhysicsEntity(self), contacts); +} + void player_start(Player* self) { input_add_axis_action(SDL_SCANCODE_A, SDL_SCANCODE_D, &player_input_h); input_add_axis_action(SDL_SCANCODE_S, SDL_SCANCODE_W, &player_input_v); diff --git a/src/player.h b/src/player.h index 1e17633..01eb867 100644 --- a/src/player.h +++ b/src/player.h @@ -28,6 +28,7 @@ extern Transform* player_get_transform(Player* self); extern Transform* player_get_rigidbody_transform(Player* self); extern RigidBody* player_get_rigidbody(Player* self); extern Shape* player_get_shape(Player* self); +extern void player_collision_solver(Player* self, List* contacts); impl_Transformable_for(Player, player_get_transform @@ -40,7 +41,8 @@ impl_SpriteEntity_for(Player, impl_PhysicsEntity_for(Player, player_get_rigidbody, player_get_shape, - player_collision + player_collision, + player_collision_solver ) impl_BehaviourEntity_for(Player, diff --git a/src/rigidbody.c b/src/rigidbody.c index 60f758c..dd91f40 100644 --- a/src/rigidbody.c +++ b/src/rigidbody.c @@ -5,12 +5,6 @@ #include "collision.h" #include "transformable.h" -typedef struct { - Collision hit; - float warming; - int expiry; -} Contact; - struct RigidBody { Transformable transformable; @@ -71,8 +65,7 @@ void rigidbody_add_contact(RigidBody* self, Collision hit) { }); } -static inline -void _internal_rigidbody_collect_contacts(RigidBody* self) { +void rigidbody_collect_contacts(RigidBody* self) { for(size_t i = 0; i < self->contacts.len; ++i) { Contact* contact = list_at(&self->contacts, i); @@ -85,6 +78,10 @@ void _internal_rigidbody_collect_contacts(RigidBody* self) { } } +List* rigidbody_get_contacts(RigidBody* self) { + return &self->contacts; +} + static inline void _internal_debug_draw_collision_edge(Vector left, Vector right, Vector normal) { #if !NDEBUG @@ -100,57 +97,7 @@ void _internal_debug_draw_collision_edge(Vector left, Vector right, Vector norma #endif } -static inline -Vector _internal_calculate_contact_force(RigidBody* self, Contact* contact) { - Collision hit = contact->hit; - - const float depth = -vdotf(hit.normal, hit.penetration_vector); - const float elasticity = 200.0f; - const float damping = 4.f; - const Vector velocity = hit.velocity; - const Vector normal = hit.normal; - - return vmulff(normal, -depth * elasticity - damping * vdotf(normal, velocity)); -} - -static inline -void _internal_rigidbody_solve_contact(RigidBody* self, Contact* contact, Vector* solve_forces) { - Collision hit = contact->hit; - if(vdotf(hit.normal, self->linear_velocity) > 0.0) - return; - - float normalmag = vmagnitudef(contact->hit.normal); - ASSERT_RETURN(fabsf(normalmag - 1.f) < 0.99999f,, "Normal of collision not one"); - - Vector force = _internal_calculate_contact_force(self, contact); - float dot = vdotf(vnormalizedf(*solve_forces), force); - if (veqf(*solve_forces, ZeroVector) || dot * dot > vsqrmagnitudef(*solve_forces) || dot <= 0.0f) { - *solve_forces = vaddf(*solve_forces, force); - rigidbody_add_impulse(self, force, 1); - } - - ASSERT_RETURN(!visnanf(force),, "Force contains NaN (1)"); -} - -void rigidbody_solve_contacts(RigidBody* self, List* contacts) { - Vector solve_forces = ZeroVector; - list_foreach(Contact, contact, &self->contacts) { - _internal_rigidbody_solve_contact(self, contact, &solve_forces); - } -} - -void rigidbody_handle_contacts(RigidBody* self) { - ASSERT_RETURN(!visnanf(self->linear_velocity),, "Velocity is NaN (0)"); - - _internal_rigidbody_collect_contacts(self); - rigidbody_solve_contacts(self, &self->contacts); - - - ASSERT_RETURN(!visnanf(self->linear_velocity),, "Velocity is NaN (1)"); -} - -static inline -void _internal_rigidbody_integrate_forces(RigidBody* self) { +void rigidbody_integrate_forces(RigidBody* self) { const float dt = delta_time(); Vector position = self->internal_transform.position; @@ -172,11 +119,6 @@ void _internal_rigidbody_integrate_forces(RigidBody* self) { self->next_linear_force = ZeroVector; } -void rigidbody_apply_physics(RigidBody* self) { - rigidbody_handle_contacts(self); - _internal_rigidbody_integrate_forces(self); -} - float rigidbody_get_mass(const RigidBody* self) { return self->mass; } @@ -214,7 +156,7 @@ void rigidbody_set_velocity(RigidBody* self, Vector velocity) { } Vector rigidbody_get_force(RigidBody* self) { - return self->last_linear_force; + return self->next_linear_force; } void rigidbody_debug_draw_contacts(RigidBody* self) { diff --git a/src/rigidbody.h b/src/rigidbody.h index bb0b6e5..c41fc09 100644 --- a/src/rigidbody.h +++ b/src/rigidbody.h @@ -4,8 +4,14 @@ #include "shape.h" #include "transformable.h" #include "list.h" +#include "collision.h" struct Collision; +typedef struct { + struct Collision hit; + float warming; + int expiry; +} Contact; typedef struct RigidBody RigidBody; typedef void (*CollisionHandlerFn)(void* obj, List* collisions); @@ -14,10 +20,10 @@ extern RigidBody* rigidbody_make(Transformable transform); extern void rigidbody_destroy(RigidBody* self); extern void rigidbody_add_contact(RigidBody* self, struct Collision hit); -extern void rigidbody_handle_contacts(RigidBody* self); -extern void rigidbody_solve_contacts(RigidBody* self, List* contacts); +extern void rigidbody_collect_contacts(RigidBody* self); +extern List* rigidbody_get_contacts(RigidBody* self); -extern void rigidbody_apply_physics(RigidBody* self); +extern void rigidbody_integrate_forces(RigidBody* self); extern float rigidbody_get_mass(const RigidBody* self); extern void rigidbody_set_mass(RigidBody* self, float mass); diff --git a/src/tilemap.c b/src/tilemap.c index 314b698..d9af19a 100644 --- a/src/tilemap.c +++ b/src/tilemap.c @@ -146,3 +146,5 @@ Vector* tile_instance_get_scale(TileInstance* self) { float* tile_instance_get_rotation(TileInstance* self) { return &self->transform.rotation; } + +impl_PhysicsEntity_default_solver(tile_instance_solve_contacts, TileInstance) diff --git a/src/tilemap.h b/src/tilemap.h index 78bb952..c217718 100644 --- a/src/tilemap.h +++ b/src/tilemap.h @@ -29,6 +29,8 @@ extern Transform* tile_instance_get_transform(TileInstance* self); extern Shape* tile_instance_get_shape(TileInstance* self); extern void tile_instance_on_collision(TileInstance* self, Collision collision); +extern void tile_instance_solve_contacts(TileInstance* self, List* contacts); + impl_Transformable_for(Tilemap, tilemap_get_transform @@ -40,7 +42,8 @@ impl_Transformable_for(TileInstance, impl_PhysicsEntity_for(TileInstance, tile_instance_get_rigidbody, tile_instance_get_shape, - tile_instance_on_collision + tile_instance_on_collision, + tile_instance_solve_contacts ) #endif // !_fencer_tilemap_h