more debugging for collision sovler

src/collision.c

@@ -121,17 +121,15 @@ int collision_check(PhysicsEntity a, PhysicsEntity b, Collision* out_a, Collisio
     int collision_a_overlaps = _internal_collision_get_overlap(a, b, &collision_a);
     int collision_b_overlaps = _internal_collision_get_overlap(b, a, &collision_b);
 
-    if(!collision_a_overlaps && !collision_b_overlaps) {
+    if(!collision_a_overlaps && !collision_b_overlaps)
         return 0;
-    }
-    
-    if(vsqrmagnitudef(collision_a.separation_force) < vsqrmagnitudef(collision_b.separation_force)) {
-        collision_b = _internal_collision_invert(collision_a, a);
-    } else {
+    else if(!collision_a_overlaps && collision_b_overlaps)
         collision_a = _internal_collision_invert(collision_b, b);
-    }
+    else if(!collision_b_overlaps && collision_a_overlaps)
+        collision_b = _internal_collision_invert(collision_a, a);
+
 
     *out_a = collision_a;
     *out_b = collision_b;
-    return 1;
+    return (collision_b_overlaps << 1) | collision_a_overlaps;
 }

src/fencer.c

@@ -35,7 +35,7 @@ static
 void draw() {
     level_draw(level);
     sprite_entity_draw(Player_as_SpriteEntity(player));
-    shape_draw(player->shape, player->transform);
+    physics_entity_debug_draw(Player_as_PhysicsEntity(player));
 }
 
 int main(int argc, char* argv[]) {

src/physics_entity.c

@@ -0,0 +1,21 @@
+#include "physics_entity.h"
+#include "camera.h"
+#include "rigidbody.h"
+#include "shape.h"
+#include "render.h"
+
+void physics_entity_debug_draw(PhysicsEntity self) {
+    RigidBody* body = self.tc->get_rigidbody(self.data);
+    Shape* shape = self.tc->get_shape(self.data);
+    Transform* transform = self.transformable->get_transform(self.data);
+
+    shape_draw(shape, *transform);
+    rigidbody_debug_draw_contacts(body);
+
+    Vector lhs = transform->position;
+    Vector rhs = vaddf(lhs, rigidbody_get_velocity(body));
+    lhs = camera_world_to_pixel_point(&g_camera, lhs);
+    rhs = camera_world_to_pixel_point(&g_camera, rhs);
+    SDL_SetRenderDrawColor(g_renderer, 0, 255, 0, 255);
+    SDL_RenderDrawLine(g_renderer, lhs.x, lhs.y, rhs.x, rhs.y);
+}

src/physics_entity.h

@@ -21,6 +21,8 @@ typedef struct {
     ITransformable const* transformable;
 } PhysicsEntity;
 
+extern void physics_entity_debug_draw(PhysicsEntity self);
+
 #define impl_PhysicsEntity_for(T, get_rigidbody_f, get_shape_f, on_collision_f)\
 static inline PhysicsEntity T##_as_PhysicsEntity(T* x) {\
     TC_FN_TYPECHECK(Transformable, T##_as_Transformable, T*);\

src/player.c

@@ -41,11 +41,13 @@ void player_start(Player* self) {
     float ex_w = 0.1f;
     float h = .75f;
     float r = 0.05f;
+    float rr = (r/3)*2;
+
     self->shape = shape_new((Vector[]){
-        {r-ex_w, 0.f}, {-ex_w, -r},
-        {-ex_w, r-h}, {r-ex_w, -h},
-        {ex_w-r, -h}, {ex_w, r-h},
-        {ex_w, -r}, {ex_w-r, 0.f},
+        {r-ex_w, 0.f}, {-ex_w, -rr},
+        {-ex_w, rr-h}, {r-ex_w, -h},
+        {ex_w-r, -h}, {ex_w, rr-h},
+        {ex_w, -rr}, {ex_w-r, 0.f},
     }, 8);
 
     physics_world_add_entity(Player_as_PhysicsEntity(self));
@@ -54,7 +56,7 @@ 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};
-    rigidbody_accelerate(self->rigidbody, vmulff(vsubf(velocity_target, velocity), 200.f));
+    rigidbody_accelerate(self->rigidbody, vmulff(vsubf(velocity_target, velocity), 20.f));
 }
 
 void player_collision(Player* self, Collision hit) {

src/rigidbody.c

@@ -69,11 +69,11 @@ static inline
 void _internal_rigidbody_collect_contacts(RigidBody* self) {
     for(size_t i = 0; i < self->contacts.len; ++i) {
         Contact* contact = list_at(&self->contacts, i);
-        --(contact->expiry);
         if(contact->expiry <= 0) {
             list_erase(&self->contacts, i);
             i--;
         }
+        --(contact->expiry);
     }
 }
 
@@ -96,9 +96,9 @@ static  inline
 Vector _internal_calculate_contact_force(Contact* contact) {
     Collision hit = contact->hit;
     const float warming = fminf(1.f, contact->warming);
-    const float d = 1.f;
-    const float k = 50.0 * warming;
-    const float b = 1.f;
+    const float d = 1.0f;
+    const float k = 1.0f * warming;
+    const float b = 1.0f;
 
     const Vector damping = vmulff(hit.normal, k * d);
     const Vector bounce = vprojectf(vmulff(hit.normal, -b), hit.velocity);
@@ -109,8 +109,14 @@ Vector _internal_calculate_contact_force(Contact* contact) {
 static inline
 void _internal_rigidbody_solve_contact(RigidBody* self, Contact* contact, Vector* solve_forces) {
     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);
     float dot = vdotf(vnormalizedf(*solve_forces), force);
     if (veqf(*solve_forces, ZeroVector) || dot * dot > vsqrmagnitudef(*solve_forces) || dot <= 0.0f) {
@@ -124,21 +130,19 @@ void _internal_rigidbody_solve_contact(RigidBody* self, Contact* contact, Vector
         rigidbody_add_impulse(self, force);
     }
 
-    _internal_debug_draw_collision_edge(hit.edge_left, hit.edge_right, hit.normal);
-
     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);
+
     Vector solve_forces = ZeroVector;
     list_foreach(Contact, contact, &self->contacts) {
         _internal_rigidbody_solve_contact(self, contact, &solve_forces);
     }
 
-    _internal_rigidbody_collect_contacts(self);
-
     ASSERT_RETURN(!visnanf(self->linear_velocity),, "Velocity is NaN (1)");
 }
 
@@ -146,7 +150,7 @@ void rigidbody_apply_physics(RigidBody* self) {
     Vector position = transformable_get_position(self->transformable);
     Vector velocity = vmulff(self->linear_velocity, delta_time());
 
-    if(vsqrmagnitudef(velocity) > powf(0.00001f, 2)) {
+    if(vsqrmagnitudef(velocity) > powf(0.00000f, 2)) {
         transformable_set_position(self->transformable, vaddf(position, velocity));
     }
 
@@ -186,3 +190,9 @@ void rigidbody_set_velocity(RigidBody* self, Vector velocity) {
     self->linear_force = vaddf(self->linear_force, vsubf(velocity, self->linear_force));
     self->linear_velocity = velocity;
 }
+
+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

@@ -10,24 +10,26 @@ struct Collision;
 typedef struct RigidBody RigidBody;
 
 // Referenced transform is stored but not owned by the rigidbody.
-RigidBody* rigidbody_make(Transformable transform);
-void rigidbody_destroy(RigidBody* self);
+extern RigidBody* rigidbody_make(Transformable transform);
+extern void rigidbody_destroy(RigidBody* self);
 
-void rigidbody_add_contact(RigidBody* self, struct Collision hit);
-void rigidbody_solve_contacts(RigidBody* self);
+extern void rigidbody_add_contact(RigidBody* self, struct Collision hit);
+extern void rigidbody_solve_contacts(RigidBody* self);
 
-void rigidbody_apply_physics(RigidBody* self);
+extern void rigidbody_apply_physics(RigidBody* self);
 
-float rigidbody_get_mass(const RigidBody* self);
-void rigidbody_set_mass(RigidBody* self, float mass);
+extern float rigidbody_get_mass(const RigidBody* self);
+extern void rigidbody_set_mass(RigidBody* self, float mass);
 
-void rigidbody_add_impulse(RigidBody* self, Vector force);
-void rigidbody_accelerate(RigidBody* self, Vector force);
+extern void rigidbody_add_impulse(RigidBody* self, Vector force);
+extern void rigidbody_accelerate(RigidBody* self, Vector force);
 
-int rigidbody_is_static(const RigidBody* self);
-void rigidbody_set_static(RigidBody* self, int is_static);
+extern int rigidbody_is_static(const RigidBody* self);
+extern void rigidbody_set_static(RigidBody* self, int is_static);
 
-Vector rigidbody_get_velocity(const RigidBody* self);
-void rigidbody_set_velocity(RigidBody* self, Vector velocity);
+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);
 
 #endif // !_fencer_rigidbody_h