more physics testing

src/entity.h

@@ -12,7 +12,7 @@ enum entity_prop {
     ENTITY_PROP_RELEASE,
 
     ENTITY_PROP_PHYSICAL,
-    ENTITY_PROP_CONTACT,
+    ENTITY_PROP_MANIFOLD,
 
     ENTITY_PROP_PLAYER_CONTROLLED,
     ENTITY_PROP_CAMERA,
@@ -62,15 +62,18 @@ struct entity_store {
 
 
 
-struct entity_contact_point {
-    struct v2 local;
-};
 
-struct entity_contact_pair {
+struct contact {
-    struct entity_contact_point p0, p1;
+    struct v2 p0_local;
-    struct v2 pen;
+    struct v2 p0_initial_world;
-};
 
+    struct v2 p1_local;
+    struct v2 p1_initial_world;
+
+    f32 depth;          /* How far is p0 from p1 along normal */
+
+    b32 persisted;
+};
 
 
 struct entity {
@@ -121,20 +124,14 @@ struct entity {
 
 
     /* ====================================================================== */
-    /* Contact */
+    /* Manifold */
-
-    /* ENTITY_PROP_CONTACT */
-    struct entity_handle contact_e0;
-    struct entity_handle contact_e1;
-    //struct entity_contact_point contact_points[4];
-    struct entity_contact_pair contact_pairs[4];
-    u32 num_contact_pairs;
-    u32 next_pair_index;
-
-
-
-
 
+    /* ENTITY_PROP_MANIFOLD */
+    struct entity_handle manifold_e0;
+    struct entity_handle manifold_e1;
+    struct v2 manifold_normal;   /* Normal vector of collision from e0 -> e1 */
+    struct contact contacts[2];
+    u32 num_contacts;
 
 
 

src/game.c

@@ -125,10 +125,11 @@ INTERNAL void spawn_test_entities(void)
         //struct v2 pos = V2(1.1230469346046448864129274625156, -1);  /* Touching right side of box */
         //struct v2 pos = V2(1.1230469346046448864129274625156 - 0.0001, -1);  /* Touching right side of box */
         //struct v2 pos = V2(0.374142020941, -0.246118023992);  /* Touching glitch spot */
-        struct v2 size = V2(1, 1);
+        //struct v2 size = V2(1, 1);
-        //f32 r = PI / 4;
+        struct v2 size = V2(0.5, 0.5);
+        f32 r = PI / 4;
         //f32 r = PI / 3;
-        f32 r = PI / 2;
+        //f32 r = PI / 2;
         //f32 r = 0;
         f32 skew = 0;
 
@@ -138,20 +139,22 @@ INTERNAL void spawn_test_entities(void)
         xf = xform_skewed_to(xf, skew);
         entity_set_xform(e, xf);
 
-        e->sprite = sprite_tag_from_path(STR("res/graphics/tim.ase"));
+        //e->sprite = sprite_tag_from_path(STR("res/graphics/tim.ase"));
+        e->sprite = sprite_tag_from_path(STR("res/graphics/box.ase"));
         //e->sprite_span_name = STR("idle.unarmed");
         //e->sprite_span_name = STR("idle.one_handed");
         e->sprite_span_name = STR("idle.two_handed");
 
         entity_enable_prop(e, ENTITY_PROP_PLAYER_CONTROLLED);
-        e->control_force = 4500;
+        //e->control_force = 4500;
-        //e->control_force = 1200;
+        e->control_force = 1200;
         e->control_torque = 10;
         e->control.focus = V2(0, -1);
 
         entity_enable_prop(e, ENTITY_PROP_PHYSICAL);
         e->mass_unscaled = 100;
-        e->inertia_unscaled = 25;
+        //e->inertia_unscaled = 1;
+        e->inertia_unscaled = F32_INFINITY;
         e->linear_ground_friction = 1000;
         e->angular_ground_friction = 100;
 
@@ -577,6 +580,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
                 bullet->bullet_impulse = 0.25;
                 //bullet->bullet_impulse = 1;
                 bullet->mass_unscaled = 0.04;
+                bullet->inertia_unscaled = 0.00001;
 
                 entity_enable_prop(bullet, ENTITY_PROP_BULLET);
             }
@@ -770,7 +774,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
      * Create ground friction force (gravity)
      * ========================== */
 
-#if 1
+#if 0
     /* TODO: Do this globally rather than creating entities for constant forces */
     for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
         struct entity *ent = &store->entities[entity_index];
@@ -829,7 +833,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
 #endif
 
     /* ========================== *
-     * Integrate forces into velocity
+     * Integrate velocities from forces
      * ========================== */
 
     for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
@@ -856,7 +860,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
     }
 
     /* ========================== *
-     * Create contact points
+     * Create contact manifolds
      * ========================== */
 
     for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) {
@@ -909,94 +913,210 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
                 dict = fixed_dict_init(&dict_arena, 4096);
             }
 
-            struct string contact_key;
+            struct string manifold_key;
             {
                 /* FIXME: Contact should be same regardless of entity order */
-                u64 contact_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&e0->handle));
+                u64 manifold_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&e0->handle));
-                contact_hash = hash_fnv64(contact_hash, BUFFER_FROM_STRUCT(&e1->handle));
+                manifold_hash = hash_fnv64(manifold_hash, BUFFER_FROM_STRUCT(&e1->handle));
-                contact_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&contact_hash));
+                manifold_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&manifold_hash));
             }
 
