diff --git a/src/config.h b/src/config.h index 2c2b3621..5de24b10 100644 --- a/src/config.h +++ b/src/config.h @@ -31,7 +31,9 @@ #define GAME_FPS 50.0 #define GAME_TIMESCALE 1.0 + #define GAME_PHYSICS_SUBSTEPS 4 +#define GAME_PHYSICS_ENABLE_WARM_STARTING 1 /* How many ticks back in time should the user blend between? * = * diff --git a/src/entity.h b/src/entity.h index 96ea2249..519d6b98 100644 --- a/src/entity.h +++ b/src/entity.h @@ -73,6 +73,13 @@ struct contact { f32 normal_impulse; /* Accumulated impulse along normal */ f32 tangent_impulse; /* Accumulated impulse along tangent */ + f32 inv_normal_mass; + f32 inv_tangent_mass; + f32 inv_m0; + f32 inv_m1; + f32 inv_i0; + f32 inv_i1; + b32 persisted; }; diff --git a/src/game.c b/src/game.c index 15a7c857..b82eafe1 100644 --- a/src/game.c +++ b/src/game.c @@ -20,7 +20,7 @@ GLOBAL struct { struct sprite_scope *sprite_frame_scope; /* TODO: Remove this (testing) */ - b32 box_spawned; + b32 first_spawn; /* Game thread input */ struct sys_mutex game_cmds_mutex; @@ -119,7 +119,7 @@ INTERNAL void activate_now(struct entity *ent) /* TODO: Remove this */ -INTERNAL void spawn_test_entities(void) +INTERNAL void spawn_test_entities(f32 offset) { struct entity *root = entity_from_handle(G.tick.entity_store, G.tick.entity_store->root); @@ -133,10 +133,13 @@ INTERNAL void spawn_test_entities(void) //struct v2 pos = V2(1.1230469346046448864129274625156, -1); /* Touching right side of box */ //struct v2 pos = V2(1.1230469346046448864129274625156 - 0.0001, -1); /* Touching right side of box */ //struct v2 pos = V2(0.374142020941, -0.246118023992); /* Touching glitch spot */ + + pos = v2_add(pos, V2(0, -offset)); + //struct v2 size = V2(1, 1); struct v2 size = V2(0.5, 0.5); - //f32 r = PI; - f32 r = PI / 4; + f32 r = PI; + //f32 r = PI / 4; //f32 r = PI / 3; //f32 r = 0.05; //f32 r = PI / 2; @@ -164,8 +167,8 @@ INTERNAL void spawn_test_entities(void) entity_enable_prop(e, ENTITY_PROP_PHYSICAL); e->mass_unscaled = 100; - e->inertia_unscaled = F32_INFINITY; - //e->inertia_unscaled = 25; + //e->inertia_unscaled = F32_INFINITY; + e->inertia_unscaled = 25; e->linear_ground_friction = 1000; e->angular_ground_friction = 100; @@ -197,14 +200,13 @@ INTERNAL void spawn_test_entities(void) } /* Box */ - if (!G.box_spawned) { - G.box_spawned = true; + if (!G.first_spawn) { //struct v2 pos = V2(0.5, -1); struct v2 pos = V2(0.5, 20); //struct v2 pos = V2(1, -1); //struct v2 size = V2(1, 1); - struct v2 size = V2(50, 50); + struct v2 size = V2(500, 50); //f32 rot = PI / 4; f32 rot = 0; struct entity *e = entity_alloc(root); @@ -226,7 +228,7 @@ INTERNAL void spawn_test_entities(void) } /* Camera */ - { + if (!G.first_spawn) { struct entity *e = entity_alloc(root); entity_set_xform(e, XFORM_IDENT); @@ -238,6 +240,8 @@ INTERNAL void spawn_test_entities(void) f32 height = (f32)DEFAULT_CAMERA_HEIGHT; e->camera_quad_xform = XFORM_TRS(.s = V2(width, height)); } + + G.first_spawn = true; } @@ -391,7 +395,10 @@ INTERNAL void create_contact_manifolds(void) manifold->solved = res.solved; } } - manifold->manifold_normal = res.normal; + struct v2 normal = res.normal; + struct v2 tangent = v2_perp(normal); + + manifold->manifold_normal = normal; /* Delete old contacts that are no longer present */ for (u32 i = 0; i < manifold->num_contacts; ++i) { @@ -428,7 +435,7 @@ INTERNAL void create_contact_manifolds(void) } if (contact) { /* Update existing */ -#if 1 +#if !GAME_PHYSICS_ENABLE_WARM_STARTING contact->normal_impulse = 0; contact->tangent_impulse = 0; #endif @@ -441,6 +448,54 @@ INTERNAL void create_contact_manifolds(void) contact->point_local_e0 = xform_invert_mul_v2(e0_xf, point); contact->point_local_e1 = xform_invert_mul_v2(e1_xf, point); contact->starting_separation = sep; + + { + 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; + + struct v2 vcp0 = v2_sub(point, e0_xf.og); + struct v2 vcp1 = v2_sub(point, e1_xf.og); + + + + + + + + + /* Normal mass */ + { + f32 vcp0_wedge = v2_wedge(vcp0, normal); + f32 vcp1_wedge = v2_wedge(vcp1, normal); + f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge); + //f32 k = ((inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge)) * manifold->num_contacts; + contact->inv_normal_mass = k > 0.0f ? 1.0f / k : 0.0f; + } + + /* Tangent mass */ + { + f32 vcp0_wedge = v2_wedge(vcp0, tangent); + f32 vcp1_wedge = v2_wedge(vcp1, tangent); + f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge); + //f32 k = ((inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge)) * manifold->num_contacts; + contact->inv_tangent_mass = k > 0.0f ? 1.0f / k : 0.0f; + } + + contact->inv_m0 = inv_m0; + contact->inv_m1 = inv_m1; + contact->inv_i0 = inv_i0; + contact->inv_i1 = inv_i1; + } + + } @@ -484,21 +539,11 @@ INTERNAL void warm_start_contacts(void) f32 w0 = e0->angular_velocity; f32 w1 = e1->angular_velocity; - 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; - /* Warm start */ + f32 inv_num_contacts = 1.f / num_contacts; struct v2 normal = manifold->manifold_normal; struct v2 tangent = v2_perp(normal); - for (u32 i = 0; i < manifold->num_contacts; ++i) { + for (u32 i = 0; i < num_contacts; ++i) { struct contact *contact = &manifold->contacts[i]; struct v2 p0 = xform_mul_v2(e0_xf, contact->point_local_e0); struct v2 p1 = xform_mul_v2(e1_xf, contact->point_local_e1); @@ -506,10 +551,14 @@ INTERNAL void warm_start_contacts(void) struct v2 vcp1 = v2_sub(p1, e1_xf.og); struct v2 impulse = v2_add(v2_mul(normal, contact->normal_impulse), v2_mul(tangent, contact->tangent_impulse)); - v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); - v1 = v2_add(v1, v2_mul(impulse, inv_m1)); - w0 -= v2_wedge(vcp0, impulse) * inv_i0; - w1 += v2_wedge(vcp1, impulse) * inv_i1; + + (UNUSED)inv_num_contacts; + //impulse = v2_mul(impulse, inv_num_contacts); + + v0 = v2_sub(v0, v2_mul(impulse, contact->inv_m0)); + v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1)); + w0 -= v2_wedge(vcp0, impulse) * contact->inv_i0; + w1 += v2_wedge(vcp1, impulse) * contact->inv_i1; } e0->linear_velocity = v0; @@ -568,13 +617,9 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) struct v2 v0 = e0->linear_velocity; struct v2 v1 = e1->linear_velocity; - //struct v2 v0_start = v0; - //struct v2 v1_start = v1; f32 w0 = e0->angular_velocity; f32 w1 = e1->angular_velocity; - //f32 w0_start = w0; - //f32 w1_start = w1; u32 num_contacts = manifold->num_contacts; f32 inv_num_contacts = 1.f / num_contacts; @@ -583,17 +628,6 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) struct xform e0_xf = entity_get_xform(e0); struct xform e1_xf = entity_get_xform(e1); - f32 scale0 = math_fabs(xform_get_determinant(e0_xf)); - f32 scale1 = math_fabs(xform_get_determinant(e1_xf)); - f32 m0 = e0->mass_unscaled * scale0; - f32 m1 = e1->mass_unscaled * scale1; - f32 i0 = e0->inertia_unscaled * scale0; - f32 i1 = e1->inertia_unscaled * scale1; - f32 inv_m0 = 1.f / m0; - f32 inv_m1 = 1.f / m1; - f32 inv_i0 = 1.f / i0; - f32 inv_i1 = 1.f / i1; - /* Normal impulse */ struct v2 normal = manifold->manifold_normal; for (u32 contact_index = 0; contact_index < num_contacts; ++contact_index) { @@ -603,52 +637,40 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) struct v2 vcp0 = v2_sub(p0, e0_xf.og); struct v2 vcp1 = v2_sub(p1, e1_xf.og); - f32 penetration = -(v2_dot(v2_sub(p1, p0), normal) + contact->starting_separation); + f32 separation = v2_dot(v2_sub(p1, p0), normal) + contact->starting_separation; f32 velocity_bias = 0.0f; f32 mass_scale = 1.0f; f32 impulse_scale = 0.0f; - if (penetration <= 0.0f) { - velocity_bias = penetration / dt; + if (separation > 0.0f) { + velocity_bias = separation / dt; } else if (apply_bias) { -#if 0 - /* Baumgarte Stabilization */ - const f32 bias_factor = 0.2f; - const f32 bias_slop = 0.005f; - velocity_bias = (bias_factor / dt) * max_f32(-separation - bias_slop, 0); -#else /* Soft constraint */ f32 contact_pushout_velocity = 3.0f; f32 contact_damping_ratio = 10.0f; - - f32 contact_hertz = 25.0f; - contact_hertz = min_f32(contact_hertz, 0.25f / dt); + f32 contact_hertz = (GAME_FPS * GAME_PHYSICS_SUBSTEPS) / 8.f; struct soft_result softness = make_soft(contact_hertz, contact_damping_ratio, dt); - velocity_bias = max_f32(softness.bias_rate * penetration, -contact_pushout_velocity); + velocity_bias = max_f32(softness.bias_rate * separation, -contact_pushout_velocity); mass_scale = softness.mass_scale; impulse_scale = softness.impulse_scale; -#endif } struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0)); struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1)); struct v2 vrel = v2_sub(vel0, vel1); - f32 vcp0_wedge = v2_wedge(vcp0, normal); - f32 vcp1_wedge = v2_wedge(vcp1, normal); - f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge); - k = k > 0.0f ? 1.0f / k : 0.0f; + f32 k = contact->inv_normal_mass; /* (to be applied along n) */ f32 vn = v2_dot(vrel, normal); - f32 j = ((k * mass_scale) * (vn + velocity_bias)) - (contact->normal_impulse * impulse_scale) ; + f32 j = ((k * mass_scale) * (vn - velocity_bias)) - (contact->normal_impulse * impulse_scale); //j *= inv_num_contacts; f32 old_impulse = contact->normal_impulse; @@ -657,13 +679,14 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) contact->normal_impulse = new_impulse; struct v2 impulse = v2_mul(normal, delta); - v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); - v1 = v2_add(v1, v2_mul(impulse, inv_m1)); - w0 -= v2_wedge(vcp0, impulse) * inv_i0; - w1 += v2_wedge(vcp1, impulse) * inv_i1; + v0 = v2_sub(v0, v2_mul(impulse, contact->inv_m0)); + v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1)); + w0 -= v2_wedge(vcp0, impulse) * contact->inv_i0; + w1 += v2_wedge(vcp1, impulse) * contact->inv_i1; } /* Tangent impulse */ +#if 1 struct v2 tangent = v2_perp(normal); for (u32 contact_index = 0; contact_index < num_contacts; ++contact_index) { struct contact *contact = &manifold->contacts[contact_index]; @@ -676,19 +699,16 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1)); struct v2 vrel = v2_sub(vel0, vel1); - f32 vcp0_wedge = v2_wedge(vcp0, tangent); - f32 vcp1_wedge = v2_wedge(vcp1, tangent); - f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge); - k = k > 0.0f ? 1.0f / k : 0.0f; + f32 k = contact->inv_tangent_mass; /* (to be applied along t) */ f32 vt = v2_dot(vrel, tangent); f32 j = vt * k; //j *= inv_num_contacts; - //f32 friction = 0.6f; + f32 friction = 0.6f; //f32 friction = 1.0f; - f32 friction = 999999.f; + //f32 friction = F32_INFINITY; f32 max_friction = friction * contact->normal_impulse; f32 old_impulse = contact->tangent_impulse; f32 new_impulse = clamp_f32(old_impulse + j, -max_friction, max_friction); @@ -696,11 +716,12 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) contact->tangent_impulse = new_impulse; struct v2 impulse = v2_mul(tangent, delta); - v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); - v1 = v2_add(v1, v2_mul(impulse, inv_m1)); - w0 -= v2_wedge(vcp0, impulse) * inv_i0; - w1 += v2_wedge(vcp1, impulse) * inv_i1; + v0 = v2_sub(v0, v2_mul(impulse, contact->inv_m0)); + v1 = v2_add(v1, v2_mul(impulse, contact->inv_m1)); + w0 -= v2_wedge(vcp0, impulse) * contact->inv_i0; + w1 += v2_wedge(vcp1, impulse) * contact->inv_i1; } +#endif e0->linear_velocity = v0; e0->angular_velocity = w0; @@ -813,7 +834,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) static b32 run = 0; if (!run) { run = 1; - spawn_test_entities(); + spawn_test_entities(0); } } @@ -830,14 +851,20 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) { logf_info("Clearing level"); entity_store_release_all_entities(store); - G.box_spawned = false; + G.first_spawn = false; } break; /* Spawn test */ case GAME_CMD_KIND_SPAWN_TEST: { logf_info("Spawning (test)"); - spawn_test_entities(); +#if 1 + for (u32 i = 0; i < 50; ++i) { + spawn_test_entities(i); + } +#else + spawn_test_entities(0); +#endif } break; default: break; @@ -1405,18 +1432,18 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) { create_contact_manifolds(); - warm_start_contacts(); (UNUSED)create_contact_manifolds; (UNUSED)solve_collisions; (UNUSED)integrate_positions; (UNUSED)warm_start_contacts; - f32 substep_dt = dt / GAME_PHYSICS_SUBSTEPS; for (u32 i = 0; i < GAME_PHYSICS_SUBSTEPS; ++i) { #if 1 - //warm_start_contacts(); +#if GAME_PHYSICS_ENABLE_WARM_STARTING + warm_start_contacts(); +#endif solve_collisions(substep_dt, true); integrate_positions(substep_dt); solve_collisions(substep_dt, false); /* Relaxation */ diff --git a/src/gjk.c b/src/gjk.c index a4a4d0f8..a6ffbd6f 100644 --- a/src/gjk.c +++ b/src/gjk.c @@ -164,6 +164,7 @@ struct v2_array cloud(struct arena *arena, struct v2_array poly0, struct v2_arra +#if 0 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 */ @@ -171,8 +172,9 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru /* TODO: Set all epsilons used in this function to 0.005 */ - struct gjk_simplex s = ZI; b32 colliding = false; + + struct gjk_simplex s = ZI; struct v2 *proto = NULL; u32 proto_count = 0; @@ -193,8 +195,6 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru * ========================== */ { - - /* 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); @@ -253,13 +253,9 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru if (colliding) { /* ========================== * - * Epa (to find collision normal) + * Epa (to find collision normal from inside shape) * ========================== */ - /* If penetration is less than this, collision will return false (to - * prevent wacky extrapolated normals at small penetration values) */ - const f32 min_pen_len = 0.0001f; - proto = arena_dry_push(scratch.arena, struct v2); proto_count = 0; { @@ -331,6 +327,7 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru } } if (!unique) { +#if 0 f32 len = v2_len(normal); if (len < min_pen_len) { colliding = false; @@ -338,6 +335,18 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru normal = v2_mul(normal, 1.f / len); } break; +#else + /* Re-do normal calculation in high precision (for accuracy with small separation values) */ + struct v2_64 ps = V2_64_FROM_V2(proto[pen_ps_index]); + struct v2_64 pe = V2_64_FROM_V2(proto[pen_pe_index]); + struct v2_64 vse = v2_sub64(pe, ps); + struct v2_64 vso = v2_neg64(ps); + struct v2_64 vsd = v2_mul64(vse, (v2_dot64(vso, vse) / v2_len_sq64(vse))); + struct v2_64 normal64 = v2_add64(ps, vsd); + normal64 = v2_norm64(normal64); + normal = V2(normal64.x, normal64.y); + break; +#endif } } @@ -516,6 +525,7 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru f32 a_sep = v2_dot(va0a1_clipped, normal); f32 b_sep = v2_dot(vb0b1_clipped, normal); +#if 0 if (a_sep < 0) { struct gjk_contact_point *point = &points[num_points++]; point->id = id_a0 | (id_a1 << 4); @@ -528,6 +538,21 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru point->separation = b_sep; point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f)); } +#else + f32 sticky = 0.05f; + if (a_sep < sticky) { + 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 < sticky) { + 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)); + } +#endif } } @@ -563,6 +588,551 @@ abort: return res; } +#else + +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: Parameterize */ + const f32 tolerance = 0.001f; + const f32 min_unique_pt_dist_sq = 0.0001f * 0.0001f; + + b32 colliding = false; + + struct gjk_simplex s = ZI; + 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 + * + * Determine encapsulating simplex if colliding, or closest edge / point to + * origin on simplex (for check if shape distances are within tolerance) + * ========================== */ + { + /* 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])); + s.len = 1; + + f32 dist_test = 0; + + struct v2 removed_a = ZI; + struct v2 removed_b = ZI; + u32 num_removed = 0; + + while (!colliding) { + if (s.len == 1) { + /* Second point is support point towards origin */ + dir = v2_neg(s.a); + + DBGSTEP; + m = menkowski_point(shape0, shape1, dir); + dist_test = v2_len_sq(v2_sub(m, s.a)); + if (dist_test < min_unique_pt_dist_sq) { + 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, s.a), v2_neg(s.a)); + } + + { + DBGSTEP; + m = menkowski_point(shape0, shape1, dir); + /* Check that new point is far enough away from existing points */ + if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) break; + if (v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq) break; + if (num_removed >= 1) { + if (v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) break; + if (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq) break; + } + s.c = s.b; + s.b = s.a; + s.a = m; + s.len = 3; + } + + /* Determine voronoi region of the simplex in which the origin lies */ + i32 voronoi_mask = 0; + struct v2 vab = v2_sub(s.b, s.a); + struct v2 vac = v2_sub(s.c, s.a); + struct v2 vbc = v2_sub(s.c, s.b); + 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)) > 0) << 0; /* Regions ab, a, and b*/ + voronoi_mask |= (v2_dot(rac_dir, v2_neg(s.a)) > 0) << 1; /* Regions ac, a, and c */ + voronoi_mask |= (v2_dot(rbc_dir, v2_neg(s.b)) > 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 */ + colliding = true; + } break; + case 1: + { /* Region ab, remove c */ + num_removed = 1; + removed_a = s.c; + s.len = 2; + dir = rab_dir; /* Next third point is in direction of region ab */ + + } break; + case 2: + { /* Region ac, remove b */ + num_removed = 1; + removed_a = s.b; + s.len = 2; + s.b = s.c; + dir = rac_dir; /* Next third point is in direction of region ac */ + } break; + case 4: + { /* Region bc, remove a */ + num_removed = 1; + removed_a = s.a; + s.len = 2; + s.a = s.b; + s.b = s.c; + dir = rbc_dir; /* Next third point is in direction of region bc */ + } break; + case 3: + { /* Region a, remove bc */ + num_removed = 2; + removed_a = s.b; + removed_b = s.c; + s.len = 1; + } break; + case 5: + { /* Region b, remove ac */ + num_removed = 2; + removed_a = s.a; + removed_b = s.c; + s.len = 1; + s.a = s.b; + } break; + case 6: + { /* Region c, remove ab */ + num_removed = 2; + removed_a = s.a; + removed_b = s.b; + s.len = 1; + s.a = s.c; + } break; + } + } + } + + if (colliding) { + /* ========================== * + * Epa (to find collision normal from inside shape) + * ========================== */ + + 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 (colliding) { + 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); + + struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_len_sq(vse))); + 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; + } + } + + /* 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 vab = v2_sub(b, a); + struct v2 vna = v2_sub(a, n); + dir = v2_perp_towards_dir(vab, vna); + } + m = menkowski_point(shape0, shape1, dir); + + /* Check unique */ + /* TODO: Better */ + { + b32 unique = true; + for (u32 i = 0; i < proto_count; ++i) { + struct v2 p = proto[i]; + if (v2_len_sq(v2_sub(p, m)) < min_unique_pt_dist_sq) { + unique = false; + break; + } + } + if (!unique) { +#if 0 + f32 len = v2_len(normal); + if (len < min_pen_len) { + colliding = false; + } else { + normal = v2_mul(normal, 1.f / len); + } + break; +#elif 0 + /* Re-do normal calculation in high precision (for accuracy with small separation values) */ + struct v2_64 ps = V2_64_FROM_V2(proto[pen_ps_index]); + struct v2_64 pe = V2_64_FROM_V2(proto[pen_pe_index]); + struct v2_64 vse = v2_sub64(pe, ps); + struct v2_64 vso = v2_neg64(ps); + struct v2_64 vsd = v2_mul64(vse, (v2_dot64(vso, vse) / v2_len_sq64(vse))); + struct v2_64 normal64 = v2_add64(ps, vsd); + normal64 = v2_norm64(normal64); + normal = V2(normal64.x, normal64.y); + break; +#else + normal = v2_norm(dir); + break; +#endif + } + } + + /* 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; + } + } else { + if (s.len == 1) { + /* TODO? */ + } else { + ASSERT(s.len == 2); +#if 0 + struct v2 vab = v2_sub(s.b, s.a); + struct v2 vao = v2_neg(s.a); + f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); + struct v2 p = v2_add(s.a, v2_mul(vab, ratio)); + if (v2_len_sq(p) <= (tolerance * tolerance)) { + normal = v2_norm(v2_perp_towards_dir(vab, vao)); + colliding = true; + } +#elif 0 + struct v2 vab = v2_sub(s.b, s.a); + struct v2 vao = v2_neg(s.a); + f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); + struct v2 p = v2_add(s.a, v2_mul(vab, ratio)); + if (v2_len_sq(p) <= (tolerance * tolerance)) { + struct v2_64 a64 = V2_64_FROM_V2(s.a); + struct v2_64 b64 = V2_64_FROM_V2(s.b); + struct v2_64 vab64 = v2_sub64(b64, a64); + + //struct v2_64 vao64 = v2_neg64(a64); + //struct v2_64 normal64 = v2_norm64(v2_perp_towards_dir64(vab64, vao64)); + + struct v2_64 normal64 = v2_norm64(v2_perp_towards_dir64(vab64, V2_64_FROM_V2(dir))); + + normal = V2(normal64.x, normal64.y); + + colliding = true; + } +#elif 0 + struct v2 vab = v2_sub(s.b, s.a); + struct v2 vao = v2_neg(s.a); + f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); + struct v2 p = v2_add(s.a, v2_mul(vab, ratio)); + if (v2_len_sq(p) <= (tolerance * tolerance)) { + normal = v2_norm(v2_perp_towards_dir(vab, dir)); + colliding = true; + } +#else + struct v2 vab = v2_sub(s.b, s.a); + struct v2 vao = v2_neg(s.a); + f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); + struct v2 p = v2_add(s.a, v2_mul(vab, ratio)); + if (v2_len_sq(p) <= (tolerance * tolerance)) { +#if 0 + normal = v2_norm(v2_perp_towards_dir(vab, dir)); +#else + normal = v2_norm(dir); +#endif + colliding = true; + } +#endif + } + } + + if (colliding) { + /* ========================== * + * Clipping + * ========================== */ + + /* FIXME: Limit max vertices in shape structure to at least < 16 for id generation to be correct */ + ASSERT(shape0.count <= 16); + ASSERT(shape1.count <= 16); + + DBGSTEP; + { + const f32 wedge_epsilon = 0.001f; + + /* 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 0 + 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)); + } +#else + if (a_sep < tolerance) { + 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 < tolerance) { + 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)); + } +#endif + } + } + + 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; +} +#endif + diff --git a/src/intrinsics.h b/src/intrinsics.h index e0175547..fd4c8c01 100644 --- a/src/intrinsics.h +++ b/src/intrinsics.h @@ -14,6 +14,13 @@ INLINE f32 ix_sqrt_f32(f32 f) return _mm_cvtss_f32(n); } +INLINE f64 ix_sqrt_f64(f64 f) +{ + __m128d n = _mm_set_sd(f); + n = _mm_sqrt_sd(_mm_setzero_pd(), n); + return _mm_cvtsd_f64(n); +} + INLINE f32 ix_rsqrt_f32(f32 f) { __m128 n = _mm_set_ss(f); diff --git a/src/math.h b/src/math.h index eeee9873..c1d039c7 100644 --- a/src/math.h +++ b/src/math.h @@ -386,6 +386,11 @@ INLINE f32 math_sqrt(f32 x) return ix_sqrt_f32(x); } +INLINE f32 math_sqrt64(f32 x) +{ + return ix_sqrt_f64(x); +} + INLINE f32 math_rsqrt(f32 x) { return ix_rsqrt_f32(x); diff --git a/src/user.c b/src/user.c index 2ed10179..2f08a530 100644 --- a/src/user.c +++ b/src/user.c @@ -936,6 +936,7 @@ INTERNAL void user_update(void) debug_draw_xform(xf); } +#if 0 /* Draw focus arrow */ if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) { struct sprite_sheet *sheet = sprite_sheet_from_tag_async(sprite_frame_scope, ent->sprite); @@ -946,6 +947,7 @@ INTERNAL void user_update(void) end = xform_mul_v2(G.world_view, end); draw_solid_arrow_line(G.viewport_canvas, start, end, 3, 10, RGBA_32_F(1, 1, 1, 0.5)); } +#endif /* Draw slices */ if (!sprite_tag_is_nil(ent->sprite)) { @@ -1030,7 +1032,7 @@ INTERNAL void user_update(void) (UNUSED)e1_quad; (UNUSED)e1_poly; -#if 1 +#if 0 /* Draw menkowski */ { @@ -1159,7 +1161,7 @@ INTERNAL void user_update(void) FMT_HEX(contact.id), FMT_FLOAT_P(contact.normal_impulse, 3), FMT_FLOAT_P(contact.tangent_impulse, 3), - FMT_FLOAT_P(contact.starting_separation, 3)); + FMT_FLOAT_P(contact.starting_separation, 6)); draw_text(G.viewport_canvas, disp_font, v2_add(v2_round(xform_mul_v2(G.world_view, point)), V2(0, offset_px)), text);