From e86fa1bdef8bd251af1d752504c8a2595cab860e Mon Sep 17 00:00:00 2001 From: Sara Date: Tue, 24 Oct 2023 23:14:30 +0200 Subject: [PATCH] implemented collision constraint solver --- src/collision.c | 26 +++++++++++------ src/fencer.c | 2 +- src/physics_entity.c | 66 +++++++++++++++++++++++++++++--------------- src/physics_entity.h | 2 +- src/physics_world.c | 8 ++++-- src/player.c | 23 ++------------- src/rigidbody.c | 15 ++++++---- src/rigidbody.h | 2 +- src/transform.h | 5 ++++ src/vmath.h | 8 ++++++ 10 files changed, 95 insertions(+), 62 deletions(-) diff --git a/src/collision.c b/src/collision.c index c555a91..8a2cedb 100644 --- a/src/collision.c +++ b/src/collision.c @@ -35,13 +35,18 @@ Range _internal_collision_get_range_on_axis(PhysicsEntity self, Vector axis) { } static -Vector _internal_collision_overlap_on_axis(PhysicsEntity self, PhysicsEntity other, Vector axis) { +Vector _internal_collision_overlap_on_axis(PhysicsEntity self, PhysicsEntity other, Vector axis, Vector* out_point) { Range a_range = _internal_collision_get_range_on_axis(self, axis); Range b_range = _internal_collision_get_range_on_axis(other, axis); if(a_range.min <= b_range.max && a_range.min <= b_range.max) { - return vmulff(axis, fminf(a_range.max - b_range.min, b_range.min - a_range.max)); + const float overlap_left = a_range.max - b_range.min; + const float overlap_right = b_range.min - a_range.max; + const float shortest = fminf(overlap_left, overlap_right); + *out_point = overlap_right < overlap_left ? a_range.maxpoint : a_range.minpoint; + return vmulff(axis, shortest); } else { + *out_point = InfinityVector; return ZeroVector; } } @@ -70,8 +75,9 @@ int _internal_collision_get_overlap(PhysicsEntity self, PhysicsEntity other, Col // the direction of the line Vector normal = vperpendicularf(vsubf(b, a)); + Vector overlap_point; // the smallest escape vector on this axis - Vector escape = _internal_collision_overlap_on_axis(self, other, vnormalizedf(normal)); + Vector escape = _internal_collision_overlap_on_axis(self, other, vnormalizedf(normal), &overlap_point); float sqr_mag = vsqrmagnitudef(escape); if(sqr_mag < shortest_sqrmag) { shortest_sqrmag = sqr_mag; @@ -87,13 +93,15 @@ int _internal_collision_get_overlap(PhysicsEntity self, PhysicsEntity other, Col RigidBody* rba = self.tc->get_rigidbody(self.data); RigidBody* rbb = other.tc->get_rigidbody(other.data); - Vector velocity = vsubf(rigidbody_get_velocity(rba), rigidbody_get_velocity(rbb)); + const Vector velocity = vsubf(rigidbody_get_velocity(rba), rigidbody_get_velocity(rbb)); + const Vector normal = vnormalizedf(shortest_escape); + Vector world_point = _internal_collision_get_range_on_axis(self, normal).minpoint; *out = (Collision) { .other = other, - .point = InfinityVector, - .normal = vnormalizedf(shortest_escape), + .point = inverse_transform_point(rigidbody_get_transform(rba), world_point), + .normal = normal, .velocity = velocity, .penetration_vector = shortest_escape, @@ -107,9 +115,11 @@ int _internal_collision_get_overlap(PhysicsEntity self, PhysicsEntity other, Col static Collision _internal_collision_invert(Collision collision_a, PhysicsEntity a) { + Vector world_point = _internal_collision_get_range_on_axis(collision_a.other, collision_a.normal).maxpoint; + RigidBody* body = collision_a.other.tc->get_rigidbody(collision_a.other.data); return (Collision){ .other = a, - .point = collision_a.point, + .point = inverse_transform_point(rigidbody_get_transform(body), world_point), .normal = vinvf(collision_a.normal), .velocity = vinvf(collision_a.velocity), .penetration_vector = vinvf(collision_a.penetration_vector), @@ -126,7 +136,7 @@ int collision_check(PhysicsEntity a, PhysicsEntity b, Collision* out_a, Collisio if(!collision_a_overlaps || !collision_b_overlaps) return 0; - if(vsqrmagnitudef(collision_a.penetration_vector) < vsqrmagnitudef(collision_b.penetration_vector)) + if(vsqrmagnitudef(collision_a.penetration_vector) > vsqrmagnitudef(collision_b.penetration_vector)) collision_b = _internal_collision_invert(collision_a, a); else collision_a = _internal_collision_invert(collision_b, b); diff --git a/src/fencer.c b/src/fencer.c index 96b3d85..47abd0b 100644 --- a/src/fencer.c +++ b/src/fencer.c @@ -35,7 +35,7 @@ static void draw() { level_draw(level); sprite_entity_draw(Player_as_SpriteEntity(player)); - // physics_entity_debug_draw(Player_as_PhysicsEntity(player)); + physics_entity_debug_draw(Player_as_PhysicsEntity(player)); } int main(int argc, char* argv[]) { diff --git a/src/physics_entity.c b/src/physics_entity.c index b28be3f..e379298 100644 --- a/src/physics_entity.c +++ b/src/physics_entity.c @@ -32,46 +32,68 @@ Vector _internal_calculate_contact_force(RigidBody* self, Contact* contact) { const Vector velocity = hit.velocity; const Vector normal = hit.normal; - const float elasticity = 4.0; - // const float damping = 1.0;//4.f; + const float elasticity = 0.0; + const float damping = 0.0; + const float push = vdotf(normal, velocity); - return vmulff(normal, elasticity * -vdotf(normal, velocity)); + return vsubf(vmulff(normal, elasticity * fmaxf(1.0, -push)), vmulff(normal, damping * fminf(0.0, push))); } static inline -void _internal_default_contact_solver(RigidBody* body, Contact* contact, Vector* solve_forces) { +int _internal_default_contact_solver(RigidBody* body, Contact* contact, Transform pre_solve) { + // the desired position is anywhere the overlapping vertex is further along the normal than the contact point Collision hit = contact->hit; - if(vdotf(hit.normal, rigidbody_get_velocity(body)) > 0.0) - return; + Transform* trans = rigidbody_get_transform(body); + const Vector world_collision_point = vaddf(transform_point(&pre_solve, hit.point), hit.penetration_vector); + const float min_dot = vdotf(hit.normal, world_collision_point); + const float current_dot = vdotf(hit.normal, transform_point(trans, hit.point)); + if(current_dot >= min_dot) + return 1; + const Vector target = vaddf(trans->position, vmulff(hit.normal, min_dot - current_dot)); + trans->position = vmovetowardsf(trans->position, target, 0.01f); + return 0; +} - 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); +void physics_entity_apply_collision_forces(PhysicsEntity self, List* contacts) { + RigidBody* body = self.tc->get_rigidbody(self.data); + // apply collision impulse + list_foreach(Contact, contact, contacts) { + rigidbody_add_impulse(body, _internal_calculate_contact_force(body, contact), 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); + RigidBody* body = self.tc->get_rigidbody(self.data); + const Transform pre_solve = *rigidbody_get_transform(body); + // attempt to solve constraints + int done; + for(size_t iteration = 100; iteration != 0; --iteration) { + done = 1; + list_foreach(Contact, contact, contacts) { + if(!_internal_default_contact_solver(body, contact, pre_solve)) + done = 0; + } + if(done) + break; + if(iteration == 1) + LOG_WARNING("gave up on solving %zu contacts", contacts->len); } } -void physics_entity_handle_contacts(PhysicsEntity self) { +void physics_entity_update(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)); + + + List* contacts = rigidbody_get_contacts(body); + if(contacts->len > 0) { + physics_entity_apply_collision_forces(self, contacts); + self.tc->collision_solver(self.data, contacts); + } ASSERT_RETURN(!visnanf(rigidbody_get_velocity(body)),, "Velocity is NaN (1)"); } diff --git a/src/physics_entity.h b/src/physics_entity.h index f85ab57..63dbc80 100644 --- a/src/physics_entity.h +++ b/src/physics_entity.h @@ -24,7 +24,7 @@ typedef struct { 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); +extern void physics_entity_update(PhysicsEntity self); #define impl_PhysicsEntity_default_solver(__Name, T)\ diff --git a/src/physics_world.c b/src/physics_world.c index 1b7ced8..2ad33bc 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -1,6 +1,7 @@ #include "physics_world.h" #include "debug.h" #include "collision.h" +#include "rigidbody.h" #include "tilemap.h" static PhysicsEntity* _world_bodies = NULL; @@ -169,13 +170,14 @@ void _internal_physics_apply() { PhysicsEntity* end = _world_bodies + _world_bodies_len; for(PhysicsEntity* entity = _world_bodies; entity < end; ++entity) { - physics_entity_handle_contacts(*entity); - rigidbody_integrate_forces(entity->tc->get_rigidbody(entity->data)); + physics_entity_update(*entity); } } void physics_world_tick() { - // _internal_physics_broad_collision(); + PhysicsEntity* end = _world_bodies + _world_bodies_len; + for(PhysicsEntity* entity = _world_bodies; entity < end; ++entity) + rigidbody_integrate_forces(entity->tc->get_rigidbody(entity->data)); _internal_physics_narrow_collision(); _internal_physics_tilemap_collision(); _internal_physics_apply(); diff --git a/src/player.c b/src/player.c index 2383f29..80b5ad2 100644 --- a/src/player.c +++ b/src/player.c @@ -25,23 +25,6 @@ void player_spawn(Player* self, Vector 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); } @@ -78,11 +61,11 @@ void player_start(Player* self) { void player_update(Player* self, float dt) { Vector velocity = rigidbody_get_velocity(self->rigidbody); - Vector velocity_target = {directional.x, velocity.y}; + Vector velocity_target = {directional.x, directional.y}; rigidbody_accelerate(self->rigidbody, vmulff(vsubf(velocity_target, velocity), 20.f), 0); - rigidbody_accelerate(self->rigidbody, (Vector){0.0f, 20.f}, 0); + // rigidbody_accelerate(self->rigidbody, (Vector){0.0f, 20.f}, 0); - LOG_INFO("%f %f", self->transform.position.x, self->transform.position.y); + // LOG_INFO("%f %f", self->transform.position.x, self->transform.position.y); } void player_collision(Player* self, Collision hit) { diff --git a/src/rigidbody.c b/src/rigidbody.c index dd91f40..a38d9d7 100644 --- a/src/rigidbody.c +++ b/src/rigidbody.c @@ -51,7 +51,7 @@ void rigidbody_add_contact(RigidBody* self, Collision hit) { ++contact->expiry; hit.velocity = contact->hit.velocity; contact->hit = hit; - contact->warming += delta_time(); + contact->duration += delta_time(); return; } } @@ -61,7 +61,7 @@ void rigidbody_add_contact(RigidBody* self, Collision hit) { &(Contact) { .expiry = 1, .hit = hit, - .warming = delta_time() + .duration = delta_time() }); } @@ -83,14 +83,17 @@ List* rigidbody_get_contacts(RigidBody* self) { } static inline -void _internal_debug_draw_collision_edge(Vector left, Vector right, Vector normal) { +void _internal_debug_draw_collision_edge(RigidBody* self, Contact* contact) { #if !NDEBUG + Vector left = contact->hit.edge_left; + Vector right = contact->hit.edge_right; + Vector point = transform_point(&self->internal_transform, contact->hit.point); Vector a = camera_world_to_pixel_point(&g_camera, left); Vector b = camera_world_to_pixel_point(&g_camera, right); - Vector n = transform_direction(&g_camera.transform, normal); + Vector n = transform_direction(&g_camera.transform, contact->hit.normal); SDL_SetRenderDrawColor(g_renderer, 255, 255, 255, 255); SDL_RenderDrawLine(g_renderer, a.x, a.y, b.x, b.y); - a = vlerpf(a, b, 0.5f); + a = camera_world_to_pixel_point(&g_camera, point); b = vaddf(a, vmulff(n, 100.f)); SDL_SetRenderDrawColor(g_renderer, 255, 0, 0, 255); SDL_RenderDrawLine(g_renderer, a.x, a.y, b.x, b.y); @@ -161,7 +164,7 @@ Vector rigidbody_get_force(RigidBody* self) { void rigidbody_debug_draw_contacts(RigidBody* self) { list_foreach(Contact, contact, &self->contacts) { - _internal_debug_draw_collision_edge(contact->hit.edge_left, contact->hit.edge_right, contact->hit.normal); + _internal_debug_draw_collision_edge(self, contact); } } diff --git a/src/rigidbody.h b/src/rigidbody.h index c41fc09..4a77219 100644 --- a/src/rigidbody.h +++ b/src/rigidbody.h @@ -9,7 +9,7 @@ struct Collision; typedef struct { struct Collision hit; - float warming; + float duration; int expiry; } Contact; typedef struct RigidBody RigidBody; diff --git a/src/transform.h b/src/transform.h index fa88cf5..7ed243e 100644 --- a/src/transform.h +++ b/src/transform.h @@ -46,6 +46,11 @@ Vector transform_point(Transform* self, Vector position) { return vaddf(vmulf(vrotatef(position, self->rotation), self->scale), self->position); } +static inline +Vector inverse_transform_point(Transform* self, Vector position) { + return vrotatef(vmulf(vsubf(position, self->position), vreciprocalf(self->scale)), -self->rotation); +} + static Transform* transform_get_transform(Transform* self) { return self; diff --git a/src/vmath.h b/src/vmath.h index 07e5e02..3504882 100644 --- a/src/vmath.h +++ b/src/vmath.h @@ -139,6 +139,14 @@ static inline Vector vmovetowardsf(Vector start, Vector end, float delta) { return vlerpf(start, end, delta / vdistancef(end, start)); } +static inline +Vector vaveragef(Vector* array, size_t count) { + Vector acc = ZeroVector; + for(size_t i = 0; i < count; ++i) { + acc = vaddf(acc, array[i]); + } + return vmulff(acc, 1.0/(float)count); +} static inline IVector vaddi(IVector a, IVector b) {