From 14163d2a09bf548a24d70277fb2abe6ab2ebceb6 Mon Sep 17 00:00:00 2001 From: jacob Date: Fri, 6 Sep 2024 16:00:44 -0500 Subject: [PATCH] semi working sweeping collision along velocity --- src/entity.h | 7 + src/game.c | 219 +++++---- src/gjk.c | 1236 +++++++++++++++++++++++++++++++------------------- src/gjk.h | 35 +- src/user.c | 136 ++++-- 5 files changed, 1054 insertions(+), 579 deletions(-) diff --git a/src/entity.h b/src/entity.h index 9e337a7b..580f5f1b 100644 --- a/src/entity.h +++ b/src/entity.h @@ -97,8 +97,15 @@ struct entity { struct entity_handle colliding_with; struct v2 point; struct gjk_simplex simplex; + struct gjk_prototype prototype; struct v2 pendir; b32 velocity_intersects; + struct xform xf0; + struct xform xf1; + b32 solved; + + + struct xform predicted_xform; diff --git a/src/game.c b/src/game.c index 5109546b..4927cc86 100644 --- a/src/game.c +++ b/src/game.c @@ -124,7 +124,8 @@ INTERNAL void spawn_test_entities(void) { //struct v2 pos = V2(-1, -1); //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(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); f32 r = PI / 4; //f32 r = 0; @@ -558,7 +559,8 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) bullet->bullet_src = ent->handle; bullet->bullet_src_pos = rel_pos; bullet->bullet_src_dir = rel_dir; - bullet->bullet_impulse = 0.25; + //bullet->bullet_impulse = 0.25; + bullet->bullet_impulse = 100; bullet->mass_unscaled = 0.04; entity_enable_prop(bullet, ENTITY_PROP_BULLET); @@ -649,8 +651,9 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) struct v2 tick_velocity = v2_sub(xf.og, ent->verlet_xform.og); tick_velocity = v2_add(tick_velocity, v2_mul(acceleration, dt)); ent->verlet_xform = xf; + xf.og = v2_add(xf.og, tick_velocity); - entity_set_xform(ent, xf); + ent->predicted_xform = xf; } /* ========================== * @@ -671,78 +674,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * ========================== */ #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)) 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_extended_simplex simplex = { 0 }; - struct v2 pen = V2(0, 0); - struct v2 spot = V2(0, 0); - struct entity_handle colliding_with = { 0 }; - 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; - - 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 - }; - } - - colliding = gjk_boolean(e0_poly, e1_poly, G.gjk_steps); - colliding_with = e1->handle; - //simplex = res.final_simplex; - - if (colliding) { - //pen = epa_colliding_pen(e0_poly, e1_poly, simplex); -#if 0 - /* Pen movement test */ - { - struct xform xf = e1_xf; - xf.og = v2_add(xf.og, pen); - entity_set_xform(e1, xf); - //e1->verlet_xform.og = v2_add(e1->verlet_xform.og, pen); - } -#endif - } else { - spot = e0->spot; - } - } - - e0->colliding = colliding; - e0->colliding_with = colliding_with; - e0->simplex = simplex; - e0->pen = pen; - e0->spot = spot; - } -#else 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; @@ -790,15 +721,15 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) }; } -#if 0 +#if 1 struct v2 e0_velocity = v2_sub(e0_xf.og, e0->verlet_xform.og); struct v2 e1_velocity = v2_sub(e1_xf.og, e1->verlet_xform.og); - pendir = v2_sub(e0_velocity, e1_velocity); /* TODO: What if both point in opposite directions? */ + pendir = v2_sub(e0_velocity, e1_velocity); //pendir = v2_norm(pendir); #else - //pendir = V2(0, 0); + pendir = V2(0, 0); //pendir = V2(-0.25, -1); - pendir = v2_mul(v2_norm(V2(-0.25, -1)), 0.2); + //pendir = v2_mul(v2_norm(V2(-0.25, -1)), 0.2); //pendir = V2(0, 99999); //pendir = V2(-99999999999, -99999999999); //pendir = V2(-0.0000794729349, -1.00000); @@ -845,6 +776,136 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) colliding_with->point = point1; } + } +#else + 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 xf0 = 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(xf0, 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 point1 = 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; + b32 velocity_intersects = 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; + 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 + }; + } + + /* Testing */ + { + /* TODO: Continuity gen check */ + + velocity = v2_sub(xf1.og, xf0.og); + //velocity = V2(0.014992147684, -0.000010356307); + //velocity = V2(0.005, 0); + //velocity = V2(0.005, 0.005); + + //velocity = V2(-1, -1); + //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, xf0, xf1); + + colliding = res.colliding; + point0 = res.p0; + point1 = res.p1; + colliding_with = e1; + simplex = res.simplex; + prototype = res.prototype; + velocity_intersects = res.velocity_intersects; + solved = res.solved; + + if (colliding) { +#if 1 + any_collision = true; + pen = v2_sub(point1, point0); + /* Pen movement test */ #if 0 + f32 epsilon = 0.000100; pen = v2_add(pen, v2_mul(v2_norm(pen), epsilon)); +#endif + //xf0.og = v2_add(xf0.og, pen); + //e0->verlet_xform.og = v2_add(e0->verlet_xform.og, pen); + //e0->verlet_xform.og = v2_add(e0->verlet_xform.og, pen); +#endif + } + } + + e0->colliding = colliding; + e0->colliding_with = colliding_with->handle; + e0->point = point0; + e0->simplex = simplex; + e0->prototype = prototype; + e0->xf0 = xf0; + e0->xf1 = xf1; + e0->pendir = velocity; + e0->velocity_intersects = velocity_intersects; + e0->solved = solved; + + if (colliding_with->valid) { + colliding_with->colliding = colliding; + colliding_with->colliding_with = e0->handle; + colliding_with->point = point1; + } + + (UNUSED)any_collision; +#if 1 + { + //if (!v2_eq(pen, V2(0, 0))) { + if (any_collision) { + xf1.og = v2_add(xf0.og, pen); + //e0->verlet_xform.og = xf0.og; + e0->verlet_xform.og = v2_sub(xf1.og, velocity); + } + entity_set_xform(e0, xf1); + } +#else + { + entity_set_xform(e0, e0->predicted_xform); + } +#endif } #endif diff --git a/src/gjk.c b/src/gjk.c index b671b4b0..532abc71 100644 --- a/src/gjk.c +++ b/src/gjk.c @@ -30,15 +30,6 @@ INTERNAL struct v2 menkowski_point(struct v2_array poly0, struct v2_array shape1 return v2_sub(poly_support(poly0, dir), poly_support(shape1, v2_neg(dir))); } -INTERNAL struct gjk_menkowski_point menkowski_point_extended(struct v2_array poly0, struct v2_array poly1, struct v2 dir) -{ - struct gjk_menkowski_point res; - res.p0 = poly_support(poly0, dir); - res.p1 = poly_support(poly1, v2_neg(dir)); - res.p = v2_sub(res.p0, res.p1); - return res; -} - b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1) { struct { struct v2 a, b, c; } s = { 0 }; @@ -87,14 +78,25 @@ b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1) } #if 0 -struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 penetration_dir) + +INTERNAL struct gjk_menkowski_point menkowski_point_extended(struct v2_array poly0, struct v2_array poly1, struct v2 dir) { - struct temp_arena scratch = scratch_begin_no_conflict(); + struct gjk_menkowski_point res; + res.p0 = poly_support(poly0, dir); + res.p1 = poly_support(poly1, v2_neg(dir)); + res.p = v2_sub(res.p0, res.p1); + return res; +} + +struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct xform shape0_xf0, struct xform shape0_xf1) +{ + (UNUSED)shape0_xf0; + (UNUSED)shape0_xf1; + struct gjk_extended_result res = { 0 }; /* TODO: Verify epsilon */ - f32 unique_epsilon = 0.00001; - b32 use_penetration_dir = false; + f32 epsilon = 0.0000100; struct gjk_simplex s = { 0 }; b32 colliding = false; struct v2 shape0_p = { 0 }; @@ -106,285 +108,90 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array /* ========================== * * GJK collision check + * Construct encapsulating simplex OR closest feature if not colliding * ========================== */ struct v2 dir = { 0 }; struct gjk_menkowski_point m = { 0 }; + /* Determine encapsulating simplex if colliding, or closest edge / point to origin on simplex */ { - DBGSTEP; /* First point is support point in shape's general directions to eachother */ s.a = menkowski_point_extended(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0])); s.len = 1; - DBGSTEP; - /* Second point is support point towards origin */ - dir = v2_neg(s.a.p); - m = menkowski_point_extended(shape0, shape1, dir); - if (v2_dot(dir, m.p) >= 0) { - s.b = s.a; - s.a = m; - s.len = 2; + while (!colliding) { + if (s.len == 1) { + /* Second point is support point towards origin */ + dir = v2_neg(s.a.p); - while (true) { DBGSTEP; - /* Third point is support point in direction of line normal towards origin */ - dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p)); m = menkowski_point_extended(shape0, shape1, dir); - f32 dot = v2_dot(dir, m.p); - if (dot <= 0) { - /* New point did not cross origin, collision impossible */ + if (v2_eq(m.p, s.a.p)) { + /* Point is the same */ + break; + } + s.b = s.a; + s.a = m; + s.len = 2; + if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { + /* New ab lies on origin */ break; } - s.c = s.b; - s.b = s.a; - s.a = m; - s.len = 3; - - DBGSTEP; - dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(v2_sub(s.c.p, s.a.p))); /* Normal of ab pointing away from c */ - struct v2 a_to_origin = v2_neg(s.a.p); - - dot = v2_dot(dir, a_to_origin); - if (dot >= 0) { - /* Point is in region ab, remove c from simplex */ - s.len = 2; - } else { - /* Point is not in region ab */ - dir = v2_perp_towards_dir(v2_sub(s.c.p, s.a.p), v2_neg(v2_sub(s.b.p, s.a.p))); /* Normal of ac pointing away from b */ - dot = v2_dot(dir, a_to_origin); - if (dot >= 0) { - /* Point is in region ac, remove b from simplex */ - s.b = s.c; - s.len = 2; - } else { - colliding = true; - break; - } - } + /* Third point is support point in direction of line normal towards origin */ + dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p)); } - } - } - - if (colliding) { - use_penetration_dir = !v2_eq(penetration_dir, V2(0, 0)); - if (use_penetration_dir) { - - /* ========================== * - * Move simplex towards penetration dir - * ========================== */ - - while (true) { - /* Second point is support point towards penetration_dir */ - if (s.len == 1) { - DBGSTEP; - dir = v2_sub(v2_mul(penetration_dir, v2_dot(penetration_dir, s.a.p) / v2_dot(penetration_dir, penetration_dir)), s.a.p); - m = menkowski_point_extended(shape0, shape1, dir); - if (v2_eq(m.p, s.a.p)) { - break; - } - s.b = s.a; - s.a = m; - s.len = 2; - /* Third point is support point in direction of line normal towards `a` projected onto penetration_dir */ - dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), penetration_dir); - } - - if (s.len == 2) { - DBGSTEP; - m = menkowski_point_extended(shape0, shape1, dir); - if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < unique_epsilon) { - /* New point is already on the current line */ - break; - } - s.c = s.b; - s.b = s.a; - s.a = m; - s.len = 3; - } - - DBGSTEP; - i32 a_wedgesign = math_fsign(v2_wedge(penetration_dir, s.a.p)); - i32 b_wedgesign = math_fsign(v2_wedge(penetration_dir, s.b.p)); - i32 c_wedgesign = math_fsign(v2_wedge(penetration_dir, s.c.p)); - if (a_wedgesign != b_wedgesign) { - /* Remove c */ - dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), penetration_dir); - s.len = 2; - } else if (b_wedgesign != c_wedgesign) { - /* Remove b */ - dir = v2_perp_towards_dir(v2_sub(s.c.p, s.a.p), penetration_dir); - s.b = s.c; - s.len = 2; - } else { - /* Remove a */ - dir = v2_perp_towards_dir(v2_sub(s.c.p, s.b.p), penetration_dir); - s.a = s.b; - s.b = s.c; - s.len = 2; - } - } - } else { - - /* ========================== * - * Expand simplex towards closest edge to origin inside menkowski (EPA) - * ========================== */ - - struct gjk_menkowski_point *proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point); - u32 proto_count = 0; { - ASSERT(s.len == 3); - struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3); - tmp[0] = s.a; - tmp[1] = s.b; - tmp[2] = s.c; - proto_count = 3; - } - - const f32 pen_epsilon = 0.00001; - while (true) { - DBGSTEP; - struct v2 pen = V2(0, 0); - f32 pen_len_sq = F32_INFINITY; - - /* Find dir from origin to closest edge */ - /* FIXME: Winding order of ps & pe index */ - u32 pen_ps_index = 0; - u32 pen_pe_index = 0; - for (u32 i = 0; i < proto_count; ++i) { - u32 ps_index = i; - u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0; - struct v2 ps = proto[ps_index].p; - struct v2 pe = proto[pe_index].p; - - struct v2 vse = v2_sub(pe, ps); - struct v2 vso = v2_neg(ps); - - f32 d1 = v2_dot(vso, vse); - f32 d2 = v2_dot(vse, vse); - struct v2 vsd = v2_mul(vse, (d1 / d2)); - struct v2 pd = v2_add(ps, vsd); - - f32 pd_len_sq = v2_len_sq(pd); - if (pd_len_sq < pen_len_sq) { - pen_ps_index = ps_index; - pen_pe_index = pe_index; - pen_len_sq = pd_len_sq; - -#if 0 - pen = pd; -#else - if (pd_len_sq > pen_epsilon) { - pen = pd; - } else { - u32 p_next_index = pe_index < proto_count - 1 ? pe_index + 1 : 0; - struct v2 p_next = proto[p_next_index].p; - pen = v2_neg(v2_perp_towards_dir(vse, v2_sub(p_next, ps))); - } -#endif - } - } - - /* TODO: Move to break (debugging) */ - s.a = proto[pen_ps_index]; - s.b = proto[pen_pe_index]; - s.len = 2; - - /* Find new point in dir */ - m = menkowski_point_extended(shape0, shape1, pen); - - /* Check unique */ - /* TODO: Better */ - { - b32 unique = true; - for (u32 i = 0; i < proto_count; ++i) { - struct v2 edge_start = proto[i].p; - struct v2 edge_end = i < proto_count - 1 ? proto[i + 1].p : proto[0].p; - if (math_fabs(v2_wedge(v2_sub(edge_end, edge_start), v2_sub(m.p, edge_start))) < unique_epsilon) { - unique = false; - break; - } - } - if (!unique) { - break; - } - } - - /* Insert point into prototype */ - /* FIXME: Preserve winding order */ - arena_push(scratch.arena, struct v2); - ++proto_count; - for (u32 i = proto_count - 1; i > pen_pe_index; --i) { - u32 shift_from = (i > 0) ? i - 1 : proto_count - 1; - u32 shift_to = i; - proto[shift_to] = proto[shift_from]; - } - proto[pen_pe_index] = m; - } - } - } else { - - /* ========================== * - * Move simplex towards closest edge to origin outside menkowski - * ========================== */ - - if (s.len == 2) { - /* Third point is support point in direction of line normal towards `a` projected onto penetration_dir */ - dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), penetration_dir); - } - - while (true) { - /* Second point is support point towards origin */ - if (s.len == 1) { - DBGSTEP; - m = menkowski_point_extended(shape0, shape1, v2_neg(s.a.p)); - if (v2_eq(m.p, s.a.p) || v2_eq(m.p, V2(0, 0))) { - break; - } - - s.b = s.a; - s.a = m; - s.len = 2; - - /* Third point is support point in direction of line normal towards origin */ - dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p)); - } - - if (s.len == 2) { DBGSTEP; m = menkowski_point_extended(shape0, shape1, dir); - if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < unique_epsilon || v2_eq(m.p, V2(0, 0))) { - /* New point is already on the current line */ + if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < epsilon) { + /* New point is on existing line ab */ break; } s.c = s.b; s.b = s.a; s.a = m; s.len = 3; + if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { + /* New ab lies on origin */ + s.len = 2; + break; + } else if (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) < epsilon) { + /* New ac lies on origin */ + s.b = s.c; + s.len = 2; + break; + } } - DBGSTEP; - /* Remove point or edge and determine next direction based on voronoi regions */ - struct v2 rab_dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(v2_sub(s.c.p, s.a.p))); - struct v2 rac_dir = v2_perp_towards_dir(v2_sub(s.c.p, s.a.p), v2_neg(v2_sub(s.b.p, s.a.p))); - struct v2 rbc_dir = v2_perp_towards_dir(v2_sub(s.c.p, s.b.p), v2_neg(v2_sub(s.a.p, s.b.p))); + /* Determine voronoi region of the simplex in which the origin lies */ i32 voronoi_mask = 0; - 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(rbc_dir, v2_neg(s.b.p)) >= 0) << 2); /* Regions bc, b, and c */ + struct v2 vab = v2_sub(s.b.p, s.a.p); + struct v2 vac = v2_sub(s.c.p, s.a.p); + struct v2 vbc = v2_sub(s.c.p, s.b.p); + 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(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 */ + /* Remove point or edge and determine next direction based on voronoi region */ switch (voronoi_mask) { + case 0: { /* No region, must be in simplex */ + colliding = true; + } break; case 1: { /* Region ab, remove c */ - dir = rab_dir; + dir = rab_dir; /* Next third point is in direction of region ab */ s.len = 2; } break; case 2: { /* Region ac, remove b */ - dir = rac_dir; + dir = rac_dir; /* Next third point is in direction of region ac */ s.b = s.c; s.len = 2; } break; case 4: { /* Region bc, remove a */ - dir = rbc_dir; + dir = rbc_dir; /* Next third point is in direction of region bc */ s.a = s.b; s.b = s.c; s.len = 2; @@ -405,19 +212,14 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array } /* Resolve points */ - DBGSTEP; if (s.len == 1) { shape0_p = s.a.p0; shape1_p = s.a.p1; } else if (s.len == 2) { /* FIXME: Winding order dependent? */ + ASSERT(s.len == 2); f32 ratio; - if (use_penetration_dir) { - /* Determine ratio between edge a & b that penetration dir intersection lies */ - f32 wedgea = math_fabs(v2_wedge(penetration_dir, s.a.p)); - f32 wedgeb = math_fabs(v2_wedge(penetration_dir, s.b.p)); - ratio = wedgea / (wedgea + wedgeb); - } else { + { /* Determine ratio between edge a & b that projected origin lies */ struct v2 vab = v2_sub(s.b.p, s.a.p); struct v2 vao = v2_neg(s.a.p); @@ -438,25 +240,180 @@ abort: res.p0 = shape0_p; res.p1 = shape1_p; res.simplex = s; - scratch_end(scratch); + return res; +} + +/* TODO: Remove this (debugging) */ +struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1) +{ + struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; + u64 rays = 500; + for (u64 i = 0; i < rays; ++i) { + f32 angle = ((f32)i / rays) * (2 * PI); + struct v2 dir = v2_from_angle(angle); + struct v2 p = menkowski_point_extended(poly0, poly1, dir).p; + if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) { + *arena_push(arena, struct v2) = p; + ++res.count; + } + } return res; } #else -struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct v2 penetration_dir) + + +struct poly_support_test_result { + struct v2 p; + struct v2 original; +}; + +#if 1 + +INTERNAL struct poly_support_test_result poly_support_test(struct v2_array a, struct v2 dir, struct xform xf0, struct xform xf1) { + (UNUSED)xf0; + (UNUSED)xf1; + + /* TODO: Could probably binary search for largest dot since shape is convex */ + struct v2 furthest = V2(0, 0); + struct v2 furthest_original = V2(0, 0); + f32 furthest_dot = -F32_INFINITY; + + for (u32 i = 0; i < a.count; ++i) { + struct v2 p = a.points[i]; + f32 dot = v2_dot(dir, p); + if (dot > furthest_dot) { + furthest = p; + furthest_original = p; + furthest_dot = dot; + } + } + + struct v2 velocity = v2_sub(xf1.og, xf0.og); + //struct v2 velocity = V2(1, 1); + for (u32 i = 0; i < a.count; ++i) { + struct v2 p = a.points[i]; + struct v2 modified; +#if 0 + if (v2_dot(velocity, dir) > 0) { + modified = v2_add(p, velocity); + } else { + modified = p; + } +#else + modified = v2_add(p, velocity); +#endif + f32 dot = v2_dot(dir, modified); + if (dot > furthest_dot) { + furthest = modified; + furthest_original = p; + furthest_dot = dot; + } + } + + struct poly_support_test_result res = { 0 }; + res.p = furthest; + res.original = furthest_original; + return res; +} +#else + +INTERNAL struct poly_support_test_result poly_support_test(struct v2_array a, struct v2 dir, struct xform xf0, struct xform xf1) +{ + f32 epsilon = 0.001; + (UNUSED)xf0; + (UNUSED)xf1; + + /* TODO: Could probably binary search for largest dot since shape is convex */ + struct v2 furthest = V2(0, 0); + struct v2 furthest_original = V2(0, 0); + f32 furthest_wedge = -F32_INFINITY; + f32 furthest_dot = -F32_INFINITY; + + for (u32 i = 0; i < a.count; ++i) { + struct v2 p = a.points[i]; + f32 dot = v2_dot(dir, p); + f32 wedge = v2_wedge(dir, p); + if (dot > furthest_dot + epsilon || (dot > furthest_dot - epsilon && wedge > furthest_wedge)) { + furthest = p; + furthest_original = p; + furthest_dot = dot; + furthest_wedge = wedge; + } + } + + struct v2 velocity = v2_sub(xf1.og, xf0.og); + for (u32 i = 0; i < a.count; ++i) { + struct v2 p = a.points[i]; + struct v2 modified; +#if 0 + if (v2_dot(velocity, dir) > 0) { + modified = v2_add(p, velocity); + } else { + modified = p; + } +#else + modified = v2_add(p, velocity); +#endif + f32 dot = v2_dot(dir, modified); + f32 wedge = v2_wedge(dir, modified); + if (dot > furthest_dot + epsilon || (dot > furthest_dot - epsilon && wedge > furthest_wedge)) { + furthest = modified; + furthest_original = p; + furthest_dot = dot; + furthest_wedge = wedge; + } + } + + struct poly_support_test_result res = { 0 }; + res.p = furthest; + res.original = furthest_original; + return res; +} + +#endif + +INTERNAL struct gjk_menkowski_point menkowski_point_extended_test(struct v2_array poly0, struct v2_array poly1, struct v2 dir, struct xform xf0, struct xform xf1) +{ + struct gjk_menkowski_point res; + struct poly_support_test_result res0 = poly_support_test(poly0, dir, xf0, xf1); + struct poly_support_test_result res1 = poly_support_test(poly1, v2_neg(dir), XFORM_IDENT, XFORM_IDENT); + res.p0 = res0.original; + res.p1 = res1.original; + res.p = v2_sub(res0.p, res1.p); + return res; +} + + +struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct xform xf0, struct xform xf1) +{ + (UNUSED)xf0; + (UNUSED)xf1; + struct v2 velocity = v2_sub(xf1.og, xf0.og); struct temp_arena scratch = scratch_begin_no_conflict(); + struct gjk_extended_result res = { 0 }; /* TODO: Verify epsilon */ - f32 epsilon = 0.0000100; - b32 use_penetration_dir = false; - struct gjk_simplex s = { 0 }; + f32 epsilon = 0.00001; + struct gjk_simplex s = { + .len = 3, + .a = V2(F32_NAN, F32_NAN), + .b = V2(F32_NAN, F32_NAN), + .c = V2(F32_NAN, F32_NAN) + }; b32 colliding = false; struct v2 shape0_p = { 0 }; struct v2 shape1_p = { 0 }; b32 velocity_intersects = false; + b32 use_penetration_dir = false; + (UNUSED)use_penetration_dir; + + /* TODO: Move this back down */ + struct gjk_menkowski_point *proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point); + u32 proto_count = 0; #if GJK_DEBUG u32 dbg_step = 0; @@ -472,27 +429,29 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array /* Determine encapsulating simplex if colliding, or closest edge / point to origin on simplex */ { /* First point is support point in shape's general directions to eachother */ - s.a = menkowski_point_extended(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0])); + s.a = menkowski_point_extended_test(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0]), xf0, xf1); s.len = 1; while (!colliding) { - /* Second point is support point towards origin */ if (s.len == 1) { + /* Second point is support point towards origin */ + dir = v2_neg(s.a.p); + DBGSTEP; - m = menkowski_point_extended(shape0, shape1, v2_neg(s.a.p)); + m = menkowski_point_extended_test(shape0, shape1, dir, xf0, xf1); if (v2_eq(m.p, s.a.p)) { /* Point is the same */ break; } - s.b = s.a; s.a = m; s.len = 2; - +#if 1 if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { - /* ab lies on origin */ + /* New ab lies on origin */ break; } +#endif /* Third point is support point in direction of line normal towards origin */ dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p)); @@ -500,28 +459,35 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array { DBGSTEP; - m = menkowski_point_extended(shape0, shape1, dir); + m = menkowski_point_extended_test(shape0, shape1, dir, xf0, xf1); if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < epsilon) { /* New point is on existing line ab */ break; } - + if (v2_eq(m.p, s.a.p) || v2_eq(m.p, s.b.p) || v2_eq(m.p, s.c.p)) { + /* New point is existing c */ + s.b = s.a; + s.a = m; + s.len = 2; + break; + } s.c = s.b; s.b = s.a; s.a = m; s.len = 3; - - DBGSTEP; +#if 1 if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { - /* ab lies on origin */ + /* New ab lies on origin */ s.len = 2; break; - } else if (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) < epsilon) { - /* ac lies on origin */ + } + if (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) < epsilon) { + /* New ac lies on origin */ s.b = s.c; s.len = 2; break; } +#endif } DBGSTEP; @@ -546,12 +512,12 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array s.len = 2; } break; case 2: { /* Region ac, remove b */ - dir = rac_dir; /* Next third point is in direction of region ab */ + dir = rac_dir; /* Next third point is in direction of region ac */ s.b = s.c; s.len = 2; } break; case 4: { /* Region bc, remove a */ - dir = rbc_dir; /* Next third point is in direction of region ab */ + dir = rbc_dir; /* Next third point is in direction of region bc */ s.a = s.b; s.b = s.c; s.len = 2; @@ -571,162 +537,10 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array } } +#if 0 if (colliding) { - use_penetration_dir = !v2_eq(penetration_dir, V2(0, 0)); - if (use_penetration_dir) { - - /* ========================== * - * Move simplex towards penetration dir - * ========================== */ - - while (true) { - /* Second point is support point towards penetration_dir */ - if (s.len == 1) { - DBGSTEP; - dir = v2_sub(v2_mul(penetration_dir, v2_dot(penetration_dir, s.a.p) / v2_dot(penetration_dir, penetration_dir)), s.a.p); - m = menkowski_point_extended(shape0, shape1, dir); - if (v2_eq(m.p, s.a.p)) { - break; - } - - if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { - /* ab lies on origin */ - break; - } - - s.b = s.a; - s.a = m; - s.len = 2; - /* Third point is support point in direction of line normal towards `a` projected onto penetration_dir */ - dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), penetration_dir); - } - - if (s.len == 2) { - DBGSTEP; - m = menkowski_point_extended(shape0, shape1, dir); - if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < epsilon) { - /* New point is already on the current line */ - break; - } - s.c = s.b; - s.b = s.a; - s.a = m; - s.len = 3; - - DBGSTEP; - if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { - /* ab lies on origin */ - s.len = 2; - break; - } else if (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) < epsilon) { - /* ac lies on origin */ - s.b = s.c; - s.len = 2; - break; - } - } - - DBGSTEP; -#if 0 - i32 a_wedgesign = math_fsign(v2_wedge(penetration_dir, s.a.p)); - i32 b_wedgesign = math_fsign(v2_wedge(penetration_dir, s.b.p)); - i32 c_wedgesign = math_fsign(v2_wedge(penetration_dir, s.c.p)); - if (a_wedgesign != b_wedgesign) { - /* Remove c */ - dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), penetration_dir); - s.len = 2; - } else if (a_wedgesign != c_wedgesign) { - /* Remove b */ - dir = v2_perp_towards_dir(v2_sub(s.c.p, s.a.p), penetration_dir); - s.b = s.c; - s.len = 2; - } else { - /* Remove a */ - dir = v2_perp_towards_dir(v2_sub(s.c.p, s.b.p), penetration_dir); - s.a = s.b; - s.b = s.c; - s.len = 2; - } -#else - - //struct v2 vab = v2_sub(s.b.p, s.a.p); - - - f32 a_dot = v2_dot(penetration_dir, s.a.p); - f32 a_wedge = v2_wedge(penetration_dir, s.a.p); - - f32 b_dot = v2_dot(penetration_dir, s.b.p); - f32 b_wedge = v2_wedge(penetration_dir, s.b.p); - - f32 c_dot = v2_dot(penetration_dir, s.c.p); - f32 c_wedge = v2_wedge(penetration_dir, s.c.p); - - - i32 code = 0; /* 0 = ab, 1 = ac, 2 = bc */ - -#if 0 - f32 highest_intercept; - if (math_fsign(a_wedge) != math_fsign(b_wedge)) { - highest_intercept = ((y2 - y1) / (x2 - x1)) * (-x1) + y1; - } - if (math_fsign(a_wedge) != math_fsign(c_wedge)) { - f32 intercept = ((y2 - y1) / (x2 - x1)) * (-x1) + y1; - if (intercept > highest_intercept) { - code = 1; - highest_intercept = intercept; - } - } - if (math_fsign(b_wedge) != math_fsign(c_wedge)) { - f32 intercept = ((y2 - y1) / (x2 - x1)) * (-x1) + y1; - if (intercept > highest_intercept) { - code = 2; - highest_intercept = intercept; - } - } -#else - f32 highest_intercept = -F32_INFINITY; - if (math_fsign(a_wedge) != math_fsign(b_wedge)) { - highest_intercept = ((b_dot - a_dot) / (b_wedge - a_wedge)) * (-a_wedge) + a_dot; - } - if (math_fsign(a_wedge) != math_fsign(c_wedge)) { - f32 intercept = ((c_dot - a_dot) / (c_wedge - a_wedge)) * (-a_wedge) + a_dot; - if (intercept > highest_intercept) { - code = 1; - highest_intercept = intercept; - } - } - if (math_fsign(b_wedge) != math_fsign(c_wedge)) { - f32 intercept = ((c_dot - b_dot) / (c_wedge - b_wedge)) * (-b_wedge) + b_dot; - if (intercept > highest_intercept) { - code = 2; - highest_intercept = intercept; - } - } -#endif - - switch (code) { - case 0: { /* ab is furthest, remove c */ - dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), penetration_dir); - s.len = 2; - } break; - - case 1: { /* ac is furthest, remove b */ - dir = v2_perp_towards_dir(v2_sub(s.c.p, s.a.p), penetration_dir); - s.b = s.c; - s.len = 2; - } break; - - case 2: { /* bc is furthest, remove a */ - dir = v2_perp_towards_dir(v2_sub(s.c.p, s.b.p), penetration_dir); - s.a = s.b; - s.b = s.c; - s.len = 2; - } break; - } -#endif - } - } else { - + use_penetration_dir = !v2_eq(velocity, V2(0, 0)); + if (!use_penetration_dir) { /* ========================== * * Expand simplex towards closest edge to origin inside menkowski (EPA) * ========================== */ @@ -780,7 +594,7 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array s.len = 2; /* Find new point in dir */ - m = menkowski_point_extended(shape0, shape1, dir); + m = menkowski_point_extended_test(shape0, shape1, dir, xf0, xf1); /* Check unique */ /* TODO: Better */ @@ -810,25 +624,473 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array } proto[pen_pe_index] = m; } + } else { + /* ========================== * + * Move simplex towards penetration dir + * ========================== */ + + while (true) { + /* Second point is support point towards velocity */ + if (s.len == 1) { + DBGSTEP; + dir = v2_sub(v2_mul(velocity, v2_dot(velocity, s.a.p) / v2_dot(velocity, velocity)), s.a.p); + m = menkowski_point_extended_test(shape0, shape1, dir, xf0, xf1); + if (v2_eq(m.p, s.a.p)) { + break; + } + + if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { + /* ab lies on origin */ + break; + } + + s.b = s.a; + s.a = m; + s.len = 2; + /* Third point is support point in direction of line normal towards `a` projected onto velocity */ + dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), velocity); + } + + if (s.len == 2) { + DBGSTEP; + m = menkowski_point_extended_test(shape0, shape1, dir, xf0, xf1); + if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_sub(m.p, s.a.p))) < epsilon) { + /* New point is already on the current line */ + break; + } + s.c = s.b; + s.b = s.a; + s.a = m; + s.len = 3; + + DBGSTEP; + if (math_fabs(v2_wedge(v2_sub(s.b.p, s.a.p), v2_neg(s.a.p))) < epsilon) { + /* ab lies on origin */ + s.len = 2; + break; + } else if (math_fabs(v2_wedge(v2_sub(s.c.p, s.a.p), v2_neg(s.a.p))) < epsilon) { + /* ac lies on origin */ + s.b = s.c; + s.len = 2; + break; + } + } + +#if 0 + DBGSTEP; + //struct v2 vab = v2_sub(s.b.p, s.a.p); + + f32 a_dot = v2_dot(velocity, s.a.p); + f32 a_wedge = v2_wedge(velocity, s.a.p); + + f32 b_dot = v2_dot(velocity, s.b.p); + f32 b_wedge = v2_wedge(velocity, s.b.p); + + f32 c_dot = v2_dot(velocity, s.c.p); + f32 c_wedge = v2_wedge(velocity, s.c.p); + + + i32 code = 0; /* 0 = ab, 1 = ac, 2 = bc */ + + f32 highest_intercept = -F32_INFINITY; + if (math_fsign(a_wedge) != math_fsign(b_wedge)) { + highest_intercept = ((b_dot - a_dot) / (b_wedge - a_wedge)) * (-a_wedge) + a_dot; + } + if (math_fsign(a_wedge) != math_fsign(c_wedge)) { + f32 intercept = ((c_dot - a_dot) / (c_wedge - a_wedge)) * (-a_wedge) + a_dot; + if (intercept > highest_intercept) { + code = 1; + highest_intercept = intercept; + } + } + if (math_fsign(b_wedge) != math_fsign(c_wedge)) { + f32 intercept = ((c_dot - b_dot) / (c_wedge - b_wedge)) * (-b_wedge) + b_dot; + if (intercept > highest_intercept) { + code = 2; + highest_intercept = intercept; + } + } + + switch (code) { + case 0: + { /* ab is furthest, remove c */ + dir = v2_perp_towards_dir(v2_sub(s.b.p, s.a.p), velocity); + s.len = 2; + } break; + + case 1: + { /* ac is furthest, remove b */ + dir = v2_perp_towards_dir(v2_sub(s.c.p, s.a.p), velocity); + s.b = s.c; + s.len = 2; + } break; + + case 2: + { /* bc is furthest, remove a */ + dir = v2_perp_towards_dir(v2_sub(s.c.p, s.b.p), velocity); + s.a = s.b; + s.b = s.c; + s.len = 2; + } break; + } + +#else + + DBGSTEP; + struct v2 vab = v2_sub(s.b.p, s.a.p); + struct v2 vac = v2_sub(s.c.p, s.a.p); + struct v2 vbc = v2_sub(s.c.p, s.b.p); + + /* TODO: De-duplicate operations */ + i32 code = 0; /* 0 = ab, 1 = ac, 2 = bc */ + f32 furthest = -F32_INFINITY; + { + struct v2 vao = v2_neg(s.a.p); + f32 w = 1 / v2_wedge(vab, velocity); + f32 t1 = v2_wedge(vao, velocity) * w; + f32 t2 = v2_wedge(vao, vab) * w; + (UNUSED)t1; + (UNUSED)t2; + + //b32 intersects = 0 <= t1 && 0 <= t2 && t2 <= 1; + b32 intersects = true; + if (intersects && t1 > furthest) { + code = 0; + furthest = t2; + } + } + { + struct v2 vao = v2_neg(s.a.p); + f32 w = 1 / v2_wedge(vac, velocity); + f32 t1 = v2_wedge(vao, velocity) * w; + f32 t2 = v2_wedge(vao, vac) * w; + (UNUSED)t1; + (UNUSED)t2; + + //b32 intersects = 0 <= t1 && 0 <= t2 && t2 <= 1; + b32 intersects = true; + if (intersects && t1 > furthest) { + code = 1; + furthest = t2; + } + } + { + struct v2 vbo = v2_neg(s.b.p); + f32 w = 1 / v2_wedge(vbc, velocity); + f32 t1 = v2_wedge(vbo, velocity) * w; + f32 t2 = v2_wedge(vbo, vbc) * w; + (UNUSED)t1; + (UNUSED)t2; + + //b32 intersects = 0 <= t1 && 0 <= t2 && t2 <= 1; + b32 intersects = true; + if (intersects && t1 > furthest) { + code = 2; + furthest = t2; + } + } + + switch (code) { + case 0: + { /* ab is furthest, remove c */ + dir = v2_perp_towards_dir(vab, velocity); + s.len = 2; + } break; + + case 1: + { /* ac is furthest, remove b */ + dir = v2_perp_towards_dir(vac, velocity); + s.b = s.c; + s.len = 2; + } break; + + case 2: + { /* bc is furthest, remove a */ + dir = v2_perp_towards_dir(vbc, velocity); + s.a = s.b; + s.b = s.c; + s.len = 2; + } break; + } + +#endif + } } + } - /* Resolve points */ +#else + if (colliding) { + /* ========================== * + * Epa + * ========================== */ + use_penetration_dir = !v2_eq(velocity, V2(0, 0)); + proto = arena_dry_push(scratch.arena, struct gjk_menkowski_point); + proto_count = 0; + { + ASSERT(s.len == 3); + struct gjk_menkowski_point *tmp = arena_push_array(scratch.arena, struct gjk_menkowski_point, 3); + tmp[0] = s.a; + tmp[1] = s.b; + tmp[2] = s.c; + proto_count = 3; + } + + if (!use_penetration_dir) { + /* ========================== * + * Expand simplex towards closest edge to origin inside menkowski + * ========================== */ + + while (true) { + DBGSTEP; + f32 pen_len_sq = F32_INFINITY; + + /* Find dir from origin to closest edge */ + /* FIXME: Winding order of ps & pe index */ + u32 pen_ps_index = 0; + u32 pen_pe_index = 0; + for (u32 i = 0; i < proto_count; ++i) { + u32 ps_index = i; + u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0; + struct v2 ps = proto[ps_index].p; + struct v2 pe = proto[pe_index].p; + + struct v2 vse = v2_sub(pe, ps); + struct v2 vso = v2_neg(ps); + + f32 d1 = v2_dot(vso, vse); + f32 d2 = v2_dot(vse, vse); + struct v2 vsd = v2_mul(vse, (d1 / d2)); + struct v2 pd = v2_add(ps, vsd); + + f32 pd_len_sq = v2_len_sq(pd); + if (pd_len_sq < pen_len_sq) { + pen_ps_index = ps_index; + pen_pe_index = pe_index; + pen_len_sq = pd_len_sq; + dir = pd; + } + } + + /* TODO: Move to break (debugging) */ + s.a = proto[pen_ps_index]; + s.b = proto[pen_pe_index]; + s.len = 2; + + /* Find new point in dir */ + m = menkowski_point_extended_test(shape0, shape1, dir, xf0, xf1); + + /* Check unique */ + /* TODO: Better */ + { + b32 unique = true; + for (u32 i = 0; i < proto_count; ++i) { + struct v2 edge_start = proto[i].p; + struct v2 edge_end = i < proto_count - 1 ? proto[i + 1].p : proto[0].p; + if (math_fabs(v2_wedge(v2_sub(edge_end, edge_start), v2_sub(m.p, edge_start))) < epsilon) { + unique = false; + break; + } + } + if (!unique) { + break; + } + } + + /* Insert point into prototype */ + /* FIXME: Preserve winding order */ + arena_push(scratch.arena, struct v2); + ++proto_count; + for (u32 i = proto_count - 1; i > pen_pe_index; --i) { + u32 shift_from = (i > 0) ? i - 1 : proto_count - 1; + u32 shift_to = i; + proto[shift_to] = proto[shift_from]; + } + proto[pen_pe_index] = m; + } + } else { + /* ========================== * + * Expand simplex towards furthest edge along velocity + * ========================== */ + + while (true) { + DBGSTEP; + + /* FIXME: Winding order of ps & pe index */ #if 0 - DBGSTEP; + f32 furthest = -F32_INFINITY; + u32 pen_ps_index = 0; + u32 pen_pe_index = 0; + for (u32 i = 0; i < proto_count; ++i) { + u32 ps_index = i; + u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0; + struct v2 ps = proto[ps_index].p; + struct v2 pe = proto[pe_index].p; + + + + struct v2 vse = v2_sub(pe, ps); + struct v2 vso = v2_neg(ps); + + f32 w = v2_wedge(vse, velocity); + if (w != 0) { + w = 1 / w; + f32 t1 = v2_wedge(vso, velocity) * w; + f32 t2 = v2_wedge(vso, vse) * w; + (UNUSED)t1; + (UNUSED)t2; + b32 intersects = 0 <= t1 && 0 <= t2 && t2 <= 1; + if (t2 > furthest) { + pen_ps_index = ps_index; + pen_pe_index = pe_index; + furthest = t2; + dir = v2_perp_towards_dir(vse, velocity); + //dir = velocity; + } + } + } +#else + f32 highest_score = -F32_INFINITY; + u32 pen_ps_index = 0; + u32 pen_pe_index = 0; + for (u32 i = 0; i < proto_count; ++i) { + u32 ps_index = i; + u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0; + struct v2 ps = proto[ps_index].p; + struct v2 pe = proto[pe_index].p; + + struct v2 vse = v2_sub(pe, ps); + struct v2 vso = v2_neg(ps); + + f32 w = v2_wedge(vse, velocity); + f32 t1 = 0; + f32 t2 = 0; + b32 intersects = false; + if (w != 0) { + w = 1 / w; + t1 = v2_wedge(vso, velocity) * w; + t2 = v2_wedge(vso, vse) * w; + intersects = 0 <= t1 && t1 <= 1 && 0 <= t2 && t2 <= 1; + } + + struct v2 next_dir = dir; + f32 score = highest_score; + if (intersects) { + /* Score is intersection (+1 to ensure intersections get priority) */ + score = 1 + t1; + next_dir = v2_perp_towards_dir(vse, velocity); + } else { + /* Score is inverse projection len */ + //f32 len = v2_len(v2_sub(velocity, ps)); + + struct v2 vsv = v2_sub(velocity, ps); + + f32 r = v2_dot(vse, vsv) / v2_dot(vse, vse); + struct v2 vsd = v2_mul(vse, r); + + f32 len = v2_len(v2_sub(vsd, vsv)); + + score = 1.0 - len; + + next_dir = v2_perp_towards_dir(vse, ps); + } + + if (score > highest_score) { + pen_ps_index = ps_index; + pen_pe_index = pe_index; + highest_score = score; + + (UNUSED)next_dir; + dir = next_dir; + //dir = velocity; + } + } +#endif + + /* TODO: Move to break (debugging) */ + s.a = proto[pen_ps_index]; + s.b = proto[pen_pe_index]; + s.len = 2; + + /* Find new point in dir */ + m = menkowski_point_extended_test(shape0, shape1, dir, xf0, xf1); + + /* Check unique */ + /* TODO: Better */ + /* FIXME: Winding order */ + u32 insert_start_index = 0; + u32 insert_end_index = 1; + { + b32 unique = true; + for (u32 i = 0; i < proto_count; ++i) { + u32 start_index = i; + u32 end_index = i < proto_count - 1 ? i + 1 : 0; + struct v2 edge_start = proto[start_index].p; + struct v2 edge_end = proto[end_index].p; + + struct v2 vso = v2_neg(edge_start); + struct v2 vse = v2_sub(edge_end, edge_start); + struct v2 vsm = v2_sub(m.p, edge_start); + + f32 w = v2_wedge(vse, vsm); + if (math_fabs(w) > epsilon) { + w = v2_wedge(vse, m.p); + if (w != 0) { + w = 1 / w; + f32 t1 = v2_wedge(vso, m.p) * w; + f32 t2 = v2_wedge(vso, vse) * w; + b32 intersects = 0 <= t1 && t1 <= 1 && 0 <= t2 && t2 <= 1; + if (intersects) { + insert_start_index = start_index; + insert_end_index = end_index; + } + } + } else { + unique = false; + break; + } + } + if (!unique) { + break; + } + } + + (UNUSED)insert_start_index; + DBGSTEP; + /* Insert point into prototype */ + /* FIXME: Preserve winding order */ + arena_push(scratch.arena, struct v2); + ++proto_count; + for (u32 i = proto_count - 1; i > insert_end_index; --i) { + u32 shift_from = (i > 0) ? i - 1 : proto_count - 1; + u32 shift_to = i; + proto[shift_to] = proto[shift_from]; + } + proto[insert_end_index] = m; + } + } + } +#endif + + + + + + + + + + +#if 0 + /* Resolve points */ if (s.len == 1) { shape0_p = s.a.p0; shape1_p = s.a.p1; - } else { - ASSERT(s.len == 2); + } else if (s.len == 2) { /* FIXME: Winding order dependent? */ + ASSERT(s.len == 2); f32 ratio; - if (use_penetration_dir) { - /* Determine ratio between edge a & b that penetration dir intersection lies */ - f32 wedgea = math_fabs(v2_wedge(penetration_dir, s.a.p)); - f32 wedgeb = math_fabs(v2_wedge(penetration_dir, s.b.p)); - ratio = wedgea / (wedgea + wedgeb); - } else { + { /* Determine ratio between edge a & b that projected origin lies */ struct v2 vab = v2_sub(s.b.p, s.a.p); struct v2 vao = v2_neg(s.a.p); @@ -844,7 +1106,6 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1_p = v2_add(shape1_p, s.a.p1); } #else - DBGSTEP; if (s.len == 1) { shape0_p = s.a.p0; shape1_p = s.a.p1; @@ -856,19 +1117,25 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array struct v2 vao = v2_neg(s.a.p); struct v2 vab = v2_sub(s.b.p, s.a.p); - f32 t1 = v2_wedge(penetration_dir, vao) / v2_wedge(penetration_dir, vab); - f32 t2 = v2_wedge(vab, vao) / v2_wedge(penetration_dir, vab); + f32 w = 1 / v2_wedge(vab, velocity); + f32 t1 = v2_wedge(vao, velocity) * w; + f32 t2 = v2_wedge(vao, vab) * w; - velocity_intersects = 0 <= t1 && 0 <= t2 && t2 <= 1; + velocity_intersects = 0 <= t1 && t1 <= 1 && 0 <= t2 && t2 <= 1; +#if 1 if (velocity_intersects) { /* Ratio between edge a & b that velocity intersection lies */ ratio = t1; } else { /* Ratio between edge a & b that projected velocity lies */ - struct v2 vap = v2_sub(penetration_dir, s.a.p); + struct v2 vap = v2_sub(velocity, s.a.p); ratio = clamp_f32(v2_dot(vab, vap) / v2_dot(vab, vab), 0, 1); } +#else + /* Ratio between edge a & b that velocity intersection lies */ + ratio = t1; +#endif } else { /* Determine ratio between edge a & b that projected origin lies */ struct v2 vab = v2_sub(s.b.p, s.a.p); @@ -884,9 +1151,29 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1_p = v2_mul(shape1_p, ratio); shape1_p = v2_add(shape1_p, s.a.p1); } + #endif + + res.solved = true; abort: + if (proto_count > 0) { + for (u32 i = 0; i < min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); ++i) { + res.prototype.points[i] = proto[i].p; + } + res.prototype.len = proto_count; + } else { + if (s.len >= 1) { + res.prototype.points[0] = s.a.p; + if (s.len >= 2) { + res.prototype.points[1] = s.b.p; + if (s.len >= 3) { + res.prototype.points[2] = s.c.p; + } + } + } + res.prototype.len = s.len; + } res.colliding = colliding; res.p0 = shape0_p; res.p1 = shape1_p; @@ -896,28 +1183,16 @@ abort: return res; } -#endif - - - - - - - - - - - /* TODO: Remove this (debugging) */ -struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1) +struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct xform xf0 ,struct xform xf1) { struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; u64 rays = 500; for (u64 i = 0; i < rays; ++i) { f32 angle = ((f32)i / rays) * (2 * PI); struct v2 dir = v2_from_angle(angle); - struct v2 p = menkowski_point_extended(poly0, poly1, dir).p; + struct v2 p = menkowski_point_extended_test(poly0, poly1, dir, xf0, xf1).p; if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) { *arena_push(arena, struct v2) = p; ++res.count; @@ -925,3 +1200,32 @@ struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_ } return res; } + +struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct xform xf0, struct xform xf1) +{ + (UNUSED)xf0; + (UNUSED)xf1; + + struct v2_array res = { .points = arena_dry_push(arena, struct v2) }; + + struct v2 velocity = v2_sub(xf1.og, xf0.og); + for (u64 i = 0; i < poly0.count; ++i) { + struct v2 p0 = poly0.points[i]; + for (u64 j = 0; j < poly1.count; ++j) { + struct v2 p1 = poly1.points[j]; + struct v2 diff = v2_sub(p0, p1); + { + *arena_push(arena, struct v2) = diff; + ++res.count; + } + { + *arena_push(arena, struct v2) = v2_add(diff, velocity);; + ++res.count; + } + } + } + + return res; +} + +#endif diff --git a/src/gjk.h b/src/gjk.h index 1e48df0c..6d5ec859 100644 --- a/src/gjk.h +++ b/src/gjk.h @@ -18,6 +18,11 @@ struct gjk_simplex { struct gjk_menkowski_point a, b, c; }; +/* Returns simple true or false indicating shape collision */ +b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1); + +#if 0 + struct gjk_extended_result { b32 colliding; struct v2 p0, p1; /* Closest points (or penetrating points if colliding) on each shape */ @@ -27,9 +32,6 @@ struct gjk_extended_result { b32 velocity_intersects; }; -/* Returns simple true or false indicating shape collision */ -b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1); - /* Returns shape whether shapes are colliding well as closest / penetrating points on each shape. * * If shapes are colliding and `penetration_dir` is non-zero, the shortest @@ -41,4 +43,31 @@ struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1); +#else + +/* TODO: Remove this (debugging) */ +struct gjk_prototype { + struct v2 points[256]; + u32 len; +}; + +struct gjk_extended_result { + b32 colliding; + struct v2 p0, p1; /* Closest points on each shape */ + + /* For debugging */ + struct gjk_simplex simplex; + b32 velocity_intersects; + b32 solved; + struct gjk_prototype prototype; +}; + +struct gjk_extended_result gjk_extended(struct v2_array shape0, struct v2_array shape1, struct xform xf0, struct xform xf1); + +struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct xform xf0, struct xform xf1); + +struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_array poly1, struct xform xf0, struct xform xf1); + +#endif + #endif diff --git a/src/user.c b/src/user.c index 768d466d..2fa7c9cc 100644 --- a/src/user.c +++ b/src/user.c @@ -991,45 +991,94 @@ INTERNAL void user_update(void) (UNUSED)colliding; #if 1 + /* Create shapes */ + struct quad ent_quad_xf0; + struct v2_array ent_poly_xf0; + struct quad ent_quad_xf1; + struct v2_array ent_poly_xf1; + struct quad e1_quad; + struct v2_array e1_poly; + { + { + struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, ent->sprite); + struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), ent->animation_frame); + ent_quad_xf0 = xform_mul_quad(ent->sprite_local_xform, quad_from_rect(slice.rect)); + ent_quad_xf0 = xform_mul_quad(ent->xf0, ent_quad_xf0); + ent_poly_xf0 = (struct v2_array) { + .count = ARRAY_COUNT(ent_quad_xf0.e), + .points = ent_quad_xf0.e + }; + } + { + struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, ent->sprite); + struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), ent->animation_frame); + ent_quad_xf1 = xform_mul_quad(ent->sprite_local_xform, quad_from_rect(slice.rect)); + ent_quad_xf1 = xform_mul_quad(ent->xf1, ent_quad_xf1); + ent_poly_xf1 = (struct v2_array) { + .count = ARRAY_COUNT(ent_quad_xf1.e), + .points = ent_quad_xf1.e + }; + } + { + 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(entity_get_xform(e1), e1_quad); + e1_poly = (struct v2_array) { + .count = ARRAY_COUNT(e1_quad.e), + .points = e1_quad.e + }; + } + } + + /* Draw collision shapes */ + { + (UNUSED)ent_poly_xf0; + (UNUSED)ent_poly_xf1; + f32 thickness = 2; + { + u32 color = RGBA_32_F(1, 0, 0, 0.25); + struct quad quad = xform_mul_quad(G.world_view, ent_quad_xf0); + draw_solid_quad_line(G.viewport_canvas, quad, thickness, color); + } + { + u32 color = RGBA_32_F(0, 1, 0, 0.25); + struct quad quad = xform_mul_quad(G.world_view, ent_quad_xf1); + draw_solid_quad_line(G.viewport_canvas, quad, thickness, color); + } + } + /* Draw menkowski */ if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) { - struct quad ent_quad; - struct v2_array ent_poly; - struct quad e1_quad; - struct v2_array e1_poly; - { - { - struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, ent->sprite); - struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("shape"), ent->animation_frame); - ent_quad = xform_mul_quad(ent->sprite_local_xform, quad_from_rect(slice.rect)); - ent_quad = xform_mul_quad(entity_get_xform(ent), ent_quad); - ent_poly = (struct v2_array) { - .count = ARRAY_COUNT(ent_quad.e), - .points = ent_quad.e - }; - } - { - 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(entity_get_xform(e1), e1_quad); - e1_poly = (struct v2_array) { - .count = ARRAY_COUNT(e1_quad.e), - .points = e1_quad.e - }; - } - } - u32 color = RGBA_32_F(0, 0, 0.25, 1); + u32 color = ent->solved ? RGBA_32_F(0, 0, 0.25, 1) : RGBA_32_F(0, 0.25, 0.25, 1); f32 thickness = 2; (UNUSED)thickness; - struct v2_array m = menkowski(temp.arena, ent_poly, e1_poly); + //struct v2_array m = menkowski(temp.arena, ent_poly, e1_poly); + + struct v2_array m = menkowski(temp.arena, ent_poly_xf0, e1_poly, ent->xf0, ent->xf1); + for (u64 i = 0; i < m.count; ++i) m.points[i] = xform_mul_v2(G.world_view, m.points[i]); draw_solid_poly_line(G.viewport_canvas, m, true, thickness, color); //draw_solid_poly(G.viewport_canvas, m, color); } + /* Draw cloud */ + if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) { + u32 color = RGBA_32_F(1, 1, 1, 1); + f32 radius = 2; + + //struct v2_array m = menkowski(temp.arena, ent_poly, e1_poly); + + struct v2_array m = cloud(temp.arena, ent_poly_xf0, e1_poly, ent->xf0, ent->xf1); + + for (u64 i = 0; i < m.count; ++i) { + struct v2 p = xform_mul_v2(G.world_view, m.points[i]);; + draw_solid_circle(G.viewport_canvas, p, radius, color, 10); + } + } + /* Draw pendir */ if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) { f32 thickness = 2; @@ -1041,6 +1090,19 @@ INTERNAL void user_update(void) draw_solid_arrow_ray(G.viewport_canvas, start, ray, thickness, thickness * 4, color); } + /* Draw prototype */ + if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) { + f32 thickness = 2; + u32 color = RGBA_32_F(1, 1, 1, 0.25); + + struct v2_array m = { + .points = ent->prototype.points, + .count = ent->prototype.len + }; + for (u64 i = 0; i < m.count; ++i) m.points[i] = xform_mul_v2(G.world_view, m.points[i]); + draw_solid_poly_line(G.viewport_canvas, m, true, thickness, color); + } + /* Draw simplex */ if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) { f32 thickness = 2; @@ -1074,10 +1136,10 @@ INTERNAL void user_update(void) /* Draw point */ { - u32 color = entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED) ? COLOR_PURPLE : COLOR_TURQOISE; - f32 thickness = 5; + u32 color = entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED) ? RGBA_32_F(1, 0, 0, 0.5) : RGBA_32_F(0, 1, 1, 0.5); + f32 radius = 5; struct v2 point = xform_mul_v2(G.world_view, ent->point); - draw_solid_circle(G.viewport_canvas, point, thickness, color, 10); + draw_solid_circle(G.viewport_canvas, point, radius, color, 10); } } @@ -1256,6 +1318,18 @@ INTERNAL void user_update(void) draw_text(G.viewport_canvas, font, pos, string_format(temp.arena, STR("debug_camera: %F"), FMT_STR(G.debug_camera ? STR("true") : STR("false")))); pos.y += spacing; + struct v2 player_collision_vel = entity_find_first_match_one(store, ENTITY_PROP_PLAYER_CONTROLLED)->pendir; + draw_text(G.viewport_canvas, font, pos, string_format(temp.arena, STR("player collision velocity: (%F, %F)"), FMT_FLOAT_P((f64)player_collision_vel.x, 12), FMT_FLOAT_P((f64)player_collision_vel.y, 12))); + pos.y += spacing; + + struct v2 xf0_pos = entity_find_first_match_one(store, ENTITY_PROP_PLAYER_CONTROLLED)->xf0.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))); + 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 draw_text(G.viewport_canvas, font, pos, string_format(temp.arena, STR("gjk steps: %F"), FMT_UINT(gjk_debug_steps))); pos.y += spacing;