diff --git a/src/draw.c b/src/draw.c index 4cbef17b..b58fe8ba 100644 --- a/src/draw.c +++ b/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_perp_mul(head_start_dir, head_width_ratio); + struct v2 head_p1_dir = v2_perp_right_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); diff --git a/src/entity.h b/src/entity.h index 7c280fd1..18c57c8f 100644 --- a/src/entity.h +++ b/src/entity.h @@ -66,6 +66,7 @@ struct entity_store { struct contact { struct v2 point; + u32 id; f32 separation; /* How far is p0 from p1 along normal */ f32 accumulated_impulse; /* Accumulated impulse along normal */ diff --git a/src/game.c b/src/game.c index 6df49ac4..b5b97218 100644 --- a/src/game.c +++ b/src/game.c @@ -133,8 +133,8 @@ INTERNAL void spawn_test_entities(void) //struct v2 pos = V2(0.374142020941, -0.246118023992); /* Touching glitch spot */ //struct v2 size = V2(1, 1); struct v2 size = V2(0.5, 0.5); - //f32 r = PI / 4; - f32 r = PI / 3; + f32 r = PI / 4; + //f32 r = PI / 3; //f32 r = PI / 2; //f32 r = 0; f32 skew = 0; @@ -196,8 +196,8 @@ INTERNAL void spawn_test_entities(void) if (!G.box_spawned) { G.box_spawned = true; - //struct v2 pos = V2(0.5, -0.5); - struct v2 pos = V2(0.5, -1); + //struct v2 pos = V2(0.5, -1); + struct v2 pos = V2(1, -1); struct v2 size = V2(1, 1); //f32 rot = PI / 4; f32 rot = 0; @@ -356,13 +356,12 @@ INTERNAL void create_contact_manifolds(void) manifold->solved = res.solved; } - if (res.num_pairs > 0) { + if (res.num_points > 0) { if (!manifold->valid) { manifold = entity_alloc(root); manifold->manifold_e0 = e0->handle; manifold->manifold_e1 = e1->handle; /* TODO: Should we recalculate normal as more contact points are added? */ - manifold->manifold_normal = v2_norm(v2_sub(res.pairs[0].p1, res.pairs[0].p0)); entity_enable_prop(manifold, ENTITY_PROP_MANIFOLD); activate_now(manifold); @@ -381,17 +380,14 @@ INTERNAL void create_contact_manifolds(void) manifold->solved = res.solved; } } + manifold->manifold_normal = res.normal; manifold->num_contacts = 0; /* Insert new contacts that aren't too close to original contacts */ - for (u32 i = 0; i < res.num_pairs; ++i) { + for (u32 i = 0; i < res.num_points; ++i) { b32 should_insert = true; - struct gjk_contact_pair new_pair = res.pairs[i]; - struct v2 p0_world = new_pair.p0; - struct v2 p1_world = new_pair.p1; - #if 0 for (u32 j = 0; j < manifold->num_contacts; ++j) { struct contact *contact = &manifold->contacts[j]; @@ -410,12 +406,14 @@ INTERNAL void create_contact_manifolds(void) struct contact *c; c = &manifold->contacts[manifold->num_contacts++]; - f32 depth = v2_len(v2_sub(p1_world, p0_world)); - struct v2 point = v2_mul(v2_add(p0_world, p1_world), 0.5f); + u32 id = res.points[i].id; + struct v2 point = res.points[i].point; + f32 separation = res.points[i].separation; MEMZERO_STRUCT(c); + c->id = id; c->point = point; - c->separation = depth; + c->separation = separation; } } @@ -481,12 +479,6 @@ INTERNAL void create_contact_manifolds(void) - /* NOTE: This assumes that all contacts will share the same normal as first contact */ - manifold->manifold_normal = v2_norm(v2_sub(res.pairs[0].p1, res.pairs[0].p0)); - - - - /* TODO: Remove this (testing) */ #if 0 @@ -578,8 +570,8 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) struct v2 vcp0 = v2_sub(p, e0_xf.og); struct v2 vcp1 = v2_sub(p, e1_xf.og); - struct v2 vel0 = v2_add(e0->linear_velocity, v2_perp_mul(vcp0, e0->angular_velocity)); - struct v2 vel1 = v2_add(e1->linear_velocity, v2_perp_mul(vcp1, e1->angular_velocity)); + struct v2 vel0 = v2_add(e0->linear_velocity, v2_perp_right_mul(vcp0, e0->angular_velocity)); + struct v2 vel1 = v2_add(e1->linear_velocity, v2_perp_right_mul(vcp1, e1->angular_velocity)); struct v2 vrel = v2_sub(vel1, vel0); @@ -587,8 +579,8 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) /* FIXME: Clamp accumulated */ 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_right_mul(vcp0, v2_wedge(vcp0, normal) * inv_i0); + struct v2 idk1 = v2_perp_right_mul(vcp1, v2_wedge(vcp1, normal) * inv_i1); f32 k = inv_m0 + inv_m1 + v2_dot(normal, v2_add(idk0, idk1)); @@ -1068,7 +1060,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * Create forces from control focus (aim) * ========================== */ -#if 0 +#if 1 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; diff --git a/src/gjk.c b/src/gjk.c index 61d5494a..65d8f639 100644 --- a/src/gjk.c +++ b/src/gjk.c @@ -9,7 +9,7 @@ u32 gjk_debug_steps = U32_MAX; #define DBGSTEP if (dbg_step++ >= gjk_debug_steps) goto abort -INTERNAL struct v2 poly_support(struct v2_array a, struct v2 dir) +INTERNAL struct v2 poly_support_point(struct v2_array a, struct v2 dir) { /* TODO: Could probably binary search for largest dot since shape is convex */ struct v2 furthest = a.points[0]; @@ -25,23 +25,42 @@ INTERNAL struct v2 poly_support(struct v2_array a, struct v2 dir) return furthest; } +INTERNAL u32 poly_support_point_index(struct v2_array a, struct v2 dir) +{ + u32 furthest = 0; + f32 furthest_dot = v2_dot(dir, a.points[0]); + for (u32 i = 1; i < a.count; ++i) { + struct v2 p = a.points[i]; + f32 dot = v2_dot(dir, p); + if (dot > furthest_dot) { + furthest = i; + furthest_dot = dot; + } + } + return furthest; +} + INTERNAL struct v2 menkowski_point(struct v2_array poly0, struct v2_array shape1, struct v2 dir) { - return v2_sub(poly_support(poly0, dir), poly_support(shape1, v2_neg(dir))); + return v2_sub(poly_support_point(poly0, dir), poly_support_point(shape1, v2_neg(dir))); } b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1) { struct { struct v2 a, b, c; } s = ZI; - /* FIXME: Infinite loop when shapes exactly overlap same space */ + /* FIXME: Infinite loop when shapes exactly overlap same space? */ + + struct v2 dir, p; /* First point is support point in shape's general directions to eachother */ - s.a = menkowski_point(shape0, shape1, v2_sub(shape1.points[0], shape0.points[0])); + dir = v2_sub(shape1.points[0], shape0.points[0]); + if (v2_is_zero(dir)) dir = V2(1, 0); + s.a = menkowski_point(shape0, shape1, dir); /* Second point is support point towards origin */ - struct v2 dir = v2_neg(s.a); - struct v2 p = menkowski_point(shape0, shape1, dir); + dir = v2_neg(s.a); + p = menkowski_point(shape0, shape1, dir); if (v2_dot(dir, p) >= 0) { s.b = s.a; s.a = p; @@ -82,25 +101,6 @@ b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1) return false; } -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; -} - - - - - - - - - - - @@ -115,7 +115,7 @@ INTERNAL struct gjk_menkowski_point menkowski_point_extended(struct v2_array pol * TODO: Remove these * ========================== */ -/* TODO: Remove this (debugging) */ + /* 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) }; @@ -123,7 +123,7 @@ struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_ 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(poly0, poly1, dir); if (res.count == 0 || !v2_eq(p, res.points[res.count - 1])) { *arena_push(arena, struct v2) = p; ++res.count; @@ -156,6 +156,444 @@ struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_arra + + + + + + + + +struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1) +{ + struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */ + struct gjk_contact_points_result res = ZI; + + /* TODO: Set all epsilons used in this function to 0.005 */ + + /* TODO: Verify epsilon */ + /* FIXME: Infinite loop when epsilon too low and a shape is too far from 0 (precision issue) */ + const f32 epsilon = 0.0000100f; + struct gjk_simplex s = ZI; + b32 colliding = false; + struct v2 *proto = NULL; + u32 proto_count = 0; + + struct v2 normal = ZI; + struct gjk_contact_point points[2] = ZI; + u32 num_points = 0; + + /* Used by GJK & EPA */ + struct v2 dir = ZI; + struct v2 m = ZI; + +#if GJK_DEBUG + u32 dbg_step = 0; +#endif + + /* ========================== * + * GJK + * ========================== */ + + { + /* FIXME: Collision is false when 2 shapes exactly overlap */ + + /* First point is support point in shape's general directions to eachother */ + dir = v2_sub(shape1.points[0], shape0.points[0]); + if (v2_is_zero(dir)) dir = V2(1, 0); + s.a = menkowski_point(shape0, shape1, dir); + s.len = 1; + + /* Second point is support point towards origin */ + DBGSTEP; + dir = v2_neg(s.a); + m = menkowski_point(shape0, shape1, dir); + if (v2_dot(dir, m) > 0) { + DBGSTEP; + s.b = s.a; + s.a = m; + s.len = 2; + while (true) { + /* Third point is support point in direction of line normal towards origin */ + DBGSTEP; + dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(s.a)); + m = menkowski_point(shape0, shape1, dir); + if (v2_dot(dir, m) < 0) { + /* New point did not cross origin, collision impossible */ + break; + } + + s.c = s.b; + s.b = s.a; + s.a = m; + s.len = 3; + + struct v2 vab = v2_sub(s.b, s.a); + struct v2 vac = v2_sub(s.c, s.a); + struct v2 a_to_origin = v2_neg(s.a); + + DBGSTEP; + dir = v2_perp_towards_dir(vab, v2_neg(vac)); /* Normal of ab pointing away from c */ + if (v2_dot(dir, a_to_origin) > 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(vac, v2_neg(vab)); /* Normal of ac pointing away from b */ + if (v2_dot(dir, a_to_origin) > 0) { + /* Point is in region ac, remove b from simplex */ + s.b = s.c; + s.len = 2; + } else { + /* Point is in simplex */ + colliding = true; + break; + } + } + } + } + } + + if (colliding) { + /* ========================== * + * Epa (to find collision normal) + * ========================== */ + + proto = arena_dry_push(scratch.arena, struct v2); + proto_count = 0; + { + ASSERT(s.len == 3); + struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3); + tmp[0] = s.a; + tmp[1] = s.b; + tmp[2] = s.c; + proto_count = 3; + } + + while (true) { + 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]; + struct v2 pe = proto[pe_index]; + + 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; + normal = pd; + } + } + + /* TODO: Remove this (debugging) */ + s.a = proto[pen_ps_index]; + s.b = proto[pen_pe_index]; + s.len = 2; + + /* Find new point in dir */ + DBGSTEP; + { + /* Next point is in direction of line normal pointing outwards from shape */ + /* TODO: If winding order is guaranteed then this can become v2_perp_left/right */ + struct v2 a = proto[pen_ps_index]; + struct v2 b = proto[pen_pe_index]; + struct v2 n = proto[(pen_pe_index < proto_count - 1) ? (pen_pe_index + 1) : 0]; /* Next point along prototype after edge */ + struct v2 van = v2_sub(n, a); + struct v2 vab = v2_sub(b, a); + dir = v2_perp_towards_dir(vab, v2_neg(van)); + } + m = menkowski_point(shape0, shape1, dir); + + /* Check unique */ + /* TODO: Better */ + { + b32 unique = true; + for (u32 i = 0; i < proto_count; ++i) { + struct v2 edge_start = proto[i]; + struct v2 edge_end = i < proto_count - 1 ? proto[i + 1] : proto[0]; + if (math_fabs(v2_wedge(v2_sub(edge_end, edge_start), v2_sub(m, edge_start))) < epsilon) { + unique = false; + break; + } + } + if (!unique) { + normal = v2_norm(normal); + break; + } + } + + /* Insert point into prototype */ + /* FIXME: Preserve winding order */ + arena_push(scratch.arena, struct gjk_menkowski_point); + ++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; + } + + /* ========================== * + * Clipping + * ========================== */ + + /* FIXME: Limit max vertices in shape structure to 8 for id generation to be correct */ + ASSERT(shape0.count <= 8); + ASSERT(shape1.count <= 8); + + DBGSTEP; + { + const f32 wedge_epsilon = 0.001; + + /* shape0 a -> b winding = clockwise */ + u32 id_a0; + u32 id_b0; + struct v2 a0; + struct v2 b0; + struct v2 vab0; + + /* shape1 a -> b winding = counterclockwise */ + u32 id_a1; + u32 id_b1; + struct v2 a1; + struct v2 b1; + struct v2 vab1; + { + u32 p_i = poly_support_point_index(shape0, normal); + u32 a_i = (p_i > 0) ? (p_i - 1) : (shape0.count - 1); + u32 b_i = ((p_i + 1) < shape0.count) ? (p_i + 1) : 0; + + struct v2 p = shape0.points[p_i]; + struct v2 a = shape0.points[a_i]; + struct v2 b = shape0.points[b_i]; + + struct v2 vap = v2_sub(p, a); + struct v2 vpb = v2_sub(b, p); + + /* FIXME: Make winding order independent */ + if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { + id_a0 = a_i; + id_b0 = p_i; + a0 = a; + b0 = p; + vab0 = vap; + } else { + id_a0 = p_i; + id_b0 = b_i; + a0 = p; + b0 = b; + vab0 = vpb; + } + } + { + struct v2 neg_normal = v2_neg(normal); + + u32 p_i = poly_support_point_index(shape1, neg_normal); + u32 a_i = ((p_i + 1) < shape1.count) ? (p_i + 1) : 0; + u32 b_i = (p_i > 0) ? (p_i - 1) : (shape1.count - 1); + + struct v2 p = shape1.points[p_i]; + struct v2 a = shape1.points[a_i]; + struct v2 b = shape1.points[b_i]; + + struct v2 vap = v2_sub(p, a); + struct v2 vpb = v2_sub(b, p); + + /* FIXME: Make winding order independent */ + if (v2_wedge(vap, normal) < (v2_wedge(vpb, normal) + wedge_epsilon)) { + id_a1 = a_i; + id_b1 = p_i; + a1 = a; + b1 = p; + vab1 = vap; + } else { + id_a1 = p_i; + id_b1 = b_i; + a1 = p; + b1 = b; + vab1 = vpb; + } + } + + /* Clip */ + (UNUSED)a0; + (UNUSED)b0; + (UNUSED)vab0; + (UNUSED)a1; + (UNUSED)b1; + (UNUSED)vab1; + + + f32 a0t = -1; + f32 a1t = -1; + f32 b0t = -1; + f32 b1t = -1; + (UNUSED)a0t; + (UNUSED)a1t; + (UNUSED)b0t; + (UNUSED)b1t; + + struct v2 vba0 = v2_neg(vab0); + struct v2 vba1 = v2_neg(vab1); + (UNUSED)vba0; + (UNUSED)vba1; + { + { + struct v2 va0a1 = v2_sub(a1, a0); + struct v2 va1a0 = v2_neg(va0a1); + (UNUSED)va0a1; + (UNUSED)va1a0; + { + f32 w = v2_wedge(vab0, normal); + if (w != 0) { + w = 1 / w; + a0t = v2_wedge(va0a1, normal) * w; + } + } + { + f32 w = v2_wedge(vab1, normal); + if (w != 0) { + w = 1 / w; + a1t = v2_wedge(va1a0, normal) * w; + } + } + } + { + struct v2 vb0b1 = v2_sub(b1, b0); + struct v2 vb1b0 = v2_neg(vb0b1); + { + f32 w = v2_wedge(vba0, normal); + if (w != 0) { + w = 1 / w; + b0t = v2_wedge(vb0b1, normal) * w; + } + } + { + f32 w = v2_wedge(vba1, normal); + if (w != 0) { + w = 1 / w; + b1t = v2_wedge(vb1b0, normal) * w; + } + } + } + } + + a0t = clamp_f32(a0t, 0, 1); + a1t = clamp_f32(a1t, 0, 1); + b0t = clamp_f32(b0t, 0, 1); + b1t = clamp_f32(b1t, 0, 1); + + struct v2 a0_clipped = v2_add(a0, v2_mul(vab0, a0t)); + struct v2 a1_clipped = v2_add(a1, v2_mul(vab1, a1t)); + struct v2 b0_clipped = v2_add(b0, v2_mul(vba0, b0t)); + struct v2 b1_clipped = v2_add(b1, v2_mul(vba1, b1t)); + + struct v2 va0a1_clipped = v2_sub(a1_clipped, a0_clipped); + struct v2 vb0b1_clipped = v2_sub(b1_clipped, b0_clipped); + + f32 a_sep = -v2_dot(va0a1_clipped, normal); + f32 b_sep = -v2_dot(vb0b1_clipped, normal); + + if (a_sep > 0) { + struct gjk_contact_point *point = &points[num_points++]; + point->id = id_a0 | (id_a1 << 4); + point->separation = a_sep; + point->point = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f)); + } + if (b_sep > 0) { + struct gjk_contact_point *point = &points[num_points++]; + point->id = id_b0 | (id_b1 << 4); + point->separation = b_sep; + point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f)); + } + } + } + + res.solved = true; +abort: + + /* TODO: Remove this (testing) */ + //num_points = max_u32(num_points, 1); + + 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]; + } + res.prototype.len = proto_count; + } else { + if (s.len >= 1) { + res.prototype.points[0] = s.a; + if (s.len >= 2) { + res.prototype.points[1] = s.b; + if (s.len >= 3) { + res.prototype.points[2] = s.c; + } + } + } + res.prototype.len = s.len; + } + res.normal = normal; + res.points[0] = points[0]; + res.points[1] = points[1]; + res.num_points = num_points; + res.simplex = s; + scratch_end(scratch); + return res; +} + + + + + + + + + + + + + + + + + + + + + + + + +/* ========================== * + * GJK contact pairs (unused) + * ========================== */ + +#if 0 +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; +} + struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1) { struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */ @@ -404,6 +842,7 @@ abort: scratch_end(scratch); return res; } +#endif @@ -436,8 +875,8 @@ abort: * ========================== */ #if 0 -/* TODO: Update this function to only calculate contact pairs if colliding. Will allow for shortcut-style GJK. - * `colliding` & `has_2nd_pair` become "num_pairs", where 0 = no collision. */ + /* TODO: Update this function to only calculate contact pairs if colliding. Will allow for shortcut-style GJK. + * `colliding` & `has_2nd_pair` become "num_pairs", where 0 = no collision. */ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1) { struct temp_arena scratch = scratch_begin_no_conflict(); @@ -828,32 +1267,39 @@ 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) { - default: { /* No region, must be in simplex */ + default: + { /* No region, must be in simplex */ colliding = true; } break; - case 1: { /* Region ab, remove c */ + case 1: + { /* Region ab, remove c */ dir = rab_dir; /* Next third point is in direction of region ab */ s.len = 2; } break; - case 2: { /* Region ac, remove b */ + case 2: + { /* Region ac, remove b */ 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 */ + case 4: + { /* Region bc, remove a */ dir = rbc_dir; /* Next third point is in direction of region bc */ s.a = s.b; s.b = s.c; s.len = 2; } break; - case 3: { /* Region a, remove bc */ + case 3: + { /* Region a, remove bc */ s.len = 1; } break; - case 5: { /* Region b, remove ac */ + case 5: + { /* Region b, remove ac */ s.a = s.b; s.len = 1; } break; - case 6: { /* Region c, remove ab */ + case 6: + { /* Region c, remove ab */ s.a = s.c; s.len = 1; } break; @@ -1056,7 +1502,7 @@ INTERNAL struct poly_support_swept_result poly_support_swept(struct v2_array a, modified = v2_add(p, linear_velocity); } else { modified = p; - } + } #else modified = v2_add(p, linear_velocity); #endif @@ -1194,32 +1640,39 @@ 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) { - default: { /* No region, must be in simplex */ + default: + { /* No region, must be in simplex */ colliding = true; } break; - case 1: { /* Region ab, remove c */ + case 1: + { /* Region ab, remove c */ dir = rab_dir; /* Next third point is in direction of region ab */ s.len = 2; } break; - case 2: { /* Region ac, remove b */ + case 2: + { /* Region ac, remove b */ 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 */ + case 4: + { /* Region bc, remove a */ dir = rbc_dir; /* Next third point is in direction of region bc */ s.a = s.b; s.b = s.c; s.len = 2; } break; - case 3: { /* Region a, remove bc */ + case 3: + { /* Region a, remove bc */ s.len = 1; } break; - case 5: { /* Region b, remove ac */ + case 5: + { /* Region b, remove ac */ s.a = s.b; s.len = 1; } break; - case 6: { /* Region c, remove ab */ + case 6: + { /* Region c, remove ab */ s.a = s.c; s.len = 1; } break; diff --git a/src/gjk.h b/src/gjk.h index 62736ac6..dde7105f 100644 --- a/src/gjk.h +++ b/src/gjk.h @@ -13,19 +13,41 @@ struct gjk_menkowski_point { struct v2 p; /* Menkowski difference point */ }; +/* Returns simple true or false indicating shape collision */ +b32 gjk_boolean(struct v2_array shape0, struct v2_array shape1); + +struct gjk_simplex { + u32 len; + struct v2 a, b, c; +}; + +struct gjk_contact_point { + struct v2 point; + f32 separation; + u32 id; /* Based on polygon edge-to-edge */ +}; + +struct gjk_prototype { struct v2 points[64]; u32 len; }; +struct gjk_contact_points_result { + + struct v2 normal; + struct gjk_contact_point points[2]; + u32 num_points; + + /* For debugging */ + b32 solved; + struct gjk_simplex simplex; + struct gjk_prototype prototype; +}; + +struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1); + +#if 0 struct gjk_simplex { u32 len; 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); - -struct gjk_contact_pair { - struct v2 p0, p1; -}; - -struct gjk_prototype { struct v2 points[64]; u32 len; }; struct gjk_contact_points_result { struct gjk_contact_pair pairs[2]; @@ -38,6 +60,7 @@ struct gjk_contact_points_result { }; struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, struct v2_array shape1); +#endif struct v2_array menkowski(struct arena *arena, struct v2_array poly0, struct v2_array poly1); diff --git a/src/math.h b/src/math.h index c9215a8a..89ef7884 100644 --- a/src/math.h +++ b/src/math.h @@ -640,27 +640,42 @@ INLINE f32 v2_dot(struct v2 a, struct v2 b) return a.x * b.x + a.y * b.y; } +/* Returns signed area between vectors (positive in clockwise direction) + * AKA perpendicular dot product + * AKA 2d cross-product */ INLINE f32 v2_wedge(struct v2 a, struct v2 b) { return a.x * b.y - a.y * b.x; } -/* Right (clockwise) perpendicular vector */ -INLINE struct v2 v2_perp(struct v2 a) +/* Clockwise perpendicular vector */ +INLINE struct v2 v2_perp_right(struct v2 a) { return V2(-a.y, a.x); } -/* Right (clockwise) perpendicular vector scaled by s */ -INLINE struct v2 v2_perp_mul(struct v2 a, f32 s) +/* Counter-clockwise perpendicular vector */ +INLINE struct v2 v2_perp_left(struct v2 a) +{ + return V2(a.y, -a.x); +} + +/* Clockwise perpendicular vector scaled by s */ +INLINE struct v2 v2_perp_right_mul(struct v2 a, f32 s) { return V2(s * -a.y, s * a.x); } +/* Counter-clockwise perpendicular vector scaled by s */ +INLINE struct v2 v2_perp_left_mul(struct v2 a, f32 s) +{ + return V2(s * a.y, s * -a.x); +} + INLINE struct v2 v2_perp_towards_dir(struct v2 v, struct v2 dir) { f32 wedge = v2_wedge(v, dir); - return v2_perp_mul(v, (wedge >= 0) - (wedge < 0)); + return v2_perp_right_mul(v, (wedge >= 0) - (wedge < 0)); } INLINE struct v2 v2_norm(struct v2 a) @@ -1085,7 +1100,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(dir); + struct v2 dir_perp = v2_perp_right(dir); struct v2 left = v2_mul(dir_perp, -width); struct v2 right = v2_mul(dir_perp, width); diff --git a/src/user.c b/src/user.c index 36f8bd8a..f2fd95a0 100644 --- a/src/user.c +++ b/src/user.c @@ -1029,7 +1029,7 @@ INTERNAL void user_update(void) (UNUSED)e1_quad; (UNUSED)e1_poly; -#if 1 +#if 0 /* Draw menkowski */ { @@ -1093,7 +1093,7 @@ INTERNAL void user_update(void) u32 color_third = RGBA_32_F(0, 0, 1, 0.75); struct gjk_simplex simplex = ent->simplex; - struct v2 simplex_points[] = { simplex.a.p, simplex.b.p, simplex.c.p }; + struct v2 simplex_points[] = { simplex.a, simplex.b, simplex.c }; for (u64 i = 0; i < ARRAY_COUNT(simplex_points); ++i) simplex_points[i] = xform_mul_v2(G.world_view, simplex_points[i]); struct v2_array simplex_array = { .count = simplex.len, .points = simplex_points }; @@ -1119,11 +1119,21 @@ INTERNAL void user_update(void) /* Draw contacts */ { f32 radius = 5; + + u32 colors[2] = { 0 }; + if (entity_has_prop(e0, ENTITY_PROP_PLAYER_CONTROLLED)) { + colors[0] = RGBA_32_F(1, 0, 0, 0.50); + colors[1] = RGBA_32_F(1, 1, 0, 0.50); + } else { + colors[0] = RGBA_32_F(1, 0, 1, 0.50); + colors[1] = RGBA_32_F(0, 1, 1, 0.50); + } + for (u32 i = 0; i < ent->num_contacts; ++i) { struct contact contact = ent->contacts[i]; /* Draw point */ { - 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); + u32 color = i < ARRAY_COUNT(colors) ? colors[i] : COLOR_RED; //struct v2 point = xform_mul_v2(e0_xf, contact.p0_local); //struct v2 point = contact.p0_initial_world; struct v2 point = contact.point; @@ -1140,17 +1150,22 @@ INTERNAL void user_update(void) struct v2 end = xform_mul_v2(G.world_view, v2_add(contact.point, v2_mul(v2_norm(ent->manifold_normal), len))); draw_solid_arrow_line(G.viewport_canvas, start, end, arrow_thickness, arrow_height, color); } - -#if 0 + /* Draw id */ { - 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, contact.p1_local); - //struct v2 point = contact.p1_initial_world; - struct v2 point = contact.point; - point = xform_mul_v2(G.world_view, point); - draw_solid_circle(G.viewport_canvas, point, radius, color, 10); + struct font *disp_font = font_load_async(STR("res/fonts/fixedsys.ttf"), 12.0f); + if (disp_font) { + f32 offset_px = -20; + u32 id = contact.id; + + struct string fmt = STR( + "id: 0x%F" + ); + struct string text = string_format(temp.arena, fmt, FMT_HEX(id)); + + + draw_text(G.viewport_canvas, disp_font, v2_add(v2_round(xform_mul_v2(G.world_view, contact.point)), V2(0, offset_px)), text); + } } -#endif } } #endif