From 59b48694e8e922c65d748e565c639932674b293c Mon Sep 17 00:00:00 2001 From: jacob Date: Wed, 23 Oct 2024 15:19:18 -0500 Subject: [PATCH] fix aim angle diff check --- src/entity.h | 11 ++++---- src/game.c | 75 ++++++++++++++++++++++++++++++---------------------- src/math.h | 11 ++++++-- 3 files changed, 58 insertions(+), 39 deletions(-) diff --git a/src/entity.h b/src/entity.h index 5f163e46..df9fa71a 100644 --- a/src/entity.h +++ b/src/entity.h @@ -75,12 +75,6 @@ struct contact { f32 inv_normal_mass; f32 inv_tangent_mass; - f32 inv_m0; - f32 inv_m1; - f32 inv_i0; - f32 inv_i1; - - b32 persisted; /* Debugging */ struct v2 dbg_pt; @@ -139,6 +133,11 @@ struct entity { struct contact contacts[2]; u32 num_contacts; + f32 manifold_inv_m0; + f32 manifold_inv_m1; + f32 manifold_inv_i0; + f32 manifold_inv_i1; + /* TODO: Remove this (debugging) */ struct collider_collision_points_result res; struct xform dbg_xf0; diff --git a/src/game.c b/src/game.c index e7fe1641..e498c112 100644 --- a/src/game.c +++ b/src/game.c @@ -154,12 +154,12 @@ INTERNAL void spawn_test_entities(f32 offset) //struct v2 size = V2(1.0, 1.0); //struct v2 size = V2(1.5, 1.5); //f32 r = PI; - //f32 r = PI / 4; + f32 r = PI / 4; //f32 r = PI / 3; //f32 r = 0.05; //f32 r = PI / 2; //f32 r = PI; - f32 r = 0; + //f32 r = 0; struct xform xf = XFORM_TRS(.t = pos, .r = r, .s = size); //xf.bx.y = -1.f; @@ -473,6 +473,25 @@ INTERNAL void generate_contacts(void) struct v2 normal = res.normal; struct v2 tangent = v2_perp(normal); + /* TODO: Cache this */ + /* Prepare manifold masses */ + f32 inv_m0; + f32 inv_m1; + f32 inv_i0; + f32 inv_i1; + { + f32 scale0 = math_fabs(xform_get_determinant(e0_xf)); + f32 scale1 = math_fabs(xform_get_determinant(e1_xf)); + inv_m0 = 1.f / (e0->mass_unscaled * scale0); + inv_m1 = 1.f / (e1->mass_unscaled * scale1); + inv_i0 = 1.f / (e0->inertia_unscaled * scale0); + inv_i1 = 1.f / (e1->inertia_unscaled * scale1); + manifold->manifold_inv_m0 = inv_m0; + manifold->manifold_inv_m1 = inv_m1; + manifold->manifold_inv_i0 = inv_i0; + manifold->manifold_inv_i1 = inv_i1; + } + /* Delete old contacts that are no longer present */ for (u32 i = 0; i < manifold->num_contacts; ++i) { struct contact *old = &manifold->contacts[i]; @@ -530,17 +549,6 @@ INTERNAL void generate_contacts(void) #endif { - 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); @@ -559,11 +567,6 @@ INTERNAL void generate_contacts(void) f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge); 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; } @@ -601,6 +604,11 @@ INTERNAL void warm_start_contacts(void) struct xform e0_xf = entity_get_xform(e0); struct xform e1_xf = entity_get_xform(e1); + f32 inv_m0 = manifold->manifold_inv_m0; + f32 inv_m1 = manifold->manifold_inv_m1; + f32 inv_i0 = manifold->manifold_inv_i0; + f32 inv_i1 = manifold->manifold_inv_i1; + struct v2 v0 = e0->linear_velocity; struct v2 v1 = e1->linear_velocity; f32 w0 = e0->angular_velocity; @@ -620,10 +628,10 @@ INTERNAL void warm_start_contacts(void) struct v2 impulse = v2_add(v2_mul(normal, contact->normal_impulse), v2_mul(tangent, contact->tangent_impulse)); 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; + 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; } e0->linear_velocity = v0; @@ -679,6 +687,11 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) struct xform e0_xf = entity_get_xform(e0); struct xform e1_xf = entity_get_xform(e1); + f32 inv_m0 = manifold->manifold_inv_m0; + f32 inv_m1 = manifold->manifold_inv_m1; + f32 inv_i0 = manifold->manifold_inv_i0; + f32 inv_i1 = manifold->manifold_inv_i1; + /* Normal impulse */ struct v2 normal = manifold->manifold_normal; for (u32 contact_index = 0; contact_index < num_contacts; ++contact_index) { @@ -728,10 +741,10 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) contact->normal_impulse = new_impulse; 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; + 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; } /* Tangent impulse */ @@ -763,10 +776,10 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) contact->tangent_impulse = new_impulse; struct v2 impulse = v2_mul(tangent, 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; + 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; } e0->linear_velocity = v0; @@ -1364,7 +1377,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) if (!F32_IS_NAN(final_xf_angle)) { const f32 angle_error_allowed = 0.001; - f32 diff = final_xf_angle - old_angle; + f32 diff = math_diff_angle(final_xf_angle, old_angle); if (math_fabs(diff) > angle_error_allowed) { xf = xform_basis_rotated_world(xf, diff); } diff --git a/src/math.h b/src/math.h index bdfe5de8..eecdaf2f 100644 --- a/src/math.h +++ b/src/math.h @@ -579,6 +579,14 @@ INLINE f32 math_acos(f32 x) return (PI / 2.0f) - math_atan2(x, math_sqrt(1.0f - (x * x))); } +/* Returns wrapped difference between angles. + * E.G. diff(PI, -PI) = 0 */ +INLINE f32 math_diff_angle(f32 a, f32 b) +{ + f32 diff = math_fmod(a - b, TAU); + return math_fmod(2.0f * diff, TAU) - diff; +} + /* ========================== * * Lerp * ========================== */ @@ -594,8 +602,7 @@ INLINE f64 math_lerp64(f64 val0, f64 val1, f64 t) } INLINE f32 math_lerp_angle(f32 a, f32 b, f32 t) { - f32 diff = math_fmod(b - a, TAU); - diff = math_fmod(2.0f * diff, TAU) - diff; + f32 diff = math_diff_angle(b, a); return a + diff * t; }