divide warm start impulse by num contacts

src/collider.c

@@ -247,8 +247,8 @@ struct collider_collision_points_result collider_collision_points(struct collide
          * Epa (to find collision normal from inside shape)
          * ========================== */
 
-        const f32 epa_epsilon_sq = 0.001f * 0.001f;
+        //const f32 epa_epsilon_sq = 0.001f * 0.001f;
-        //const f32 epa_epsilon_sq = F32_INFINITY;
+        const f32 epa_epsilon_sq = F32_INFINITY;
 
         proto = arena_dry_push(scratch.arena, struct v2);
         proto_count = 0;
@@ -544,14 +544,24 @@ struct collider_collision_points_result collider_collision_points(struct collide
             struct v2 contact_a = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f));
             struct v2 contact_b = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f));
 
-            if (a_sep < tolerance) {
+            b32 ignore_a = false;
+            b32 ignore_b = false;
+            if (v2_len_sq(v2_sub(contact_b, contact_a)) < 0.001f) {
+                if (a_sep > b_sep) {
+                    ignore_a = true;
+                } else {
+                    ignore_b = true;
+                }
+            }
+
+            if (a_sep < tolerance && !ignore_a) {
                 struct collider_collision_point *point = &points[num_points++];
                 point->id = id_a0 | (id_a1 << 4);
                 point->separation = a_sep;
                 point->point = contact_a;
             }
 
-            if (b_sep < tolerance) {
+            if (b_sep < tolerance && !ignore_b) {
                 struct collider_collision_point *point = &points[num_points++];
                 point->id = id_b0 | (id_b1 << 4);
                 point->separation = b_sep;

src/config.h

@@ -32,11 +32,11 @@
 #define GAME_FPS 50.0
 #define GAME_TIMESCALE 1.0
 
-#define GAME_PHYSICS_SUBSTEPS 4
+#define GAME_PHYSICS_SUBSTEPS 8
 #define GAME_PHYSICS_ENABLE_WARM_STARTING 1
 
 #define GAME_MAX_LINEAR_VELOCITY 100
-#define GAME_MAX_ANGULAR_VELOCITY ((2 * PI) * 5)
+#define GAME_MAX_ANGULAR_VELOCITY ((2 * PI) * 20)
 
 /* How many ticks back in time should the user blend between?
  * <Delay ms> = <USER_INTERP_OFFSET_TICK_RATIO> * <Game tick rate>

src/entity.h

@@ -118,7 +118,7 @@ struct entity {
 
 
     /* TODO: Remove this (testing) */
-    b32 colliding;
+    i32 colliding;
     struct collider_collision_points_result res;
 
     b32 test_torque_applied;

src/game.c

@@ -222,8 +222,8 @@ INTERNAL void spawn_test_entities(f32 offset)
 
         //entity_enable_prop(e, ENTITY_PROP_PLAYER_CONTROLLED);
         //e->control_force = 4500;
-        e->control_force = 1200;
+        //e->control_force = 1200;
-        //e->control_force = 250;
+        e->control_force = 250;
         e->control_torque = 10;
         e->control.focus = V2(0, -1);
 
@@ -428,11 +428,22 @@ INTERNAL void create_contact_manifolds(void)
                 manifold->manifold_normal = res.normal;
 
                 /* TODO: Remove this (debugging) */
+#if COLLIDER_DEBUG
                 {
-                    e0->colliding = true;
-                    e1->colliding = true;
                     manifold->res = res;
+                    if (manifold->num_contacts == 0) {
+                        if (res.num_points > 0) {
+                            ++e0->colliding;
+                            ++e1->colliding;
+                        }
+                    } else {
+                        if (res.num_points == 0) {
+                            --e0->colliding;
+                            --e1->colliding;
+                        }
+                    }
                 }
+#endif
             }
 
             if (res.num_points > 0) {
@@ -489,9 +500,11 @@ INTERNAL void create_contact_manifolds(void)
                     contact->starting_separation = sep;
 
                     /* TODO: Remove this (debugging) */
+#if COLLIDER_DEBUG
                     {
                         contact->dbg_pt = point;
                     }
+#endif
 
                     {
                         f32 scale0 = math_fabs(xform_get_determinant(e0_xf));
@@ -538,8 +551,6 @@ INTERNAL void create_contact_manifolds(void)
 
             } else if (manifold) {
 #if COLLIDER_DEBUG
-                if (e0->valid) e0->colliding = false;
-                if (e1->valid) e1->colliding = false;
                 manifold->num_contacts = 0;
 #else
                 /* No longer colliding, delete manifold */
@@ -577,6 +588,7 @@ INTERNAL void warm_start_contacts(void)
             /* Warm start */
             struct v2 normal = manifold->manifold_normal;
             struct v2 tangent = v2_perp(normal);
+            f32 inv_num_contacts = 1.f / num_contacts;
             for (u32 i = 0; i < num_contacts; ++i) {
                 struct contact *contact = &manifold->contacts[i];
                 struct v2 p0 = xform_mul_v2(e0_xf, contact->point_local_e0);
@@ -585,7 +597,7 @@ INTERNAL void warm_start_contacts(void)
                 struct v2 vcp1 = v2_sub(p1, e1_xf.og);
 
                 struct v2 impulse = v2_add(v2_mul(normal, contact->normal_impulse), v2_mul(tangent, contact->tangent_impulse));
-                impulse = v2_mul(impulse, 0.5f);
+                impulse = v2_mul(impulse, inv_num_contacts);
 
                 v0 = v2_sub(v0, v2_mul(impulse, contact->inv_m0));
                 v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1));
@@ -673,7 +685,7 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
                 f32 mass_scale = 1.0f;
                 f32 impulse_scale = 0.0f;
 
-                /* Only apply speculative if applying bias */
+#if 1
                 if (separation > 0.0f) {
                     velocity_bias = separation / dt;
                 } else if (apply_bias) {
@@ -690,6 +702,27 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
                     mass_scale = softness.mass_scale;
                     impulse_scale = softness.impulse_scale;
                 }
+#else
+                /* Only apply speculative if applying bias */
+                if (apply_bias) {
+                    if (separation > 0.0f) {
+                        velocity_bias = separation / dt;
+                    } else {
+                        /* Soft constraint */
+
+                        f32 contact_damping_ratio = 10.0f;
+                        f32 contact_hertz = (GAME_FPS * GAME_PHYSICS_SUBSTEPS) / 8.f;
+
+                        struct soft_result softness = make_soft(contact_hertz, contact_damping_ratio, dt);
+
+                        f32 contact_pushout_velocity = 3.0f;
+                        velocity_bias = max_f32(softness.bias_rate * separation, -contact_pushout_velocity);
+
+                        mass_scale = softness.mass_scale;
+                        impulse_scale = softness.impulse_scale;
+                    }
+                }
+#endif
 
                 struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0));
                 struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
@@ -702,7 +735,7 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias)
                 f32 j = ((k * mass_scale) * (vn - velocity_bias)) - (contact->normal_impulse * impulse_scale);
 
                 f32 old_impulse = contact->normal_impulse;