-            struct entity *contact = fixed_dict_get(&dict, contact_key);
+            struct entity *manifold = fixed_dict_get(&dict, manifold_key);
 
 
 
 
-            const f32 min_new_contact_point_dist_sq = 0.05 * 0.05;
+#if 1
+            const f32 remove_contact_threshold_global_dist_sq       = 0.005 * 0.005;
+            const f32 insert_contact_threshold_global_dist_sq       = 0.005 * 0.005;
+#else
+            const f32 remove_contact_threshold_global_dist_sq       = 0;
+            const f32 insert_contact_threshold_global_dist_sq       = 0;
+#endif
+
+
 
             struct gjk_contact_points_result res = gjk_contact_points(e0_poly, e1_poly);
             if (res.num_pairs > 0) {
-                if (!contact) {
+                if (!manifold) {
-                    contact = entity_alloc(root);
+                    manifold = entity_alloc(root);
-                    contact->contact_e0 = e0->handle;
+                    manifold->manifold_e0 = e0->handle;
-                    contact->contact_e1 = e1->handle;
+                    manifold->manifold_e1 = e1->handle;
-                    entity_enable_prop(contact, ENTITY_PROP_CONTACT);
+                    /* TODO: Should we recalculate normal as more contact points are added? */
-                    activate_now(contact);
+                    manifold->manifold_normal = v2_norm(v2_sub(res.pairs[0].p1, res.pairs[0].p0));
-                    fixed_dict_set(&dict_arena, &dict, contact_key, contact);
+                    entity_enable_prop(manifold, ENTITY_PROP_MANIFOLD);
+                    activate_now(manifold);
+                    fixed_dict_set(&dict_arena, &dict, manifold_key, manifold);
                 }
-                /* TODO: Dont need to do insertion logic for newly created contacts */
+
+
+
+
+                /* TODO: Remove this (debugging) */
+                manifold->prototype = res.prototype;
+                manifold->simplex = res.simplex;
+
+
+
+
+
+
+
+
+                /* Remove contacts that are no longer penetrating or have drifted too far from original position */
+                struct v2 old_normal = manifold->manifold_normal;
+                for (i32 j = 0; j < (i32)manifold->num_contacts; ++j) {
+                    struct contact *contact = &manifold->contacts[j];
+
+                    struct v2 p0_world_initial = contact->p0_initial_world;
+                    struct v2 p1_world_initial = contact->p1_initial_world;
+
+                    struct v2 p0_world_cur = xform_mul_v2(e0_xf, contact->p0_local);
+                    struct v2 p1_world_cur = xform_mul_v2(e1_xf, contact->p1_local);
+
+                    f32 p0_close_enough = v2_len_sq(v2_sub(p0_world_cur, p0_world_initial)) <= remove_contact_threshold_global_dist_sq;
+                    f32 p1_close_enough = v2_len_sq(v2_sub(p1_world_cur, p1_world_initial)) <= remove_contact_threshold_global_dist_sq;
+                    b32 still_penetrating = v2_dot(old_normal, v2_sub(p1_world_cur, p0_world_cur)) >= 0;
+
+                    if (still_penetrating && p0_close_enough && p1_close_enough) {
+                        /* Contact is still penetrating, mark persisted for warm starting */
+                        contact->persisted = true;
+                    } else {
+                        /* Remove contact by swapping with last in array */
+                        manifold->contacts[j--] = manifold->contacts[--manifold->num_contacts];
+                    }
+                }
+
+
+
+
+
+                /* FIXME: Test overflow */
+                struct contact overflow[2];
+                u32 overflow_count = 0;
+                CT_ASSERT(ARRAY_COUNT(overflow) >= ARRAY_COUNT(res.pairs));  /* Overflow should be able to hold collision result */
+
+
+                /* Insert new contacts that aren't too close to original contacts */
                 for (u32 i = 0; i < res.num_pairs; ++i) {
                     b32 should_insert = true;
 
                     struct gjk_contact_pair new_pair = res.pairs[i];
-                    struct v2 new_p0_world = new_pair.p0;
+                    struct v2 p0_world = new_pair.p0;
-                    struct v2 new_p1_world = new_pair.p1;
+                    struct v2 p1_world = new_pair.p1;
-                    struct v2 new_p0_local = xform_invert_mul_v2(e0_xf, new_p0_world);
-                    struct v2 new_p1_local = xform_invert_mul_v2(e1_xf, new_p1_world);
 
-                    struct v2 pen = v2_sub(new_p1_world, new_p0_world);
+                    for (u32 j = 0; j < manifold->num_contacts; ++j) {
-
+                        struct contact *contact = &manifold->contacts[j];
-                    i32 replace_index = -1;
+                        struct v2 old_p0_world = xform_mul_v2(e0_xf, contact->p0_local);
-                    for (u32 j = 0; j < contact->num_contact_pairs; ++j) {
+                        struct v2 old_p1_world = xform_mul_v2(e1_xf, contact->p1_local);
-                        struct entity_contact_pair old_pair = contact->contact_pairs[j];
+                        f32 p0_far_enough = v2_len_sq(v2_sub(p0_world, old_p0_world)) >= insert_contact_threshold_global_dist_sq;
-
+                        f32 p1_far_enough = v2_len_sq(v2_sub(p1_world, old_p1_world)) >= insert_contact_threshold_global_dist_sq;
-                        struct v2 old_p0_local = old_pair.p0.local;
+                        if (!p0_far_enough || !p1_far_enough) {
-                        struct v2 old_p1_local = old_pair.p1.local;
+                            should_insert = false;
-
-                        f32 p0_dist_sq = v2_len_sq(v2_sub(new_p0_local, old_p0_local));
-                        f32 p1_dist_sq = v2_len_sq(v2_sub(new_p1_local, old_p1_local));
-
-                        if (p0_dist_sq < min_new_contact_point_dist_sq || p1_dist_sq < min_new_contact_point_dist_sq) {
-                            replace_index = j;
-                            //should_insert = false;
                             break;
                         }
                     }
+
                     if (should_insert) {
-                        u32 index;
+                        struct contact *c;
-                        if (replace_index >= 0) {
+                        if (manifold->num_contacts < ARRAY_COUNT(manifold->contacts)) {
-                            index = replace_index;
+                            c = &manifold->contacts[manifold->num_contacts++];
-                            contact->next_pair_index = index + 1;
-                            if (contact->next_pair_index >= ARRAY_COUNT(contact->contact_pairs)) {
-                                contact->next_pair_index = 0;
-                            }
                         } else {
-                            index = contact->num_contact_pairs;
+                            c = &overflow[overflow_count++];
-                            if (index >= ARRAY_COUNT(contact->contact_pairs)) {
-                                index = contact->next_pair_index;
-                                if (index >= (ARRAY_COUNT(contact->contact_pairs) - 1)) {
-                                    contact->next_pair_index = 0;
-                                } else {
-                                    ++contact->next_pair_index;
                         }
-                            } else {
+
-                                ++contact->num_contact_pairs;
+                        MEMZERO_STRUCT(c);
+                        c->p0_local = xform_invert_mul_v2(e0_xf, p0_world);
+                        c->p0_initial_world = p0_world;
+                        c->p1_local = xform_invert_mul_v2(e1_xf, p1_world);
+                        c->p1_initial_world = p1_world;
+                        c->depth = v2_len(v2_sub(p1_world, p0_world));
                     }
                 }
-                        contact->contact_pairs[index] = (struct entity_contact_pair) {
+
-                            .p0 = { .local = new_p0_local },
+                /* Manifold already has max amount of contacts, decide which 2 to keep.
-                            .p1 = { .local = new_p1_local },
+                 * Logic: 1st contact to keep is deepest contact, 2nd contact is furthest point from 1st contact (in local space of first shape) */
-                            .pen = pen
+                if (overflow_count > 0) {
-                        };
+                    /* Find contact with greatest depth */
+                    struct contact *deepest = NULL;
+                    {
+                        f32 deepest_depth = -F32_INFINITY;
+                        for (u32 i = 0; i < manifold->num_contacts; ++i) {
+                            struct contact *c = &manifold->contacts[i];
+                            f32 depth = c->depth;
+                            if (depth > deepest_depth) {
+                                deepest = c;
+                                deepest_depth = depth;
                             }
                         }
-                /* TODO: Remove this (debugging) */
+                        for (u32 i = 0; i < overflow_count; ++i) {
-                contact->prototype = res.prototype;
+                            struct contact *c = &overflow[i];
-                contact->simplex = res.simplex;
+                            f32 depth = c->depth;
-            } else if (contact) {
+                            if (depth > deepest_depth) {
+                                deepest = c;
+                                deepest_depth = depth;
+                            }
+                        }
+                    }
+                    /* Find contact furthest from deepest point */
+                    struct contact *furthest = NULL;
+                    {
+                        struct v2 deepest_p0 = deepest->p0_local;
+                        f32 furthest_dist_sq = -F32_INFINITY;
+                        for (u32 i = 0; i < manifold->num_contacts; ++i) {
+                            struct contact *c = &manifold->contacts[i];
+                            f32 dist_sq = v2_len_sq(v2_sub(c->p0_local, deepest_p0));
+                            if (dist_sq > furthest_dist_sq) {
+                                furthest = c;
+                                furthest_dist_sq = dist_sq;
+                            }
+                        }
+                        for (u32 i = 0; i < overflow_count; ++i) {
+                            struct contact *c = &manifold->contacts[i];
+                            f32 dist_sq = v2_len_sq(v2_sub(c->p0_local, deepest_p0));
+                            if (dist_sq > furthest_dist_sq) {
+                                furthest = c;
+                                furthest_dist_sq = dist_sq;
+                            }
+                        }
+                    }
+                    /* Copy */
+                    /* TODO: Optimize */
+                    CT_ASSERT(ARRAY_COUNT(manifold->contacts) == 2);  /* Logic assumes 2 contact points */
+                    manifold->contacts[0] = *deepest;
+                    manifold->contacts[1] = *furthest;
+                }
+
+
+
+
+                /* NOTE: This assumes that all contacts will share the same normal as first contact */
+                manifold->manifold_normal = v2_norm(v2_sub(manifold->contacts[0].p1_initial_world, manifold->contacts[0].p0_initial_world));
+
+
+
+
+
+                /* TODO: Remove this (testing) */
+#if 0
+                if (manifold->num_contacts > 1) {
+                    if (manifold->contacts[1].depth > manifold->contacts[1].depth) {
+                        manifold->contacts[0] = manifold->contacts[1];
+                    }
+                    manifold->num_contacts = 1;
+                }
+#endif
+
+
+
+
+
+
+
+            } else if (manifold) {
                 /* No longer colliding, delete contact */
-                contact->num_contact_pairs = 0;
+                manifold->num_contacts = 0;
-                fixed_dict_set(&dict_arena, &dict, contact_key, NULL);
+                fixed_dict_set(&dict_arena, &dict, manifold_key, NULL);
             }
         }
     }
@@ -1006,14 +1126,14 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
      * ========================== */
 
     for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
-        struct entity *contact = &store->entities[entity_index];
+        struct entity *manifold = &store->entities[entity_index];
-        if (!(contact->valid && entity_has_prop(contact, ENTITY_PROP_ACTIVE))) continue;
+        if (!(manifold->valid && entity_has_prop(manifold, ENTITY_PROP_ACTIVE))) continue;
-        if (!entity_has_prop(contact, ENTITY_PROP_CONTACT)) continue;
+        if (!entity_has_prop(manifold, ENTITY_PROP_MANIFOLD)) continue;
 
-        struct entity *e0 = entity_from_handle(store, contact->contact_e0);
+        struct entity *e0 = entity_from_handle(store, manifold->manifold_e0);
-        struct entity *e1 = entity_from_handle(store, contact->contact_e1);
+        struct entity *e1 = entity_from_handle(store, manifold->manifold_e1);
 
-        if (contact->num_contact_pairs > 0 && e0->valid && e1->valid) {
+        if (manifold->num_contacts > 0 && e0->valid && e1->valid) {
             struct xform e0_xf = entity_get_xform(e0);
             struct xform e1_xf = entity_get_xform(e1);
 
@@ -1033,24 +1153,23 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
             struct v2 e1_accumulated_linear_impulse = { 0 };
             f32 e1_accumulated_angular_impulse = 0;
 
-            //f32 e1_accumulated_impulse = 0;
+            struct v2 normal = manifold->manifold_normal;
+            for (u32 contact_index = 0; contact_index < manifold->num_contacts; ++contact_index) {
+                struct contact *contact = &manifold->contacts[contact_index];
 
-            for (u32 pair_index = 0; pair_index < contact->num_contact_pairs; ++pair_index) {
+                struct v2 p0 = xform_mul_v2(e0_xf, contact->p0_local);
-                struct entity_contact_pair pair = contact->contact_pairs[pair_index];
+                struct v2 p1 = xform_mul_v2(e1_xf, contact->p1_local);
-                struct v2 p0 = xform_mul_v2(e0_xf, pair.p0.local);
+                f32 depth = contact->depth;
-                struct v2 p1 = xform_mul_v2(e1_xf, pair.p1.local);
-
-                struct v2 pen = pair.pen;
-                struct v2 pen_norm = v2_norm(pen);
 
         #if 0
                 f32 bias_factor = 0.2;
-                //f32 bias_slop = 0.1;
                 f32 bias_slop = 0.001;
+                //f32 bias_slop = 0.001;
                 //f32 bias_slop = 0.00;
 
-                f32 bias = (bias_factor / dt) * max((v2_len(pen) - bias_slop), 0);
+                f32 bias = (bias_factor / dt) * max((depth - bias_slop), 0);
         #else
+                (UNUSED)depth;
                 f32 bias = 0;
         #endif
 
@@ -1063,20 +1182,19 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
 
                 struct v2 vrel = v2_sub(p1_vel, p0_vel);
 
-                f32 vn = v2_dot(vrel, pen_norm);
+                f32 vn = v2_dot(vrel, normal);
                 vn = max_f32(vn, 0);
 
+                struct v2 idk0 = v2_perp_mul(vcp0, v2_wedge(vcp0, normal) * inv_i0);
+                struct v2 idk1 = v2_perp_mul(vcp1, v2_wedge(vcp1, normal) * inv_i1);
 
-                struct v2 idk0 = v2_perp_mul(vcp0, v2_wedge(vcp0, pen_norm) * inv_i0);
+                f32 k = inv_m0 + inv_m1 + v2_dot(normal, v2_add(idk0, idk1));
-                struct v2 idk1 = v2_perp_mul(vcp1, v2_wedge(vcp1, pen_norm) * inv_i1);
-
-                f32 k = inv_m0 + inv_m1 + v2_dot(pen_norm, v2_add(idk0, idk1));
 
                 /* (to be applied along n) */
                 f32 j = (vn + bias) / k;
                 //j = max_f32(j, 0);
 
-                struct v2 imp = v2_mul(pen_norm, j);
+                struct v2 imp = v2_mul(normal, j);
 
                 if (!e0->test_collided) {
                     e0->test_collided = true;
@@ -1088,8 +1206,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
                 e0_accumulated_angular_impulse += v2_wedge(vcp0, imp);
                 e1_accumulated_angular_impulse += v2_wedge(vcp1, v2_neg(imp));
 
-
-
                 //entity_apply_linear_impulse(e0, imp, p0);
                 //entity_apply_linear_impulse(e1, v2_neg(imp), p1);
             }
@@ -1097,194 +1213,30 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
             //entity_apply_linear_impulse(e0, imp, p0);
             //entity_apply_linear_impulse(e1, v2_neg(imp), p1);
 
-            /* FIXME: Clamp */
+
+            /* TODO: Remove this (testing) */
+#if 1
+            /* Divide impulses by number of contact points. */
+            {
+                f32 inv_num_contacts = 1.f / manifold->num_contacts;
+                e0_accumulated_linear_impulse = v2_mul(e0_accumulated_linear_impulse, inv_num_contacts);
+                e1_accumulated_linear_impulse = v2_mul(e1_accumulated_linear_impulse, inv_num_contacts);
+                e0_accumulated_angular_impulse *= inv_num_contacts;
+                e1_accumulated_angular_impulse *= inv_num_contacts;
+            }
+#endif
 
             entity_apply_linear_impulse_to_center(e0, e0_accumulated_linear_impulse);
             entity_apply_angular_impulse(e0, e0_accumulated_angular_impulse);
-
             entity_apply_linear_impulse_to_center(e1, e1_accumulated_linear_impulse);
             entity_apply_angular_impulse(e1, e1_accumulated_angular_impulse);
         } else {
-            entity_enable_prop(contact, ENTITY_PROP_RELEASE);
+            entity_enable_prop(manifold, ENTITY_PROP_RELEASE);
         }
     }
 
-
-
-
-
-
-
-
-
-
-
-#if 0
-    for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) {
-        struct entity *e0 = &store->entities[e0_index];
-        if (!(e0->valid && entity_has_prop(e0, ENTITY_PROP_ACTIVE))) continue;
-        if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue;
-        if (!entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED) && !entity_has_prop(e0, ENTITY_PROP_BULLET)) continue;
-
-        struct xform e0_xf = entity_get_xform(e0);
-        struct quad e0_quad;
-        struct v2_array e0_poly;
-        {
-            struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e0->sprite);
-            struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e0->animation_frame);
-            e0_quad = xform_mul_quad(e0->sprite_local_xform, quad_from_rect(slice.rect));
-            e0_quad = xform_mul_quad(e0_xf, e0_quad);
-            e0_poly = (struct v2_array) {
-                .count = ARRAY_COUNT(e0_quad.e),
-                .points = e0_quad.e
-            };
-        }
-
-        b32 colliding = false;
-        struct gjk_contact_pair pair0 = { 0 };
-        struct gjk_contact_pair pair1 = { 0 };
-        u32 num_pairs = 0;
-        struct entity *colliding_with = entity_nil();
-        struct gjk_simplex simplex = { 0 };
-        struct gjk_prototype prototype = { 0 };
-        struct v2 velocity = V2(0, 0);
-        b32 solved = false;
-        for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) {
-            struct entity *e1 = &store->entities[e1_index];
-            if (e1 == e0) continue;
-            if (!(e1->valid && entity_has_prop(e1, ENTITY_PROP_ACTIVE))) continue;
-            if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue;
-            if (entity_has_prop(e1, ENTITY_PROP_PLAYER_CONTROLLED)) continue;
-
-            struct xform e1_xf = entity_get_xform(e1);
-            struct quad e1_quad;
-            struct v2_array e1_poly;
-            {
-                struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, e1->sprite);
-                struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), e1->animation_frame);
-                e1_quad = xform_mul_quad(e1->sprite_local_xform, quad_from_rect(slice.rect));
-                e1_quad = xform_mul_quad(e1_xf, e1_quad);
-                e1_poly = (struct v2_array) {
-                    .count = ARRAY_COUNT(e1_quad.e),
-                    .points = e1_quad.e
-                };
-            }
-
-            struct gjk_contact_points_result res = gjk_contact_points(e0_poly, e1_poly);
-
-            num_pairs = res.num_pairs;
-            colliding = num_pairs > 0;
-            pair0 = res.pairs[0];
-            pair1 = res.pairs[1];
-            colliding_with = e1;
-            simplex = res.simplex;
-            prototype = res.prototype;
-            solved = res.solved;
-        }
-
-        e0->colliding_with = colliding_with->handle;
-        e0->point0 = pair0.p0;
-        e0->point1 = pair1.p0;
-        e0->num_points = num_pairs;
-        e0->simplex = simplex;
-        e0->prototype = prototype;
-        e0->pendir = velocity;
-        e0->solved = solved;
-
-        if (colliding_with->valid) {
-            colliding_with->colliding_with = e0->handle;
-            colliding_with->point0 = pair0.p1;
-            colliding_with->point1 = pair1.p1;
-            colliding_with->num_points = num_pairs;
-        }
-
-        (UNUSED)colliding;
-        {
-#if 0
-            if (colliding) {
-                struct entity *e1 = colliding_with;
-                struct xform e1_xf = entity_get_xform(e1);
-
-                f32 scale0 = math_fabs(xform_get_determinant(e0_xf));
-                f32 scale1 = math_fabs(xform_get_determinant(e1_xf));
-                f32 m0 = e0->mass_unscaled * scale0;
-                f32 m1 = e1->mass_unscaled * scale1;
-                f32 i0 = e0->inertia_unscaled * scale0;
-                f32 i1 = e1->inertia_unscaled * scale1;
-                f32 inv_m0 = 1.f / m0;
-                f32 inv_m1 = 1.f / m1;
-                f32 inv_i0 = 1.f / i0;
-                f32 inv_i1 = 1.f / i1;
-                (UNUSED)inv_m0;
-                (UNUSED)inv_m1;
-                (UNUSED)inv_i0;
-                (UNUSED)inv_i1;
-
-                struct v2 pen = v2_sub(p1, p0);
-                struct v2 pen_norm = v2_norm(pen);
-
-#if 1
-                f32 bias_factor = 0.2;
-                f32 bias_slop = 0.001;
-
-                f32 bias = (bias_factor / dt) * max((v2_len(pen) - bias_slop), 0);
-#else
-                f32 bias = 0;
-#endif
-
-
-                struct v2 vcp0 = v2_sub(p0, e0_xf.og);
-                struct v2 vcp1 = v2_sub(p1, e1_xf.og);
-
-                struct v2 p0_vel = v2_add(e0->linear_velocity, v2_perp_mul(vcp0, e0->angular_velocity));
-                struct v2 p1_vel = v2_add(e1->linear_velocity, v2_perp_mul(vcp1, e1->angular_velocity));
-
-                struct v2 vrel = v2_sub(p1_vel, p0_vel);
-
-                f32 vn = v2_dot(vrel, pen_norm);
-                vn = max_f32(vn, 0);
-
-
-                struct v2 idk0 = v2_perp_mul(vcp0, v2_wedge(vcp0, pen_norm) * inv_i0);
-                struct v2 idk1 = v2_perp_mul(vcp1, v2_wedge(vcp1, pen_norm) * inv_i1);
-
-                f32 k = inv_m0 + inv_m1 + v2_dot(pen_norm, v2_add(idk0, idk1));
-
-                /* (to be applied along n) */
-                f32 j = (vn + bias) / k;
-                //j = max_f32(j, 0);
-
-
-
-                if (j > 0) {
-                    DEBUGBREAKABLE;
-                }
-
-                if (!e0->test_collided) {
-                    e0->test_collided = true;
-                }
-
-
-
-                struct v2 imp = v2_mul(pen_norm, j);
-                (UNUSED)imp;
-
-                //imp = v2_mul(imp, dt);
-
-
-#if 1
-                entity_apply_linear_impulse(e0, imp, p0);
-                entity_apply_linear_impulse(e1, v2_neg(imp), p1);
-#endif
-
-            }
-#endif
-        }
-    }
-#endif
-
     /* ========================== *
-     * Update positions
+     * Integrate positions from velocity
      * ========================== */
 
     for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
