contact testing

src/common.h

@@ -526,7 +526,7 @@ struct sprite_tag {
 
 #define BUFFER_FROM_POINTERS(p1, p2) (CPPFRIENDLY_INITLIST_TYPE(struct buffer) { (u8 *)(p2) - (u8 *)(p1), (u8 *)p1 })
 
-#define BUFFER_FROM_STRUCT(ptr) (CPPFRIENDLY_INITLIST_TYPE(struct buffer) { sizeof(*(ptr)), (ptr) })
+#define BUFFER_FROM_STRUCT(ptr) (CPPFRIENDLY_INITLIST_TYPE(struct buffer) { sizeof(*(ptr)), (u8 *)(ptr) })
 
 /* ========================== *
  * String utils

src/entity.c

@@ -312,7 +312,7 @@ void entity_apply_linear_impulse(struct entity *ent, struct v2 impulse, struct v
 
     struct v2 vcp = v2_sub(point, center);
     ent->linear_velocity = v2_add(ent->linear_velocity, v2_mul(impulse, inv_mass));
-    ent->angular_velocity += -v2_wedge(impulse, vcp) * inv_inertia;
+    ent->angular_velocity += v2_wedge(vcp, impulse) * inv_inertia;
 }
 
 #if 0

src/entity.h

@@ -12,6 +12,7 @@ enum entity_prop {
     ENTITY_PROP_RELEASE,
 
     ENTITY_PROP_PHYSICAL,
+    ENTITY_PROP_CONTACT,
 
     ENTITY_PROP_PLAYER_CONTROLLED,
     ENTITY_PROP_CAMERA,
@@ -60,6 +61,13 @@ struct entity_store {
 
 
 
+struct entity_contact_point {
+    struct v2 local;
+};
+
+struct entity_contact_pair {
+    struct entity_contact_point p0, p1;
+};
 
 
 
@@ -100,10 +108,6 @@ struct entity {
     struct v2 pendir;
     b32 solved;
 
-    struct v2 point0;
-    struct v2 point1;
-    u32 num_points;
-
     b32 test_torque_applied;
     b32 test_collided;
 
@@ -114,15 +118,16 @@ struct entity {
 
 
 
-#if 0
     /* ====================================================================== */
     /* Contact */
 
-    struct entity *contact_e0;
+    /* ENTITY_PROP_CONTACT */
-    struct entity *contact_e1;
+    struct entity_handle contact_e0;
-    struct gjk_contact_pair contact_pairs[4];
+    struct entity_handle contact_e1;
+    //struct entity_contact_point contact_points[4];
+    struct entity_contact_pair contact_pairs[4];
     u32 num_contact_pairs;
-#endif
+    u32 next_pair_index;
 
 
 

src/game.c

@@ -128,8 +128,8 @@ INTERNAL void spawn_test_entities(void)
         struct v2 size = V2(1, 1);
         //f32 r = PI / 4;
         //f32 r = PI / 3;
-        //f32 r = PI / 2;
+        f32 r = PI / 2;
-        f32 r = 0;
+        //f32 r = 0;
         f32 skew = 0;
 
         struct entity *e = entity_alloc(root);
@@ -269,11 +269,13 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
 
     /* TODO: remove this (testing) */
     /* Initialize entities */
+    {
         static b32 run = 0;
         if (!run) {
             run = 1;
             spawn_test_entities();
         }
+    }
 
     /* ========================== *
      * Process global game cmds
@@ -827,40 +829,9 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
 #endif
 
     /* ========================== *
-     * Integrate forces
+     * Integrate forces into velocity
      * ========================== */
 
-#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;
-        if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue;
-
-        struct xform xf = entity_get_xform(ent);
-        f32 det_abs = math_fabs(xform_get_determinant(xf));
-        f32 mass = ent->mass_unscaled * det_abs;
-        f32 inertia = ent->inertia_unscaled * det_abs;
-
-        /* Determine force & torque acceleration */
-        struct v2 tick_force_accel = v2_mul(v2_div(ent->force, mass), dt);
-        f32 tick_torque_accel = (ent->torque / inertia) * dt;
-
-        /* Integrate */
-        /* TODO: Don't need to multiply each term by dt separately */
-        struct v2 tick_linear_velocity = v2_add(v2_mul(ent->linear_velocity, dt), v2_mul(tick_force_accel, dt));
-        f32 tick_angular_velocity = (ent->angular_velocity * dt) + (tick_torque_accel * dt);
-
-        xf.og = v2_add(xf.og, tick_linear_velocity);
-        xf = xform_rotated(xf, tick_angular_velocity);
-
-        ent->linear_velocity = v2_div(tick_linear_velocity, dt);
-        ent->angular_velocity = tick_angular_velocity / dt;
-        ent->force = V2(0, 0);
-        ent->torque = 0;
-
-        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;
@@ -883,13 +854,243 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
         ent->force = V2(0, 0);
         ent->torque = 0;
     }
-#endif
 
-#if 1
     /* ========================== *
-     * Collision
+     * Create contact points
      * ========================== */
 
