implemented collision constraint solver

src/collision.c

@@ -35,13 +35,18 @@ Range _internal_collision_get_range_on_axis(PhysicsEntity self, Vector axis) {
 }
 
 static
-Vector _internal_collision_overlap_on_axis(PhysicsEntity self, PhysicsEntity other, Vector axis) {
+Vector _internal_collision_overlap_on_axis(PhysicsEntity self, PhysicsEntity other, Vector axis, Vector* out_point) {
     Range a_range = _internal_collision_get_range_on_axis(self, axis);
     Range b_range = _internal_collision_get_range_on_axis(other, axis);
 
     if(a_range.min <= b_range.max && a_range.min <= b_range.max) {
-        return vmulff(axis, fminf(a_range.max - b_range.min, b_range.min - a_range.max));
+        const float overlap_left = a_range.max - b_range.min;
+        const float overlap_right = b_range.min - a_range.max;
+        const float shortest = fminf(overlap_left, overlap_right);
+        *out_point = overlap_right < overlap_left ? a_range.maxpoint : a_range.minpoint;
+        return vmulff(axis, shortest);
     } else {
+        *out_point = InfinityVector;
         return ZeroVector;
     }
 }
@@ -70,8 +75,9 @@ int _internal_collision_get_overlap(PhysicsEntity self, PhysicsEntity other, Col
         // the direction of the line
         Vector normal = vperpendicularf(vsubf(b, a));
 
+        Vector overlap_point;
         // the smallest escape vector on this axis
-        Vector escape = _internal_collision_overlap_on_axis(self, other, vnormalizedf(normal));
+        Vector escape = _internal_collision_overlap_on_axis(self, other, vnormalizedf(normal), &overlap_point);
         float sqr_mag = vsqrmagnitudef(escape);
         if(sqr_mag < shortest_sqrmag) {
             shortest_sqrmag = sqr_mag;
@@ -87,13 +93,15 @@ int _internal_collision_get_overlap(PhysicsEntity self, PhysicsEntity other, Col
 
     RigidBody* rba = self.tc->get_rigidbody(self.data);
     RigidBody* rbb = other.tc->get_rigidbody(other.data);
-    Vector velocity = vsubf(rigidbody_get_velocity(rba), rigidbody_get_velocity(rbb));
+    const Vector velocity = vsubf(rigidbody_get_velocity(rba), rigidbody_get_velocity(rbb));
+    const Vector normal = vnormalizedf(shortest_escape);
+    Vector world_point = _internal_collision_get_range_on_axis(self, normal).minpoint;
 
     *out = (Collision) {
         .other = other,
 
-        .point = InfinityVector,
-        .normal = vnormalizedf(shortest_escape),
+        .point = inverse_transform_point(rigidbody_get_transform(rba), world_point),
+        .normal = normal,
 
         .velocity = velocity,
         .penetration_vector = shortest_escape,
@@ -107,9 +115,11 @@ int _internal_collision_get_overlap(PhysicsEntity self, PhysicsEntity other, Col
 
 static
 Collision _internal_collision_invert(Collision collision_a, PhysicsEntity a) {
+    Vector world_point = _internal_collision_get_range_on_axis(collision_a.other, collision_a.normal).maxpoint;
+    RigidBody* body = collision_a.other.tc->get_rigidbody(collision_a.other.data);
     return (Collision){
         .other = a,
-        .point = collision_a.point,
+        .point = inverse_transform_point(rigidbody_get_transform(body), world_point),
         .normal = vinvf(collision_a.normal),
         .velocity = vinvf(collision_a.velocity),
         .penetration_vector = vinvf(collision_a.penetration_vector),
@@ -126,7 +136,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.penetration_vector) < vsqrmagnitudef(collision_b.penetration_vector))
+    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);

src/fencer.c

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

src/physics_entity.c

@@ -32,46 +32,68 @@ Vector _internal_calculate_contact_force(RigidBody* self, Contact* contact) {
     const Vector velocity = hit.velocity;
     const Vector normal = hit.normal;
 
-    const float elasticity = 4.0;
-    // const float damping = 1.0;//4.f;
+    const float elasticity = 0.0;
+    const float damping = 0.0;
+    const float push = vdotf(normal, velocity);
 
-    return vmulff(normal, elasticity * -vdotf(normal, velocity));
+    return vsubf(vmulff(normal, elasticity * fmaxf(1.0, -push)), vmulff(normal, damping * fminf(0.0, push)));
 }
 
 static inline
-void _internal_default_contact_solver(RigidBody* body, Contact* contact, Vector* solve_forces) {
+int _internal_default_contact_solver(RigidBody* body, Contact* contact, Transform pre_solve) {
+    // the desired position is anywhere the overlapping vertex is further along the normal than the contact point
     Collision hit = contact->hit;
-    if(vdotf(hit.normal, rigidbody_get_velocity(body)) > 0.0)
-        return;
+    Transform* trans = rigidbody_get_transform(body);
+    const Vector world_collision_point = vaddf(transform_point(&pre_solve, hit.point), hit.penetration_vector);
+    const float min_dot = vdotf(hit.normal, world_collision_point);
+    const float current_dot = vdotf(hit.normal, transform_point(trans, hit.point));
+    if(current_dot >= min_dot)
+        return 1;
+    const Vector target = vaddf(trans->position, vmulff(hit.normal, min_dot - current_dot));
+    trans->position = vmovetowardsf(trans->position, target, 0.01f);
+    return 0;
+}
 
-    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(body, contact);
-    float dot = vdotf(vnormalizedf(*solve_forces), force);
-    if (veqf(*solve_forces, ZeroVector) || dot * dot > vsqrmagnitudef(*solve_forces) || dot <= 0.0f) {
-        *solve_forces = vaddf(*solve_forces, force);
-        rigidbody_add_impulse(body, force, 1);
+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);
     }
     
-    ASSERT_RETURN(!visnanf(force),, "Force contains NaN (1)");
 }
 
 void default_contact_solver(PhysicsEntity self, List* contacts) {
-    Vector solve_forces = ZeroVector;
+    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) {
+        done = 1;
         list_foreach(Contact, contact, contacts) {
-        _internal_default_contact_solver(self.tc->get_rigidbody(self.data), contact, &solve_forces);
+            if(!_internal_default_contact_solver(body, contact, pre_solve))
+                done = 0;
+        }
+        if(done)
+            break;
+        if(iteration == 1)
+            LOG_WARNING("gave up on solving %zu contacts", contacts->len);
     }
 }
 
-void physics_entity_handle_contacts(PhysicsEntity self) {
+void physics_entity_update(PhysicsEntity self) {
     RigidBody* body = self.tc->get_rigidbody(self.data);
 
     ASSERT_RETURN(!visnanf(rigidbody_get_velocity(body)),, "Velocity is NaN (0)");
 
     rigidbody_collect_contacts(body);
-    if(rigidbody_get_contacts(body)->len > 0)
-        self.tc->collision_solver(self.data, rigidbody_get_contacts(body));
+
+
+    List* contacts = rigidbody_get_contacts(body);
+    if(contacts->len > 0) {
+        physics_entity_apply_collision_forces(self, contacts);
+        self.tc->collision_solver(self.data, contacts);
+    }
 
     ASSERT_RETURN(!visnanf(rigidbody_get_velocity(body)),, "Velocity is NaN (1)");
 }

src/physics_entity.h

@@ -24,7 +24,7 @@ typedef struct {
 
 extern void physics_entity_debug_draw(PhysicsEntity self);
 extern void default_contact_solver(PhysicsEntity self, List* contacts);
-extern void physics_entity_handle_contacts(PhysicsEntity self);
+extern void physics_entity_update(PhysicsEntity self);
 
 
 #define impl_PhysicsEntity_default_solver(__Name, T)\

src/physics_world.c

@@ -1,6 +1,7 @@
 #include "physics_world.h"
 #include "debug.h"
 #include "collision.h"
+#include "rigidbody.h"
 #include "tilemap.h"
 
 static PhysicsEntity* _world_bodies = NULL;
@@ -169,13 +170,14 @@ void _internal_physics_apply() {
     PhysicsEntity* end = _world_bodies + _world_bodies_len;
 
     for(PhysicsEntity* entity = _world_bodies; entity < end; ++entity) {
-        physics_entity_handle_contacts(*entity);
-        rigidbody_integrate_forces(entity->tc->get_rigidbody(entity->data));
+        physics_entity_update(*entity);
     }
 }
 
 void physics_world_tick() {
-    // _internal_physics_broad_collision();
+    PhysicsEntity* end = _world_bodies + _world_bodies_len;
+    for(PhysicsEntity* entity = _world_bodies; entity < end; ++entity)
+        rigidbody_integrate_forces(entity->tc->get_rigidbody(entity->data));
     _internal_physics_narrow_collision();
     _internal_physics_tilemap_collision();
     _internal_physics_apply();

src/player.c

@@ -25,23 +25,6 @@ void player_spawn(Player* self, Vector at) {
 }
 
 void player_collision_solver(Player* self, List* contacts) {
-    RigidBody* body = self->rigidbody;
-    Collision hit;
-    const Vector vel = vaddf(rigidbody_get_velocity(body), vmulff(rigidbody_get_force(body), delta_time()));
-    Vector avrg = ZeroVector;
-    int n = 0;
-    list_foreach(Contact, contact, contacts) {
-        hit = contact->hit;
-        Vector norm = vnormalizedf(hit.penetration_vector);
-        LOG_INFO("n %f %f", norm.x, norm.y);
-        float dot = fmaxf(0, vdotf(norm, hit.velocity));
-        avrg = vaddf(vmulff(avrg, (float)n), vsubf(vel, vmulff(norm, dot)));
-        ++n;
-        avrg = vmulff(avrg, 1.0f / (float)n);
-    }
-    LOG_INFO("vel %f %f", vel.x, vel.y);
-    LOG_INFO("for %f %f", avrg.x, avrg.y);
-    // rigidbody_add_impulse(body, vinvf(avrg), 0);
     default_contact_solver(Player_as_PhysicsEntity(self), contacts);
 }
 
@@ -78,11 +61,11 @@ 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};
+    Vector velocity_target = {directional.x, directional.y};
     rigidbody_accelerate(self->rigidbody, vmulff(vsubf(velocity_target, velocity), 20.f), 0);
-    rigidbody_accelerate(self->rigidbody, (Vector){0.0f, 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);
+    // LOG_INFO("%f %f", self->transform.position.x, self->transform.position.y);
 }
 
 void player_collision(Player* self, Collision hit) {

src/rigidbody.c

@@ -51,7 +51,7 @@ void rigidbody_add_contact(RigidBody* self, Collision hit) {
             ++contact->expiry;
             hit.velocity = contact->hit.velocity;
             contact->hit = hit;
-            contact->warming += delta_time();
+            contact->duration += delta_time();
             return;
         }
     }
@@ -61,7 +61,7 @@ void rigidbody_add_contact(RigidBody* self, Collision hit) {
     &(Contact) {
         .expiry = 1,
         .hit = hit,
-        .warming = delta_time()
+        .duration = delta_time()
     });
 }
 
@@ -83,14 +83,17 @@ List* rigidbody_get_contacts(RigidBody* self) {
 }
 
 static inline
-void _internal_debug_draw_collision_edge(Vector left, Vector right, Vector normal) {
+void _internal_debug_draw_collision_edge(RigidBody* self, Contact* contact) {
 #if !NDEBUG
+    Vector left = contact->hit.edge_left;
+    Vector right = contact->hit.edge_right;
+    Vector point = transform_point(&self->internal_transform, contact->hit.point);
     Vector a = camera_world_to_pixel_point(&g_camera, left);
     Vector b = camera_world_to_pixel_point(&g_camera, right);
-    Vector n = transform_direction(&g_camera.transform, normal);
+    Vector n = transform_direction(&g_camera.transform, contact->hit.normal);
     SDL_SetRenderDrawColor(g_renderer, 255, 255, 255, 255);
     SDL_RenderDrawLine(g_renderer, a.x, a.y, b.x, b.y);
-    a = vlerpf(a, b, 0.5f);
+    a = camera_world_to_pixel_point(&g_camera, point);
     b = vaddf(a, vmulff(n, 100.f));
     SDL_SetRenderDrawColor(g_renderer, 255, 0, 0, 255);
     SDL_RenderDrawLine(g_renderer, a.x, a.y, b.x, b.y);
@@ -161,7 +164,7 @@ Vector rigidbody_get_force(RigidBody* self) {
 
 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);
+        _internal_debug_draw_collision_edge(self, contact);
     }
 }
 

src/rigidbody.h

@@ -9,7 +9,7 @@
 struct Collision;
 typedef struct {
     struct Collision hit;
-    float warming;
+    float duration;
     int expiry;
 } Contact;
 typedef struct RigidBody RigidBody;

src/transform.h

@@ -46,6 +46,11 @@ Vector transform_point(Transform* self, Vector position) {
     return vaddf(vmulf(vrotatef(position, self->rotation), self->scale), self->position);
 }
 
+static inline
+Vector inverse_transform_point(Transform* self, Vector position) {
+    return vrotatef(vmulf(vsubf(position, self->position), vreciprocalf(self->scale)), -self->rotation);
+}
+
 static
 Transform* transform_get_transform(Transform* self) {
     return self;

src/vmath.h

@@ -139,6 +139,14 @@ static inline
 Vector vmovetowardsf(Vector start, Vector end, float delta) {
     return vlerpf(start, end, delta / vdistancef(end, start));
 }
+static inline
+Vector vaveragef(Vector* array, size_t count) {
+    Vector acc = ZeroVector;
+    for(size_t i = 0; i < count; ++i) {
+        acc = vaddf(acc, array[i]);
+    }
+    return vmulff(acc, 1.0/(float)count);
+}
 
 static inline
 IVector vaddi(IVector a, IVector b) {