diff --git a/.natvis b/.natvis index a4e2544e..da9af172 100644 --- a/.natvis +++ b/.natvis @@ -1,6 +1,10 @@ + + ({x}, {y}) + + ({len}) {text, [len] s} text, [len] s diff --git a/src/config.h b/src/config.h index 5de24b10..3414002d 100644 --- a/src/config.h +++ b/src/config.h @@ -35,12 +35,15 @@ #define GAME_PHYSICS_SUBSTEPS 4 #define GAME_PHYSICS_ENABLE_WARM_STARTING 1 +#define GAME_MAX_LINEAR_VELOCITY 100 +#define GAME_MAX_ANGULAR_VELOCITY ((2 * PI) * 5) + /* How many ticks back in time should the user blend between? * = * * E.g: At 1.5, the user thread will render 75ms back in time (if game thread runs at 50FPS) */ #define USER_INTERP_OFFSET_TICK_RATIO 1.1 -#define USER_INTERP_ENABLED 0 +#define USER_INTERP_ENABLED 1 /* ========================== * * Settings diff --git a/src/entity.c b/src/entity.c index be7cd695..49e3da07 100644 --- a/src/entity.c +++ b/src/entity.c @@ -279,6 +279,9 @@ struct xform entity_get_local_xform(struct entity *ent) void entity_set_xform(struct entity *ent, struct xform xf) { + /* TODO: Remove this (debugging) */ + if (v2_len_sq(xf.bx) == 0 || v2_len_sq(xf.by) == 0) DEBUGBREAK; + if (!xform_eq(xf, ent->cached_global_xform)) { struct entity_store *store = entity_get_store(ent); /* Update local xform */ diff --git a/src/game.c b/src/game.c index b82eafe1..bdf4ad2f 100644 --- a/src/game.c +++ b/src/game.c @@ -123,6 +123,10 @@ INTERNAL void spawn_test_entities(f32 offset) { struct entity *root = entity_from_handle(G.tick.entity_store, G.tick.entity_store->root); + //const f32 offset_all = -20000; + //const f32 offset_all = -5000; + const f32 offset_all = 0; + /* Player */ struct entity *player_ent; { @@ -134,12 +138,13 @@ INTERNAL void spawn_test_entities(f32 offset) //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)); + pos = v2_add(pos, V2(0, offset)); + pos = v2_add(pos, V2(0, offset_all)); //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; @@ -167,8 +172,8 @@ INTERNAL void spawn_test_entities(f32 offset) 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; @@ -211,6 +216,8 @@ INTERNAL void spawn_test_entities(f32 offset) f32 rot = 0; struct entity *e = entity_alloc(root); + pos = v2_add(pos, V2(0, offset_all)); + e->sprite = sprite_tag_from_path(STR("res/graphics/box.ase")); entity_enable_prop(e, ENTITY_PROP_PHYSICAL); @@ -466,6 +473,22 @@ INTERNAL void create_contact_manifolds(void) + /* TODO: Remove this (debugging) */ + if (F32_IS_NAN(scale0) || + F32_IS_NAN(scale1) || + F32_IS_NAN(m0) || + F32_IS_NAN(m1) || + F32_IS_NAN(i0) || + F32_IS_NAN(i1) || + F32_IS_NAN(inv_m0) || + F32_IS_NAN(inv_m1) || + F32_IS_NAN(inv_i0) || + F32_IS_NAN(inv_i1)) { + DEBUGBREAK; + } + + + @@ -559,6 +582,9 @@ INTERNAL void warm_start_contacts(void) 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; + + /* TODO: Remove this (debugging) */ + if (F32_IS_NAN(v0.x) || F32_IS_NAN(v0.y) || F32_IS_NAN(v1.x) || F32_IS_NAN(v1.y) || F32_IS_NAN(w0) || F32_IS_NAN(w1)) DEBUGBREAK; } e0->linear_velocity = v0; @@ -650,8 +676,6 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) /* Soft constraint */ f32 contact_pushout_velocity = 3.0f; - - f32 contact_damping_ratio = 10.0f; f32 contact_hertz = (GAME_FPS * GAME_PHYSICS_SUBSTEPS) / 8.f; @@ -678,15 +702,40 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) f32 delta = new_impulse - old_impulse; contact->normal_impulse = new_impulse; + + /* TODO: Remove this (debugging) */ + //if (math_fabs(delta) > 10) DEBUGBREAK; + + + + + struct v2 impulse = v2_mul(normal, delta); 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; + + +#if 1 + v0 = v2_clamp_len(v0, GAME_MAX_LINEAR_VELOCITY); + v1 = v2_clamp_len(v1, GAME_MAX_LINEAR_VELOCITY); + w0 = clamp_f32(w0, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY); + w1 = clamp_f32(w1, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY); +#endif + + + + + + /* TODO: Remove this (debugging) */ + if ((math_fabs(v0.x) > 10000000) || (math_fabs(v0.y) > 10000000) || (math_fabs(v1.x) > 10000000) || (math_fabs(v1.y) > 10000000) || (math_fabs(w0) > 10000000) || (math_fabs(w1) > 10000000)) DEBUGBREAK; + + /* TODO: Remove this (debugging) */ + if (F32_IS_NAN(v0.x) || F32_IS_NAN(v0.y) || F32_IS_NAN(v1.x) || F32_IS_NAN(v1.y) || F32_IS_NAN(w0) || F32_IS_NAN(w1)) DEBUGBREAK; } /* 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]; @@ -706,8 +755,8 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) f32 j = vt * k; //j *= inv_num_contacts; - f32 friction = 0.6f; - //f32 friction = 1.0f; + //f32 friction = 0.6f; + f32 friction = 1.0f; //f32 friction = F32_INFINITY; f32 max_friction = friction * contact->normal_impulse; f32 old_impulse = contact->tangent_impulse; @@ -720,8 +769,13 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) 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; + + /* TODO: Remove this (debugging) */ + if (F32_IS_NAN(v0.x) || F32_IS_NAN(v0.y) || F32_IS_NAN(v1.x) || F32_IS_NAN(v1.y) || F32_IS_NAN(w0) || F32_IS_NAN(w1)) DEBUGBREAK; } -#endif + + /* TODO: Remove this (debugging) */ + if (F32_IS_NAN(v0.x) || F32_IS_NAN(v0.y) || F32_IS_NAN(v1.x) || F32_IS_NAN(v1.y) || F32_IS_NAN(w0) || F32_IS_NAN(w1)) DEBUGBREAK; e0->linear_velocity = v0; e0->angular_velocity = w0; @@ -737,9 +791,41 @@ INTERNAL void solve_collisions(f32 dt, b32 apply_bias) } +INTERNAL void integrate_velocities_from_forces(f32 dt) +{ + struct entity_store *store = G.tick.entity_store; + for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { + struct entity *ent = &store->entities[entity_index]; + if (!entity_is_valid_and_active(ent)) continue; + if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue; + struct xform xf = entity_get_xform(ent); + f32 det_abs = math_fabs(xform_get_determinant(xf)); + f32 mass = ent->mass_unscaled * det_abs; + f32 inertia = ent->inertia_unscaled * det_abs; -INTERNAL void integrate_positions(f32 dt) + /* Determine force & torque acceleration */ + struct v2 force_accel = v2_mul(v2_div(ent->force, mass), dt); + f32 torque_accel = (ent->torque / inertia) * dt; + + /* Integrate */ + ent->linear_velocity = v2_add(ent->linear_velocity, force_accel); + ent->angular_velocity += torque_accel; + + /* Clamp velocities */ + ent->linear_velocity = v2_clamp_len(ent->linear_velocity, GAME_MAX_LINEAR_VELOCITY); + ent->angular_velocity = clamp_f32(ent->angular_velocity, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY); + + /* TODO: Remove this (debugging) */ + if (F32_IS_NAN(ent->linear_velocity.x) || F32_IS_NAN(ent->linear_velocity.y) || F32_IS_NAN(ent->angular_velocity)) DEBUGBREAK; + + /* Reset forces */ + ent->force = V2(0, 0); + ent->torque = 0; + } +} + +INTERNAL void integrate_positions_from_velocities(f32 dt) { struct entity_store *store = G.tick.entity_store; @@ -748,12 +834,21 @@ INTERNAL void integrate_positions(f32 dt) if (!entity_is_valid_and_active(ent)) continue; if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue; + /* Clamp velocities */ + ent->linear_velocity = v2_clamp_len(ent->linear_velocity, GAME_MAX_LINEAR_VELOCITY); + ent->angular_velocity = clamp_f32(ent->angular_velocity, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY); + struct xform xf = entity_get_xform(ent); struct v2 tick_linear_velocity = v2_mul(ent->linear_velocity, dt); f32 tick_angular_velocity = ent->angular_velocity * dt; xf.og = v2_add(xf.og, tick_linear_velocity); xf = xform_rotated(xf, tick_angular_velocity); entity_set_xform(ent, xf); + + /* TODO: Remove this (debugging) */ + if (F32_IS_NAN(xf.bx.x) || F32_IS_NAN(xf.bx.y) || F32_IS_NAN(xf.by.x) || F32_IS_NAN(xf.by.y) || F32_IS_NAN(xf.og.x) || F32_IS_NAN(xf.og.y)) { + DEBUGBREAK; + } } } @@ -858,9 +953,9 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) case GAME_CMD_KIND_SPAWN_TEST: { logf_info("Spawning (test)"); -#if 1 +#if 0 for (u32 i = 0; i < 50; ++i) { - spawn_test_entities(i); + spawn_test_entities(-i); } #else spawn_test_entities(0); @@ -1399,43 +1494,17 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) } #endif - /* ========================== * - * Integrate velocities from forces - * ========================== */ - - for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { - struct entity *ent = &store->entities[entity_index]; - if (!entity_is_valid_and_active(ent)) continue; - if (!entity_has_prop(ent, ENTITY_PROP_PHYSICAL)) continue; - - struct xform xf = entity_get_xform(ent); - f32 det_abs = math_fabs(xform_get_determinant(xf)); - f32 mass = ent->mass_unscaled * det_abs; - f32 inertia = ent->inertia_unscaled * det_abs; - - /* Determine force & torque acceleration */ - struct v2 force_accel = v2_mul(v2_div(ent->force, mass), dt); - f32 torque_accel = (ent->torque / inertia) * dt; - - /* Integrate */ - ent->linear_velocity = v2_add(ent->linear_velocity, force_accel); - ent->angular_velocity += torque_accel; - - /* Reset forces */ - ent->force = V2(0, 0); - ent->torque = 0; - } - /* ========================== * * Physics * ========================== */ { + integrate_velocities_from_forces(dt); create_contact_manifolds(); (UNUSED)create_contact_manifolds; (UNUSED)solve_collisions; - (UNUSED)integrate_positions; + (UNUSED)integrate_positions_from_velocities; (UNUSED)warm_start_contacts; f32 substep_dt = dt / GAME_PHYSICS_SUBSTEPS; @@ -1445,12 +1514,12 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) warm_start_contacts(); #endif solve_collisions(substep_dt, true); - integrate_positions(substep_dt); + integrate_positions_from_velocities(substep_dt); solve_collisions(substep_dt, false); /* Relaxation */ #else //solve_collisions(substep_dt, true); solve_collisions(substep_dt, false); - integrate_positions(substep_dt); + integrate_positions_from_velocities(substep_dt); #endif } } diff --git a/src/gjk.c b/src/gjk.c index a6ffbd6f..6879ee98 100644 --- a/src/gjk.c +++ b/src/gjk.c @@ -597,9 +597,10 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru /* TODO: Parameterize */ const f32 tolerance = 0.001f; - const f32 min_unique_pt_dist_sq = 0.0001f * 0.0001f; + const f32 min_unique_pt_dist_sq = 0.001f * 0.001f; b32 colliding = false; + b32 simplex_is_closest_edge = false; struct gjk_simplex s = ZI; struct v2 *proto = NULL; @@ -609,7 +610,6 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru struct gjk_contact_point points[2] = ZI; u32 num_points = 0; - /* Used by GJK & EPA */ struct v2 dir = ZI; struct v2 m = ZI; @@ -628,23 +628,19 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru 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) { + b32 done = false; + while (!done) { 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; - } + /* Check that new point is far enough away from existing point */ + if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) break; s.b = s.a; s.a = m; s.len = 2; @@ -657,11 +653,17 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru 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; + if ((v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) || + (v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq) || + ((num_removed >= 1) && ( + (v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) || + (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq) + )) || + (math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_sub(m, s.a))) < min_unique_pt_dist_sq) + ) { + simplex_is_closest_edge = true; + done = true; + break; } s.c = s.b; s.b = s.a; @@ -682,9 +684,10 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru 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: + case 0: { /* No region, must be in simplex */ colliding = true; + done = true; } break; case 1: { /* Region ab, remove c */ @@ -692,7 +695,6 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru 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 */ @@ -734,10 +736,21 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru s.len = 1; s.a = s.c; } break; + default: + { + /* Unknown region (should be impossible) */ + ASSERT(false); + done = true; + } break; } } } + /* TODO: Remove this (debugging) */ + if (F32_IS_NAN(s.a.x) || F32_IS_NAN(s.a.y) || F32_IS_NAN(s.b.x) || F32_IS_NAN(s.b.y) || F32_IS_NAN(s.c.x) || F32_IS_NAN(s.c.y)) { + DEBUGBREAK; + } + if (colliding) { /* ========================== * * Epa (to find collision normal from inside shape) @@ -849,62 +862,23 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru } proto[pen_pe_index] = m; } - } else { + } else if (simplex_is_closest_edge) { 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 +#if 1 normal = v2_norm(v2_perp_towards_dir(vab, dir)); #else normal = v2_norm(dir); #endif colliding = true; } -#endif } } @@ -1084,6 +1058,9 @@ struct gjk_contact_points_result gjk_contact_points(struct v2_array shape0, stru point->point = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f)); } #else + /* TODO: Remove this (debugging) */ + if (a_sep < -3 || b_sep < -3) DEBUGBREAK; + if (a_sep < tolerance) { struct gjk_contact_point *point = &points[num_points++]; point->id = id_a0 | (id_a1 << 4); diff --git a/src/math.h b/src/math.h index c1d039c7..39ed998c 100644 --- a/src/math.h +++ b/src/math.h @@ -673,10 +673,21 @@ INLINE struct v2 v2_perp_towards_dir(struct v2 v, struct v2 dir) INLINE struct v2 v2_norm(struct v2 a) { - f32 l = a.x * a.x + a.y * a.y; - if (l != 0) { + f32 l_sq = a.x * a.x + a.y * a.y; + if (l_sq != 0) { /* TODO: Benchmark vs math_rqsrt(l) */ - f32 denom = 1.f / math_sqrt(l); + f32 denom = 1.f / math_sqrt(l_sq); + a.x *= denom; + a.y *= denom; + } + return a; +} + +INLINE struct v2 v2_clamp_len(struct v2 a, f32 max) +{ + f32 l_sq = a.x * a.x + a.y * a.y; + if (l_sq > (max * max) && l_sq != 0) { + f32 denom = max / math_sqrt(l_sq); a.x *= denom; a.y *= denom; } @@ -887,6 +898,15 @@ INLINE struct xform xform_rotated(struct xform xf, f32 angle) res.bx.y = xf.bx.y * c + xf.by.y * s; res.by.x = xf.bx.x * -s + xf.by.x * c; res.by.y = xf.bx.y * -s + xf.by.y * c; + + + + /* TODO: Remove this (debugging) */ + if (v2_len_sq(res.bx) == 0 || v2_len_sq(res.by) == 0) DEBUGBREAK; + + + + return res; }