+    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
+            };
+        }
+
+        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
+                };
+            }
+
+
+            /* TODO: Remove this */
+            static struct arena dict_arena = { 0 };
+            static struct fixed_dict dict = { 0 };
+            if (dict.buckets_count == 0) {
+                dict_arena = arena_alloc(GIGABYTE(64));
+                dict = fixed_dict_init(&dict_arena, 4096);
+            }
+
+            struct string contact_key;
+            {
+                u64 contact_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&e0->handle));
+                contact_hash = hash_fnv64(contact_hash, BUFFER_FROM_STRUCT(&e1->handle));
+                contact_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&contact_hash));
+            }
+
+            struct entity *contact = fixed_dict_get(&dict, contact_key);
+
+
+
+
+            const f32 min_new_contact_point_dist_sq = 0.05 * 0.05;
+
+            struct gjk_contact_points_result res = gjk_contact_points(e0_poly, e1_poly);
+            if (res.num_pairs > 0) {
+                if (!contact) {
+                    contact = entity_alloc(root);
+                    contact->contact_e0 = e0->handle;
+                    contact->contact_e1 = e1->handle;
+                    entity_enable_prop(contact, ENTITY_PROP_CONTACT);
+                    activate_now(contact);
+                    fixed_dict_set(&dict_arena, &dict, contact_key, contact);
+                }
+                /* TODO: Dont need to do insertion logic for newly created 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 new_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);
+
+                    i32 replace_index = -1;
+                    for (u32 j = 0; j < contact->num_contact_pairs; ++j) {
+                        struct entity_contact_pair old_pair = contact->contact_pairs[j];
+
+                        struct v2 old_p0_local = old_pair.p0.local;
+                        struct v2 old_p1_local = old_pair.p1.local;
+
+                        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;
+                            break;
+                        }
+                    }
+                    if (should_insert) {
+                        u32 index;
+                        if (replace_index > 0) {
+                            index = replace_index;
+                            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;
+                            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;
+                            }
+                        }
+                        contact->contact_pairs[index] = (struct entity_contact_pair) {
+                            .p0 = {
+                                .local = new_p0_local
+                            },
+                            .p1 = {
+                                .local = new_p1_local
+                            }
+                        };
+                    }
+                }
+                /* TODO: Remove this (debugging) */
+                contact->prototype = res.prototype;
+                contact->simplex = res.simplex;
+            } else if (contact) {
+                /* No longer colliding, delete contact */
+                contact->num_contact_pairs = 0;
+                fixed_dict_set(&dict_arena, &dict, contact_key, NULL);
+            }
+        }
+    }
+
+    /* ========================== *
+     * Collision resolution
+     * ========================== */
+
+    for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) {
+        struct entity *contact = &store->entities[entity_index];
+        if (!(contact->valid && entity_has_prop(contact, ENTITY_PROP_ACTIVE))) continue;
+        if (!entity_has_prop(contact, ENTITY_PROP_CONTACT)) continue;
+
+        struct entity *e0 = entity_from_handle(store, contact->contact_e0);
+        struct entity *e1 = entity_from_handle(store, contact->contact_e1);
+
+        if (e0->valid && e1->valid && contact->num_contact_pairs > 0) {
+            struct xform e0_xf = entity_get_xform(e0);
+            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;
+
+            for (u32 pair_index = 0; pair_index < contact->num_contact_pairs; ++pair_index) {
+                struct entity_contact_pair pair = contact->contact_pairs[pair_index];
+                struct v2 p0 = xform_mul_v2(e0_xf, pair.p0.local);
+                struct v2 p1 = xform_mul_v2(e1_xf, pair.p1.local);
+
+                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);
+
+                struct v2 imp = v2_mul(pen_norm, j);
+
+                if (!e0->test_collided) {
+                    e0->test_collided = true;
+                }
+
+                entity_apply_linear_impulse(e0, imp, p0);
+                entity_apply_linear_impulse(e1, v2_neg(imp), p1);
+            }
+        } else {
+            entity_enable_prop(contact, 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;

src/user.c

@@ -823,7 +823,7 @@ INTERNAL void user_update(void)
         __profscope(user_entity_iter);
         struct entity *ent = &store->entities[entity_index];
         if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;
-        if (sprite_tag_is_nil(ent->sprite)) continue;
+        //if (sprite_tag_is_nil(ent->sprite)) continue;
 
         struct sprite_tag sprite = ent->sprite;
 
@@ -888,7 +888,8 @@ INTERNAL void user_update(void)
         /* Debug draw info */
         if (G.debug_draw && !skip_debug_draw) {
             struct temp_arena temp = arena_temp_begin(scratch.arena);
-            b32 colliding = ent->num_points > 0;
+
+            b32 colliding = false;
 
 #if 0
             struct font *disp_font = font_load_async(STR("res/fonts/fixedsys.ttf"), 12.0f);
@@ -987,13 +988,37 @@ INTERNAL void user_update(void)
                 }
             }
 
+            /* Draw contact */
+            if (entity_has_prop(ent, ENTITY_PROP_CONTACT)) {
+                f32 radius = 5;
+                struct entity *e0 = entity_from_handle(store, ent->contact_e0);
+                struct entity *e1 = entity_from_handle(store, ent->contact_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) {
+                    struct entity_contact_pair pair = ent->contact_pairs[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);
+                        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);
+                        point = xform_mul_v2(G.world_view, point);
+                        draw_solid_circle(G.viewport_canvas, point, radius, color, 10);
+                    }
+                }
+            }
+
+#if 0
             /* Draw collision */
             if (ent->is_top) {
                 struct entity *e1 = entity_from_handle(store, ent->colliding_with);
                 (UNUSED)e1;
                 (UNUSED)colliding;
 
-#if 1
                 /* Create shapes */
                 struct quad ent_quad;
                 struct v2_array ent_poly;
@@ -1111,7 +1136,6 @@ INTERNAL void user_update(void)
                         draw_solid_poly_line(G.viewport_canvas, simplex_array, simplex.len > 2, thickness, line_color);
                     }
                 }
-#endif
 
                 /* Draw points */
                 {
@@ -1128,6 +1152,7 @@ INTERNAL void user_update(void)
                     }
                 }
             }
+#endif
 
             /* Draw hierarchy */
             if (entity_has_prop(parent, ENTITY_PROP_ACTIVE) && !parent->is_root) {