diff --git a/src/physics_entity.c b/src/physics_entity.c index b48c43d..a35a866 100644 --- a/src/physics_entity.c +++ b/src/physics_entity.c @@ -26,16 +26,11 @@ void physics_entity_debug_draw(PhysicsEntity self) { } 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 = 0.0; - const float damping = 0.0; - const float push = vdotf(normal, velocity); - - return ZeroVector; //vsubf(vmulff(normal, elasticity * fmaxf(1.0, -push)), vmulff(normal, damping * fminf(0.0, push))); + const Vector velocity = contact->hit.velocity; + const Vector normal = contact->hit.normal; + const float push = -vdotf(normal, velocity); + const Vector current_velocity = rigidbody_get_velocity(self); + return vaddf(current_velocity, vmulff(normal, push * rigidbody_get_bounce(self))); } static inline @@ -52,17 +47,10 @@ int _internal_default_contact_solver(RigidBody* body, Contact* contact, Transfor return 0; } -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); -} - void physics_entity_solve_contacts(PhysicsEntity self, List* contacts) { - physics_entity_apply_collision_forces(self, contacts); 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) { @@ -79,8 +67,10 @@ void physics_entity_solve_contacts(PhysicsEntity self, List* contacts) { Vector dir = vnormalizedf(vsubf(rigidbody_get_transform(body)->position, pre_solve.position)); Vector vel = rigidbody_get_velocity(body); float dot = vdotf(dir, vel); + if(dot < 0) - vel = vsubf(vel, vmulff(dir, dot)); + vel = vsubf(vel, vmulff(dir, dot * (1.0 + rigidbody_get_bounce(body)))); + rigidbody_set_velocity(body, vel); } diff --git a/src/rigidbody.c b/src/rigidbody.c index 037ea3f..09b1c87 100644 --- a/src/rigidbody.c +++ b/src/rigidbody.c @@ -10,6 +10,8 @@ struct RigidBody { float mass; + float bounce; + Vector last_linear_force; Vector next_linear_force; Vector linear_velocity; @@ -26,6 +28,7 @@ RigidBody* rigidbody_make(Transformable transform) { *self = (RigidBody){ .transformable = transform, .mass = 1.0f, + .bounce = 0.0f, .linear_velocity = ZeroVector, .next_linear_force = ZeroVector, @@ -107,6 +110,14 @@ void rigidbody_set_mass(RigidBody* self, float mass) { self->mass = mass; } +float rigidbody_get_bounce(const RigidBody* self) { + return self->bounce; +} + +void rigidbody_set_bounce(RigidBody* self, float bounce) { + self->bounce = bounce; +} + void rigidbody_add_impulse(RigidBody* self, Vector force, int use_mass) { rigidbody_accelerate(self, vmulff(force, 1.0f/delta_time()), use_mass); } diff --git a/src/rigidbody.h b/src/rigidbody.h index c1d4637..74e5ba5 100644 --- a/src/rigidbody.h +++ b/src/rigidbody.h @@ -27,6 +27,9 @@ extern void rigidbody_integrate_forces(RigidBody* self); extern float rigidbody_get_mass(const RigidBody* self); extern void rigidbody_set_mass(RigidBody* self, float mass); +extern float rigidbody_get_bounce(const RigidBody* self); +extern void rigidbody_set_bounce(RigidBody* self, float bounce); + extern void rigidbody_add_impulse(RigidBody* self, Vector force, int use_mass); extern void rigidbody_accelerate(RigidBody* self, Vector force, int use_mass);