@@ -1330,6 +1282,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
      * Update camera position
      * ========================== */
 
+#if 0
     for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
         struct entity *ent = &store->entities[entity_index];
         if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;
@@ -1366,6 +1319,44 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
             ent->camera_applied_lerp_continuity_gen_plus_one = ent->camera_lerp_continuity_gen + 1;
             entity_set_xform(ent, xf);
         }
+#else
+    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
+        struct entity *ent = &store->entities[entity_index];
+        if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;
+
+        /* Camera follow */
+        if (entity_has_prop(ent, ENTITY_PROP_CAMERA)) {
+            struct entity *follow = entity_from_handle(store, ent->camera_follow);
+
+            struct xform xf = entity_get_xform(ent);
+
+            f32 aspect_ratio = 1.0;
+            {
+                struct xform quad_xf = xform_mul(entity_get_xform(ent), ent->camera_quad_xform);
+                struct v2 camera_size = xform_get_scale(quad_xf);
+                if (!v2_is_zero(camera_size)) {
+                    aspect_ratio = camera_size.x / camera_size.y;
+                }
+            }
+            f32 ratio_y = 0.33f;
+            f32 ratio_x = ratio_y / aspect_ratio;
+            struct v2 camera_focus_dir = v2_mul_v2(follow->control.focus, V2(ratio_x, ratio_y));
+            struct v2 camera_focus_pos = v2_add(entity_get_xform(follow).og, camera_focus_dir);
+            ent->camera_xform_target = xf;
+            ent->camera_xform_target.og = camera_focus_pos;
+
+            /* Lerp camera */
+            if (ent->camera_applied_lerp_continuity_gen_plus_one == ent->camera_lerp_continuity_gen + 1) {
+                f32 t = 1 - math_pow(2.f, -20.f * (f32)G.tick.dt);
+                xf = xform_lerp(xf, ent->camera_xform_target, t);
+            } else {
+                /* Skip lerp */
+                xf = ent->camera_xform_target;
+            }
+            ent->camera_applied_lerp_continuity_gen_plus_one = ent->camera_lerp_continuity_gen + 1;
+            entity_set_xform(ent, xf);
+        }
+#endif
     }
 
     /* ========================== *

src/gjk.c

@@ -155,7 +155,8 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
     struct gjk_contact_points_result res = { 0 };
 
     /* TODO: Verify epsilon */
