start on glitchy collision resolution

src/draw.c

@@ -246,7 +246,7 @@ void draw_solid_arrow_line(struct renderer_canvas *canvas, struct v2 start, stru
 
     struct v2 head_start = v2_add(end, head_start_dir);
 
-    struct v2 head_p1_dir = v2_mul(v2_perp_cw(head_start_dir), head_width_ratio);
+    struct v2 head_p1_dir = v2_perp_mul(head_start_dir, head_width_ratio);
     struct v2 head_p2_dir = v2_neg(head_p1_dir);
 
     struct v2 head_p1 = v2_add(head_start, head_p1_dir);

src/entity.h

@@ -101,14 +101,8 @@ struct entity {
     struct gjk_simplex simplex;
     struct gjk_prototype prototype;
     struct v2 pendir;
-    struct xform xf0;
-    struct xform xf1;
     b32 solved;
 
-    struct xform predicted_xform;
-
-
-
     b32 test_torque_applied;
 
 

src/game.c

@@ -189,7 +189,7 @@ INTERNAL void spawn_test_entities(void)
 
         entity_enable_prop(e, ENTITY_PROP_PHYSICAL);
         e->mass_unscaled = 70;
-        e->ground_friction = 1000;
+        //e->ground_friction = 1000;
 
         entity_set_xform(e, XFORM_TRS(.t = pos, .s = size, .r = rot));
     }
@@ -749,11 +749,11 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
         if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue;
         if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue;
         if (ent->ground_friction != 0) {
-#if 1
+#if 0
             /* Linear velocity */
             {
                 struct v2 linear_velocity = ent->linear_velocity;
-                if (!v2_eq(linear_velocity, V2(0, 0))) {
+                if (!v2_is_zero(linear_velocity)) {
                     /* FIXME: Incorrect behavior at low FPS & low entity density */
                     const f32 clamp_epsilon = 0.01;
                     f32 linear_velocity_len = v2_len(linear_velocity);
@@ -849,9 +849,9 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
         f32 tick_angular_velocity = (ent->angular_velocity * dt) + (tick_angular_acceleration * dt);
         xf.og = v2_add(xf.og, tick_linear_velocity);
         xf = xform_rotated(xf, tick_angular_velocity);
-        ent->predicted_xform = xf;
         ent->linear_velocity = v2_div(tick_linear_velocity, dt);
         ent->angular_velocity = tick_angular_velocity / dt;
+        entity_set_xform(ent, xf);
     }
 
     /* ========================== *
@@ -868,6 +868,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
         }
     }
 
+#if 1
     /* ========================== *
      * Collision
      * ========================== */
@@ -878,32 +879,28 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
         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 xf0 = entity_get_xform(e0);
+        struct xform e0_xf = entity_get_xform(e0);
-        struct xform xf1 = e0->predicted_xform;
         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(xf1, e0_quad);
+            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 any_collision = false;
         b32 colliding = false;
-        struct v2 point0 = V2(0, 0);
+        struct v2 p0 = V2(0, 0);
-        struct v2 point1 = V2(0, 0);
+        struct v2 p1 = V2(0, 0);
         struct entity *colliding_with = entity_nil();
         struct gjk_simplex simplex = { 0 };
         struct gjk_prototype prototype = { 0 };
         struct v2 velocity = V2(0, 0);
-        struct v2 pen = V2(0, 0);
         b32 solved = false;
-        (UNUSED)pen;
         for (u64 e1_index = 0; e1_index < store->reserved; ++e1_index) {
             struct entity *e1 = &store->entities[e1_index];
             if (e1 == e0) continue;
@@ -925,69 +922,140 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
                 };
             }
 
-            /* Testing */
-            {
-                /* TODO: Continuity gen check */
-
-                velocity = v2_sub(xf1.og, xf0.og);
-                //velocity = V2(0.014992147684, -0.000010356307);
-                //velocity = V2(-0.0001, -0.01);
-                //velocity = V2(0.005, 0);
-                //velocity = V2(-500, -500);
-
-                //velocity = V2(-1, -1);
-                //velocity = v2_neg(velocity);
-
-
-                //velocity = v2_neg(velocity);
-                //xf0.og = v2_sub(xf1.og, velocity);
-                xf1.og = v2_add(xf0.og, velocity);
-            }
-
             struct gjk_extended_result res = gjk_extended(e0_poly, e1_poly);
 
             colliding = res.colliding;
-            point0 = res.p0;
+            p0 = res.p0;
-            point1 = res.p1;
+            p1 = res.p1;
             colliding_with = e1;
             simplex = res.simplex;
             prototype = res.prototype;
             solved = res.solved;
-
-            if (colliding) {
-                any_collision = true;
-                pen = v2_sub(point1, point0);
-            }
         }
 
         e0->colliding = colliding;
         e0->colliding_with = colliding_with->handle;
-        e0->point = point0;
+        e0->point = p0;
         e0->simplex = simplex;
         e0->prototype = prototype;
-        e0->xf0 = xf0;
-        e0->xf1 = xf1;
         e0->pendir = velocity;
         e0->solved = solved;
 
         if (colliding_with->valid) {
             colliding_with->colliding = colliding;
             colliding_with->colliding_with = e0->handle;
-            colliding_with->point = point1;
+            colliding_with->point = p1;
         }
 
-        (UNUSED)any_collision;
         {
 #if 0
-            if (any_collision) {
+            if (colliding) {
                 xf1.og = v2_add(xf1.og, pen);
                 //e0->verlet_xform.og = xf0.og;
-                e0->verlet_xform.og = v2_sub(xf1.og, velocity);
+                //e0->verlet_xform.og = v2_sub(xf1.og, velocity);
+            }
+#else
+            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 0
+                f32 bias_factor = 0.3;
+                f32 bias_slop = 0.05;
+                struct v2 bias = v2_sub(v2_mul(pen, bias_factor), v2_mul(pen_norm, bias_slop));
+                if (v2_dot(pen, bias) < 0) {
+                    bias = V2(0, 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);
+
+
+
+
+
+
+                //f32 ma = 1 / (vcp0 / v2_len(vcp0)
+
+
+
+
+                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 / k;
+                //j = max_f32(j, 0);
+
+
+
+                if (j > 0) {
+                    DEBUGBREAKABLE;
+                }
+
+
+
+
+
+                struct v2 imp = v2_mul(pen_norm, j);
+                (UNUSED)imp;
+
+
+
+
+
+
+
+#if 1
+                {
+                    struct entity *impulse = entity_alloc(e0);
+                    entity_enable_prop(impulse, ENTITY_PROP_LINEAR_IMPULSE);
+                    impulse->force = v2_mul(imp, m0);
+                    activate_now(impulse);
+                }
+                {
+                    struct entity *impulse = entity_alloc(e1);
+                    entity_enable_prop(impulse, ENTITY_PROP_LINEAR_IMPULSE);
+                    impulse->force = v2_mul(v2_neg(imp), m1);
+                    activate_now(impulse);
+                }
+#endif
+
             }
 #endif
-            entity_set_xform(e0, xf1);
         }
     }
+#endif
 
     /* ========================== *
      * Initialize bullet kinematics from sources
@@ -1036,7 +1104,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds)
             {
                 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_eq(camera_size, V2(0, 0))) {
+                if (!v2_is_zero(camera_size)) {
                     aspect_ratio = camera_size.x / camera_size.y;
                 }
             }

src/gjk.c

@@ -176,7 +176,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array
             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) {
-                case 0: {  /* No region, must be in simplex */
+                default: {  /* No region, must be in simplex */
                     colliding = true;
                 } break;
                 case 1: {  /* Region ab, remove c */

src/math.h

@@ -645,23 +645,23 @@ INLINE f32 v2_wedge(struct v2 a, struct v2 b)
     return a.x * b.y - a.y * b.x;
 }
 
-/* Clockwise perpendicular vector */
+/* Right (clockwise) perpendicular vector */
-INLINE struct v2 v2_perp_cw(struct v2 a)
+INLINE struct v2 v2_perp(struct v2 a)
 {
     return V2(-a.y, a.x);
 }
 
-/* Counterclockwise perpendicular vector */
+/* Perpendicular vector scaled by s */
-INLINE struct v2 v2_perp_ccw(struct v2 a)
+INLINE struct v2 v2_perp_mul(struct v2 a, f32 s)
 {
-    return V2(a.y, -a.x);
+    return V2(s * -a.y, s * a.x);
 }
 
 INLINE struct v2 v2_perp_towards_dir(struct v2 v, struct v2 dir)
 {
-    struct v2 perp = v2_perp_cw(v);
+    struct v2 perp = v2_perp(v);
-    i32 sign = 1 - ((v2_dot(perp, dir) < 0) << 1);
+    f32 dot = v2_dot(perp, dir);
-    return v2_mul(perp, sign);
+    return v2_mul(perp, (dot >= 0) - (dot < 0));
 }
 
 INLINE struct v2 v2_norm(struct v2 a)
@@ -677,26 +677,17 @@ INLINE struct v2 v2_norm(struct v2 a)
 
 INLINE struct v2 v2_round(struct v2 a)
 {
-    return V2(
+    return V2(math_round(a.x), math_round(a.y));
-        math_round(a.x),
-        math_round(a.y)
-    );
 }
 
 INLINE struct v2 v2_floor(struct v2 a)
 {
-    return V2(
+    return V2(math_floor(a.x), math_floor(a.y));
-        math_floor(a.x),
-        math_floor(a.y)
-    );
 }
 
 INLINE struct v2 v2_ceil(struct v2 a)
 {
-    return V2(
+    return V2(math_ceil(a.x), math_ceil(a.y));
-        math_ceil(a.x),
-        math_ceil(a.y)
-    );
 }
 
 INLINE f32 v2_distance(struct v2 a, struct v2 b)
@@ -711,12 +702,14 @@ INLINE b32 v2_eq(struct v2 a, struct v2 b)
     return a.x == b.x && a.y == b.y;
 }
 
+INLINE b32 v2_is_zero(struct v2 a)
+{
+    return a.x == 0 && a.y == 0;
+}
+
 INLINE struct v2 v2_lerp(struct v2 val0, struct v2 val1, f32 t)
 {
-    struct v2 res;
+    return V2(math_lerp(val0.x, val1.x, t), math_lerp(val0.y, val1.y, t));
-    res.x = math_lerp(val0.x, val1.x, t);
-    res.y = math_lerp(val0.y, val1.y, t);
-    return res;
 }
 
 INLINE struct v2 v2_rotated(struct v2 v, f32 a)
@@ -1093,7 +1086,7 @@ INLINE struct quad quad_from_line(struct v2 start, struct v2 end, f32 thickness)
     f32 width = thickness / 2.f;
 
     struct v2 dir = v2_norm(v2_sub(end, start));
-    struct v2 dir_perp = v2_perp_cw(dir);
+    struct v2 dir_perp = v2_perp(dir);
 
     struct v2 left = v2_mul(dir_perp, -width);
     struct v2 right = v2_mul(dir_perp, width);

src/user.c

@@ -669,7 +669,7 @@ INTERNAL void user_update(void)
         {
             struct xform quad_xf = xform_mul(entity_get_xform(active_camera), active_camera->camera_quad_xform);
             struct v2 camera_size = xform_get_scale(quad_xf);
-            if (!v2_eq(camera_size, V2(0, 0))) {
+            if (!v2_is_zero(camera_size)) {
                 aspect_ratio = camera_size.x / camera_size.y;
             }
         }
@@ -735,7 +735,7 @@ INTERNAL void user_update(void)
         {
             struct xform quad_xf = xform_mul(xf, active_camera->camera_quad_xform);
             struct v2 camera_size = xform_get_scale(quad_xf);
-            if (!v2_eq(camera_size, V2(0, 0))) {
+            if (!v2_is_zero(camera_size)) {
                 size = v2_div_v2(size, camera_size);
             }
         }
@@ -1331,12 +1331,8 @@ INTERNAL void user_update(void)
             draw_text(G.viewport_canvas, font, pos, string_format(temp.arena, STR("player angular velocity: %F"), FMT_FLOAT_P((f64)player_angular_vel, 12)));
             pos.y += spacing;
 
-            struct v2 xf0_pos = entity_find_first_match_one(store, ENTITY_PROP_PLAYER_CONTROLLED)->xf0.og;
+            struct v2 plaeyr_pos = entity_get_xform(entity_find_first_match_one(store, ENTITY_PROP_PLAYER_CONTROLLED)).og;
-            draw_text(G.viewport_canvas, font, pos, string_format(temp.arena, STR("player collision xf0 pos: (%F, %F)"), FMT_FLOAT_P((f64)xf0_pos.x, 12), FMT_FLOAT_P((f64)xf0_pos.y, 12)));
+            draw_text(G.viewport_canvas, font, pos, string_format(temp.arena, STR("player pos: (%F, %F)"), FMT_FLOAT_P((f64)plaeyr_pos.x, 12), FMT_FLOAT_P((f64)plaeyr_pos.y, 12)));
-            pos.y += spacing;
-
-            struct v2 xf1_pos = entity_find_first_match_one(store, ENTITY_PROP_PLAYER_CONTROLLED)->xf1.og;
-            draw_text(G.viewport_canvas, font, pos, string_format(temp.arena, STR("player collision xf1 pos: (%F, %F)"), FMT_FLOAT_P((f64)xf1_pos.x, 12), FMT_FLOAT_P((f64)xf1_pos.y, 12)));
             pos.y += spacing;
 
 #if RTC