From 37b1fc3228de5a9fb144236b836245873bcd6f13 Mon Sep 17 00:00:00 2001 From: Sara Date: Wed, 18 Oct 2023 22:51:56 +0200 Subject: [PATCH] implemented velocity verlet integration --- src/collision.c | 8 ++--- src/collision.h | 2 +- src/physics_world.c | 3 +- src/player.c | 3 +- src/rigidbody.c | 83 +++++++++++++++++++++++++++++++++------------ src/rigidbody.h | 5 +++ 6 files changed, 75 insertions(+), 29 deletions(-) diff --git a/src/collision.c b/src/collision.c index 35d9396..15bd2d9 100644 --- a/src/collision.c +++ b/src/collision.c @@ -79,7 +79,7 @@ int _internal_collision_get_overlap(PhysicsEntity self, PhysicsEntity other, Col } if(sqr_mag == 0) { - out->separation_force = InfinityVector; + out->penetration_vector = InfinityVector; return 0; } } @@ -96,7 +96,7 @@ int _internal_collision_get_overlap(PhysicsEntity self, PhysicsEntity other, Col .normal = vnormalizedf(shortest_escape), .velocity = velocity, - .separation_force = shortest_escape, + .penetration_vector = shortest_escape, .edge_left = shape_get_point_transformed(self_shape, shortest_escape_edge, *self_transform), .edge_right = shape_get_point_transformed(self_shape, (1 + shortest_escape_edge) % self_point_count, *self_transform), @@ -112,7 +112,7 @@ Collision _internal_collision_invert(Collision collision_a, PhysicsEntity a) { .point = collision_a.point, .normal = vinvf(collision_a.normal), .velocity = vinvf(collision_a.velocity), - .separation_force = vinvf(collision_a.separation_force), + .penetration_vector = vinvf(collision_a.penetration_vector), .edge_left = collision_a.edge_left, .edge_right = collision_a.edge_right, }; @@ -126,7 +126,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.separation_force) < vsqrmagnitudef(collision_b.separation_force)) + 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/collision.h b/src/collision.h index 9d639c0..bc295eb 100644 --- a/src/collision.h +++ b/src/collision.h @@ -12,7 +12,7 @@ typedef struct Collision { Vector normal; Vector velocity; - Vector separation_force; + Vector penetration_vector; Vector edge_left; Vector edge_right; diff --git a/src/physics_world.c b/src/physics_world.c index 13ff1ef..53a9676 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -125,7 +125,7 @@ void _internal_physics_narrow_collision() { for(PhysicsEntity* left = _world_bodies; left < end; ++left) { for(PhysicsEntity* right = _world_bodies; right < half_end; ++right) { - if(left == right) continue; + if(left->data == right->data) continue; if(collision_check(*left, *right, &collision_left, &collision_right)) { left->tc->on_collision(left->data, collision_left); @@ -171,7 +171,6 @@ void _internal_physics_apply() { for(PhysicsEntity* entity = _world_bodies; entity < end; ++entity) { body = entity->tc->get_rigidbody(entity->data); - rigidbody_solve_contacts(body); rigidbody_apply_physics(body); } } diff --git a/src/player.c b/src/player.c index 1a61e8d..c585999 100644 --- a/src/player.c +++ b/src/player.c @@ -55,8 +55,9 @@ void player_start(Player* self) { void player_update(Player* self, float dt) { Vector velocity = rigidbody_get_velocity(self->rigidbody); - Vector velocity_target = {directional.x, directional.y}; + Vector velocity_target = {directional.x, velocity.y}; rigidbody_accelerate(self->rigidbody, vmulff(vsubf(velocity_target, velocity), 20.f)); + rigidbody_accelerate(self->rigidbody, (Vector){0.0f, 20.f}); } void player_collision(Player* self, Collision hit) { diff --git a/src/rigidbody.c b/src/rigidbody.c index f272e19..15b4222 100644 --- a/src/rigidbody.c +++ b/src/rigidbody.c @@ -3,6 +3,7 @@ #include "debug.h" #include "program.h" #include "collision.h" +#include "transformable.h" typedef struct { Collision hit; @@ -15,8 +16,10 @@ struct RigidBody { float mass; - Vector linear_force; + Vector last_linear_force; + Vector next_linear_force; Vector linear_velocity; + Transform internal_transform; int is_static; @@ -29,7 +32,13 @@ RigidBody* rigidbody_make(Transformable transform) { *self = (RigidBody){ .transformable = transform, .mass = 1.0f, + .linear_velocity = ZeroVector, + .next_linear_force = ZeroVector, + .last_linear_force = ZeroVector, + + .internal_transform = *transform.tc->get_transform(transform.data), + .is_static = 0, .contacts = list_from_type(Contact), }; @@ -47,6 +56,7 @@ void rigidbody_add_contact(RigidBody* self, Collision hit) { list_foreach(Contact, contact, &self->contacts) { if(contact->hit.other.data == hit.other.data) { ++contact->expiry; + hit.velocity = contact->hit.velocity; contact->hit = hit; contact->warming += delta_time(); return; @@ -74,6 +84,7 @@ void _internal_rigidbody_collect_contacts(RigidBody* self) { if(vdotf(vnormalizedf(self->linear_velocity), contact->hit.normal) >= -0.01f || contact->expiry <= -1) { list_erase(&self->contacts, i); i--; + continue; } } --(contact->expiry); @@ -96,21 +107,27 @@ void _internal_debug_draw_collision_edge(Vector left, Vector right, Vector norma } static inline -Vector _internal_calculate_contact_force(Contact* contact) { +Vector _internal_calculate_contact_force(RigidBody* self, Contact* contact) { Collision hit = contact->hit; - if(vsqrmagnitudef(hit.separation_force) < powf(0.01f, 2)) + if(vsqrmagnitudef(hit.penetration_vector) < powf(0.01f, 2)) return ZeroVector; - - const float warming = fminf(1.f, contact->warming * 100.f); + + + const float warming = fminf(1.f, contact->warming * 1000.f); const float d = 1.0f; - const float k = 0.001f; - const float b = 1.0f * warming; + const float k = 0.3f; + const float b = 0.3f; const Vector damping = vmulff(hit.normal, k * d); - const Vector bounce = vprojectf(vmulff(hit.normal, -b), hit.velocity); + const Vector bounce = vmulff(hit.normal, b * vdotf(hit.normal, hit.velocity)); - return vsubf(damping, bounce); + float dot = -vdotf(contact->hit.normal, vaddf(self->next_linear_force, self->linear_velocity)); + if(dot < 0) + dot = 0; + Vector counter_force = vmulff(contact->hit.normal, dot * vsqrmagnitudef(hit.penetration_vector)); + + return vaddf(vmulff(vsubf(damping, bounce), warming), counter_force); } static inline @@ -118,20 +135,18 @@ void _internal_rigidbody_solve_contact(RigidBody* self, Contact* contact, Vector Collision hit = contact->hit; if(vdotf(hit.normal, self->linear_velocity) > 0.0) return; - // if (vsqrmagnitudef(contact->hit.separation_force) < 0.001f*0.001f) - // 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(contact); + 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) { #if !NDEBUG LOG_INFO("warming: %f", contact->warming); LOG_INFO("force: %f %f", force.x, force.y); LOG_INFO("dot: %f", dot); - LOG_INFO("mag: %f", vmagnitudef(self->linear_force)); + LOG_INFO("mag: %f", vmagnitudef(self->next_linear_force)); #endif *solve_forces = vaddf(*solve_forces, force); rigidbody_add_impulse(self, force); @@ -153,11 +168,33 @@ void rigidbody_solve_contacts(RigidBody* self) { ASSERT_RETURN(!visnanf(self->linear_velocity),, "Velocity is NaN (1)"); } +static inline +void _internal_rigidbody_integrate_forces(RigidBody* self) { + const float dt = delta_time(); + + Vector position = self->internal_transform.position; + Vector velocity = self->linear_velocity; + Vector acceleration = vmulff(self->last_linear_force, 0.5f * dt*dt); + + position = vaddf(position, vmulff(velocity, dt)); + position = vaddf(position, acceleration); + + velocity = vaddf(velocity, acceleration); + acceleration = vmulff(self->next_linear_force, dt/2); + velocity = vaddf(velocity, acceleration); + + self->linear_velocity = velocity; + self->internal_transform.position = position; + if(vsqrdistf(position, transformable_get_position(self->transformable)) > 0.001f) + transformable_set_position(self->transformable, position); + + self->last_linear_force = self->next_linear_force; + self->next_linear_force = ZeroVector; +} + void rigidbody_apply_physics(RigidBody* self) { - Vector position = transformable_get_position(self->transformable); - Vector velocity = vmulff(self->linear_velocity, delta_time()); - transformable_set_position(self->transformable, vaddf(position, velocity)); - self->linear_force = ZeroVector; + rigidbody_solve_contacts(self); + _internal_rigidbody_integrate_forces(self); } float rigidbody_get_mass(const RigidBody* self) { @@ -169,12 +206,12 @@ void rigidbody_set_mass(RigidBody* self, float mass) { } void rigidbody_add_impulse(RigidBody* self, Vector force) { - self->linear_force = vaddf(self->linear_force, force); - self->linear_velocity = vaddf(self->linear_velocity, force); + rigidbody_accelerate(self, vmulff(force, 1.0f/delta_time())); } void rigidbody_accelerate(RigidBody* self, Vector force) { - rigidbody_add_impulse(self, vmulff(force, delta_time())); + if(vsqrmagnitudef(force) > powf(0.01f, 2)) + self->next_linear_force = vaddf(self->next_linear_force, force); } int rigidbody_is_static(const RigidBody* self) { @@ -190,7 +227,7 @@ Vector rigidbody_get_velocity(const RigidBody* self) { } void rigidbody_set_velocity(RigidBody* self, Vector velocity) { - self->linear_force = vaddf(self->linear_force, vsubf(velocity, self->linear_force)); + self->next_linear_force = vaddf(self->next_linear_force, vsubf(velocity, self->next_linear_force)); self->linear_velocity = velocity; } @@ -199,3 +236,7 @@ void rigidbody_debug_draw_contacts(RigidBody* self) { _internal_debug_draw_collision_edge(contact->hit.edge_left, contact->hit.edge_right, contact->hit.normal); } } + +Transform* rigidbody_get_transform(RigidBody* self) { + return &self->internal_transform; +} diff --git a/src/rigidbody.h b/src/rigidbody.h index 4ddb725..0d3f636 100644 --- a/src/rigidbody.h +++ b/src/rigidbody.h @@ -31,5 +31,10 @@ extern Vector rigidbody_get_velocity(const RigidBody* self); extern void rigidbody_set_velocity(RigidBody* self, Vector velocity); extern void rigidbody_debug_draw_contacts(RigidBody* self); +extern Transform* rigidbody_get_transform(RigidBody* self); + +impl_Transformable_for(RigidBody, + rigidbody_get_transform +) #endif // !_fencer_rigidbody_h