-    const f32 epsilon = 0.0000100;
+    /* FIXME: Infinite loop when epsilon too low and shapes is too far from 0 (precision issue) */
+    const f32 epsilon = 0.00100;
     struct gjk_simplex s = { 0 };
     b32 colliding = false;
     struct gjk_menkowski_point *proto = NULL;
@@ -236,9 +237,9 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru
             struct v2 rab_dir = v2_perp_towards_dir(vab, v2_neg(vac));
             struct v2 rac_dir = v2_perp_towards_dir(vac, v2_neg(vab));
             struct v2 rbc_dir = v2_perp_towards_dir(vbc, vab);
-            voronoi_mask |= (v2_dot(rab_dir, v2_neg(s.a.p)) >= 0) << 0;  /* Regions ab, a, and b*/
+            voronoi_mask |= (v2_dot(rab_dir, v2_neg(s.a.p)) > 0) << 0;  /* Regions ab, a, and b*/
-            voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a.p)) >= 0) << 1;  /* Regions ac, a, and c */
+            voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a.p)) > 0) << 1;  /* Regions ac, a, and c */
-            voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b.p)) >= 0) << 2;  /* Regions bc, b, and c */
+            voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b.p)) > 0) << 2;  /* Regions bc, b, and c */
             /* Remove point or edge and determine next direction based on voronoi region */
             switch (voronoi_mask) {
                 default:

src/math.h

@@ -1000,6 +1000,7 @@ INLINE struct v2 xform_basis_invert_mul_v2(struct xform xf, struct v2 v)
     return res;
 }
 
