diff --git a/src/rigidbody.c b/src/rigidbody.c
index 52aec00..60f758c 100644
--- a/src/rigidbody.c
+++ b/src/rigidbody.c
@@ -104,21 +104,13 @@ static  inline
 Vector _internal_calculate_contact_force(RigidBody* self, Contact* contact) {
     Collision hit = contact->hit;
 
-    if(vsqrmagnitudef(hit.penetration_vector) < powf(0.01f, 2))
-        return ZeroVector;
-
-    const float d = vmagnitudef(hit.penetration_vector);
-    const float k = 0.5f;
-    const float b = 0.5f;
+    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;
 
-    const Vector damping = vmulff(hit.normal, k * d);
-    const Vector bounce = vmulff(vmulff(hit.normal, b), vdotf(hit.normal, velocity));
-
-    // float dot = -vdotf(contact->hit.normal, vaddf(hit.velocity, self->last_linear_force)) * 0.5f;
-    // dot = fmaxf(dot, 0.0f);
-    // Vector counter_force = vmulff(contact->hit.normal, dot * vmagnitudef(hit.penetration_vector) * self->mass);
-    return vmulff(vsubf(damping, bounce), self->mass);
+    return vmulff(normal, -depth * elasticity - damping * vdotf(normal, velocity));
 }
 
 static inline
@@ -137,18 +129,22 @@ void _internal_rigidbody_solve_contact(RigidBody* self, Contact* contact, Vector
         rigidbody_add_impulse(self, force, 1);
     }
 
-    ASSERT_RETURN(!visnanf(force), , "Force contains NaN (1)");
+    ASSERT_RETURN(!visnanf(force),, "Force contains NaN (1)");
 }
 
-void rigidbody_solve_contacts(RigidBody* self) {
-    ASSERT_RETURN(!visnanf(self->linear_velocity),, "Velocity is NaN (0)");
-
-    _internal_rigidbody_collect_contacts(self);
-
+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)");
 }
@@ -177,7 +173,7 @@ void _internal_rigidbody_integrate_forces(RigidBody* self) {
 }
 
 void rigidbody_apply_physics(RigidBody* self) {
-    rigidbody_solve_contacts(self);
+    rigidbody_handle_contacts(self);
     _internal_rigidbody_integrate_forces(self);
 }