-                f32 new_impulse = max_f32(contact->normal_impulse + j, 0);
+                f32 new_impulse = max_f32(old_impulse + j, 0);
                 f32 delta = new_impulse - old_impulse;
                 contact->normal_impulse = new_impulse;
 
@@ -1018,10 +1051,10 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
             struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), ent->animation_frame);
             ent->local_collider = collider_from_quad(xform_mul_quad(ent->sprite_local_xform, quad_from_rect(slice.rect)));
 
-#if 0
+#if 1
-            //if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) {
+            if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) {
-            if ((true)) {
+            //if ((true)) {
-#if 0
+#if 1
                 ent->local_collider.points[0] = V2(0, 0);
                 ent->local_collider.count = 1;
                 ent->local_collider.radius = 0.5;
@@ -1031,7 +1064,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
                 ent->local_collider.count = 2;
                 ent->local_collider.radius = 0.25;
 #elif 1
-#if 0
+#if 1
                 /* "Bad" winding order */
                 ent->local_collider.points[0] = V2(-0.5, 0.5);
                 ent->local_collider.points[1] = V2(0.5, 0.5);

src/user.c

@@ -786,10 +786,10 @@ INTERNAL void user_update(void)
         u32 x_color = RGBA_32(0x3f, 0, 0, 0xFF);
         u32 y_color = RGBA_32(0, 0x3f, 0, 0xFF);
 
-        i64 startx = -10;
+        i64 rows = 500;
-        i64 starty = -10;
+        i64 cols = 500;
-        i64 rows = 20;
+        i64 startx = -(rows / 2);
-        i64 cols = 20;
+        i64 starty = -(cols / 2);
 
         /* Draw column lines */
         struct v2 col_ray = xform_basis_mul_v2(G.world_view, V2(0, rows));
@@ -835,7 +835,8 @@ INTERNAL void user_update(void)
         struct xform parent_xf = entity_get_xform(parent);
 
         b32 skip_debug_draw = !G.debug_camera && ent == active_camera;
-        b32 skip_debug_draw_transform = entity_has_prop(ent, ENTITY_PROP_CAMERA);
+        //b32 skip_debug_draw_transform = entity_has_prop(ent, ENTITY_PROP_CAMERA);
+        b32 skip_debug_draw_transform = true;
 
         struct xform sprite_xform = xf;
 
@@ -995,12 +996,20 @@ INTERNAL void user_update(void)
             /* Draw collider */
             if (entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) {
                 struct collider_shape collider = ent->local_collider;
-                u32 color = ent->colliding ? RGBA_32_F(1, 1, 1, 0.5) : RGBA_32_F(1, 1, 0, 0.25);
+                u32 color = ent->colliding > 0 ? RGBA_32_F(1, 1, 1, 0.5) : RGBA_32_F(1, 1, 0, 0.25);
                 f32 thickness = 2;
                 //u32 detail = 32;
                 u32 detail = 512;
-                //draw_solid_collider_line(G.viewport_canvas, collider, xform_mul(G.world_view, xf), thickness, color, detail);
                 draw_solid_collider_line(G.viewport_canvas, G.world_view, collider, xf, thickness, color, detail);
+                {
+                    struct v2 start = xf.og;
+                    struct v2 end = collider_support_point(&collider, xf, v2_neg(xf.by));
+                    start = xform_mul_v2(G.world_view, start);
+                    end = xform_mul_v2(G.world_view, end);
+
+                    /* Draw upwards line */
+                    draw_solid_line(G.viewport_canvas, start, end, thickness, color);
+                }
             }
 
             /* Draw collision */
@@ -1136,7 +1145,7 @@ INTERNAL void user_update(void)
                             struct v2 end = xform_mul_v2(G.world_view, v2_add(point, v2_mul(v2_norm(ent->manifold_normal), len)));
                             draw_solid_arrow_line(G.viewport_canvas, start, end, arrow_thickness, arrow_height, color);
                         }
-#if 0
+#if 1
                         /* Draw info text */
                         {
                             struct font *disp_font = font_load_async(STR("res/fonts/fixedsys.ttf"), 12.0f);