+/* TODO: Get rid of this? Just force caller to use invert manually since it's expensive. */
 INLINE struct v2 xform_invert_mul_v2(struct xform xf, struct v2 v)
 {
     struct xform inv = xform_invert(xf);

src/user.c

@@ -989,23 +989,27 @@ INTERNAL void user_update(void)
             }
 
             /* Draw contact */
-            if (entity_has_prop(ent, ENTITY_PROP_CONTACT)) {
+            if (entity_has_prop(ent, ENTITY_PROP_MANIFOLD)) {
                 f32 radius = 5;
-                struct entity *e0 = entity_from_handle(store, ent->contact_e0);
+                struct entity *e0 = entity_from_handle(store, ent->manifold_e0);
-                struct entity *e1 = entity_from_handle(store, ent->contact_e1);
+                struct entity *e1 = entity_from_handle(store, ent->manifold_e1);
                 struct xform e0_xf = entity_get_xform(e0);
                 struct xform e1_xf = entity_get_xform(e1);
-                for (u32 i = 0; i < ent->num_contact_pairs; ++i) {
+                (UNUSED)e0_xf;
-                    struct entity_contact_pair pair = ent->contact_pairs[i];
+                (UNUSED)e1_xf;
+                for (u32 i = 0; i < ent->num_contacts; ++i) {
+                    struct contact contact = ent->contacts[i];
                     {
                         u32 color = entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 0, 0, 0.50) : RGBA_32_F(0, 1, 1, 0.50);
-                        struct v2 point = xform_mul_v2(e0_xf, pair.p0.local);
+                        //struct v2 point = xform_mul_v2(e0_xf, contact.p0_local);
+                        struct v2 point = contact.p0_initial_world;
                         point = xform_mul_v2(G.world_view, point);
                         draw_solid_circle(G.viewport_canvas, point, radius, color, 10);
                     }
                     {
                         u32 color = entity_has_prop(e1, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 0, 0, 0.50) : RGBA_32_F(0, 1, 1, 0.50);
-                        struct v2 point = xform_mul_v2(e1_xf, pair.p1.local);
+                        //struct v2 point = xform_mul_v2(e1_xf, contact.p1_local);
+                        struct v2 point = contact.p1_initial_world;
                         point = xform_mul_v2(G.world_view, point);
                         draw_solid_circle(G.viewport_canvas, point, radius, color, 10);
                     }
@@ -1214,8 +1218,8 @@ INTERNAL void user_update(void)
         /* Queue player move cmd */
         f32 move_speed = 1.0f;
         if (G.bind_states[USER_BIND_KIND_WALK].is_held) {
-            const f32 walk_ratio = 0.25f;
+            //const f32 walk_ratio = 0.25f;
-            //const f32 walk_ratio = 0.01f;
+            const f32 walk_ratio = 0.05f;
             move_speed *= walk_ratio;
         }