improvements to collision solvers

src/player.c

@@ -20,7 +20,7 @@ void player_input_v(int val) {
 
 void player_spawn(Player* self, Vector at) {
     player_start(self);
-    self->transform.position = at;
+    rigidbody_get_transform(self->rigidbody)->position = at;
 }
 
 void player_start(Player* self) {
@@ -37,6 +37,7 @@ void player_start(Player* self) {
     sprite_set_origin(self->sprite, (Vector){0.25f, 1.f});
 
     self->rigidbody = rigidbody_make(Player_as_Transformable(self));
+    rigidbody_set_mass(self->rigidbody, 10.f);
 
     float ex_w = 0.1f;
     float h = .75f;
@@ -56,8 +57,10 @@ 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};
-    rigidbody_accelerate(self->rigidbody, vmulff(vsubf(velocity_target, velocity), 20.f));
-    rigidbody_accelerate(self->rigidbody, (Vector){0.0f, 20.f});
+    rigidbody_accelerate(self->rigidbody, vmulff(vsubf(velocity_target, velocity), 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);
 }
 
 void player_collision(Player* self, Collision hit) {
@@ -71,6 +74,10 @@ Transform* player_get_transform(Player* self) {
     return &self->transform;
 }
 
+Transform* player_get_rigidbody_transform(Player* self) {
+    return rigidbody_get_transform(self->rigidbody);
+}
+
 RigidBody* player_get_rigidbody(Player* self) {
     return self->rigidbody;
 }

src/player.h

@@ -25,6 +25,7 @@ extern void player_collision(Player* self, Collision hit);
 extern Sprite* player_get_sprite(Player* sprite);
 
 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);
 

src/rigidbody.c

@@ -51,7 +51,6 @@ void rigidbody_destroy(RigidBody* self) {
 }
 
 void rigidbody_add_contact(RigidBody* self, Collision hit) {
-    LOG_INFO("contact between rigidbody %p and %p detected", self->transformable.data, hit.other.data);
     // update an existing contact
     list_foreach(Contact, contact, &self->contacts) {
         if(contact->hit.other.data == hit.other.data) {
@@ -70,9 +69,6 @@ void rigidbody_add_contact(RigidBody* self, Collision hit) {
         .hit = hit,
         .warming = delta_time()
     });
-
-    Contact* c = list_at(&self->contacts, self->contacts.len - 1);
-    LOG_INFO("added contact %p with exp %d", c->hit.other.data, c->expiry);
 }
 
 static inline
@@ -81,13 +77,11 @@ void _internal_rigidbody_collect_contacts(RigidBody* self) {
         Contact* contact = list_at(&self->contacts, i);
 
         if(contact->expiry <= 0) {
-            if(vdotf(vnormalizedf(self->linear_velocity), contact->hit.normal) >= -0.01f || contact->expiry <= -1) {
-                list_erase(&self->contacts, i);
-                i--;
-                continue;
-            }
+            list_erase(&self->contacts, i);
+            i--;
+        } else {
+            --(contact->expiry);
         }
-        --(contact->expiry);
     }
 }
 
@@ -113,21 +107,18 @@ Vector _internal_calculate_contact_force(RigidBody* self, Contact* contact) {
     if(vsqrmagnitudef(hit.penetration_vector) < powf(0.01f, 2))
         return ZeroVector;
 
-
-    const float warming = fminf(1.f, contact->warming * 1000.f);
-    const float d = 1.0f;
-    const float k = 0.3f;
-    const float b = 0.3f;
+    const float d = vmagnitudef(hit.penetration_vector);
+    const float k = 0.5f;
+    const float b = 0.5f;
+    const Vector velocity = hit.velocity;
 
     const Vector damping = vmulff(hit.normal, k * d);
-    const Vector bounce = vmulff(hit.normal, b * vdotf(hit.normal, hit.velocity));
+    const Vector bounce = vmulff(vmulff(hit.normal, b), vdotf(hit.normal, velocity));
 
-    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);
+    // 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);
 }
 
 static inline
@@ -142,14 +133,8 @@ void _internal_rigidbody_solve_contact(RigidBody* self, Contact* contact, Vector
     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->next_linear_force));
-        #endif
         *solve_forces = vaddf(*solve_forces, force);
-        rigidbody_add_impulse(self, force);
+        rigidbody_add_impulse(self, force, 1);
     }
 
     ASSERT_RETURN(!visnanf(force), , "Force contains NaN (1)");
@@ -174,13 +159,12 @@ void _internal_rigidbody_integrate_forces(RigidBody* self) {
 
     Vector position = self->internal_transform.position;
     Vector velocity = self->linear_velocity;
-    Vector acceleration = vmulff(self->last_linear_force, 0.5f * dt*dt);
+    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);
+    acceleration = vmulff(vaddf(self->next_linear_force, acceleration), dt * 0.5f);
     velocity = vaddf(velocity, acceleration);
 
     self->linear_velocity = velocity;
@@ -205,11 +189,13 @@ void rigidbody_set_mass(RigidBody* self, float mass) {
     self->mass = mass;
 }
 
-void rigidbody_add_impulse(RigidBody* self, Vector force) {
-    rigidbody_accelerate(self, vmulff(force, 1.0f/delta_time()));
+void rigidbody_add_impulse(RigidBody* self, Vector force, int use_mass) {
+    rigidbody_accelerate(self, vmulff(force, 1.0f/delta_time()), use_mass);
 }
 
-void rigidbody_accelerate(RigidBody* self, Vector force) {
+void rigidbody_accelerate(RigidBody* self, Vector force, int use_mass) {
+    if(use_mass)
+        force = vmulff(force, 1.0f / self->mass);
     if(vsqrmagnitudef(force) > powf(0.01f, 2))
         self->next_linear_force = vaddf(self->next_linear_force, force);
 }
@@ -231,6 +217,10 @@ void rigidbody_set_velocity(RigidBody* self, Vector velocity) {
     self->linear_velocity = velocity;
 }
 
+Vector rigidbody_get_force(RigidBody* self) {
+    return self->last_linear_force;
+}
+
 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);

src/rigidbody.h

@@ -21,8 +21,8 @@ extern void rigidbody_apply_physics(RigidBody* self);
 extern float rigidbody_get_mass(const RigidBody* self);
 extern void rigidbody_set_mass(RigidBody* self, float mass);
 
-extern void rigidbody_add_impulse(RigidBody* self, Vector force);
-extern void rigidbody_accelerate(RigidBody* self, Vector force);
+extern void rigidbody_add_impulse(RigidBody* self, Vector force, int use_mass);
+extern void rigidbody_accelerate(RigidBody* self, Vector force, int use_mass);
 
 extern int rigidbody_is_static(const RigidBody* self);
 extern void rigidbody_set_static(RigidBody* self, int is_static);
@@ -30,6 +30,8 @@ extern void rigidbody_set_static(RigidBody* self, int is_static);
 extern Vector rigidbody_get_velocity(const RigidBody* self);
 extern void rigidbody_set_velocity(RigidBody* self, Vector velocity);
 
+extern Vector rigidbody_get_force(RigidBody* self);
+
 extern void rigidbody_debug_draw_contacts(RigidBody* self);
 extern Transform* rigidbody_get_transform(RigidBody* self);