From fd365313b3104bfe6cd8e54eeb0243720928de00 Mon Sep 17 00:00:00 2001 From: jacob Date: Mon, 19 May 2025 19:11:19 -0500 Subject: [PATCH] weld joint linear part working --- src/phys.c | 172 ++++++++++++++++++++++++++++++++++++++++++++++--- src/phys.h | 30 +++++++++ src/sim_ent.h | 13 ++-- src/sim_step.c | 23 ++++++- src/user.c | 30 +++++++++ 5 files changed, 249 insertions(+), 19 deletions(-) diff --git a/src/phys.c b/src/phys.c index a295db2b..8c75d4a5 100644 --- a/src/phys.c +++ b/src/phys.c @@ -593,7 +593,7 @@ void phys_prepare_motor_joints(struct phys_step_ctx *ctx) struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, joint->point_local_e0), e0_xf.og); struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, joint->point_local_e1), e1_xf.og); - struct xform linear_mass_xf; + struct xform linear_mass_xf = ZI; linear_mass_xf.bx.x = inv_m0 + inv_m1 + vcp0.y * vcp0.y * inv_i0 + vcp1.y * vcp1.y * inv_i1; linear_mass_xf.bx.y = -vcp0.y * vcp0.x * inv_i0 - vcp1.y * vcp1.x * inv_i1; linear_mass_xf.by.x = linear_mass_xf.bx.y; @@ -780,7 +780,7 @@ void phys_prepare_mouse_joints(struct phys_step_ctx *ctx) struct v2 vcp = v2_sub(xform_mul_v2(xf, joint->point_local_start), xf.og); - struct xform linear_mass_xf; + struct xform linear_mass_xf = ZI; linear_mass_xf.bx.x = inv_m + inv_i * vcp.y * vcp.y; linear_mass_xf.bx.y = -inv_i * vcp.x * vcp.y; linear_mass_xf.by.x = linear_mass_xf.bx.y; @@ -862,25 +862,23 @@ void phys_solve_mouse_joints(struct phys_step_ctx *ctx, f32 dt) struct v2 separation = v2_sub(point_start, point_end); struct math_spring_result softness = math_spring(joint->linear_spring_hz, joint->linear_spring_damp, dt); - struct v2 bias = v2_mul(separation, softness.bias_rate); + f32 bias_rate = softness.bias_rate; f32 mass_scale = softness.mass_scale; f32 impulse_scale = softness.impulse_scale; + struct v2 bias = v2_mul(separation, bias_rate); struct v2 vel = v2_add(v, v2_perp_mul(vcp, w)); struct v2 b = xform_basis_mul_v2(joint->linear_mass_xf, v2_add(vel, bias)); - struct v2 impulse; - impulse.x = -mass_scale * b.x - impulse_scale * joint->linear_impulse.x; - impulse.y = -mass_scale * b.y - impulse_scale * joint->linear_impulse.y; + struct v2 impulse = v2_mul(b, -mass_scale); + impulse = v2_sub(impulse, v2_mul(joint->linear_impulse, impulse_scale)); struct v2 old_impulse = joint->linear_impulse; - joint->linear_impulse.x += impulse.x; - joint->linear_impulse.y += impulse.y; + joint->linear_impulse = v2_add(joint->linear_impulse, impulse); joint->linear_impulse = v2_clamp_len(joint->linear_impulse, max_impulse); - impulse.x = joint->linear_impulse.x - old_impulse.x; - impulse.y = joint->linear_impulse.y - old_impulse.y; + impulse = v2_sub(joint->linear_impulse, old_impulse); v = v2_add(v, v2_mul(impulse, inv_m)); w += v2_wedge(vcp, impulse) * inv_i; @@ -892,6 +890,157 @@ void phys_solve_mouse_joints(struct phys_step_ctx *ctx, f32 dt) } } +/* ========================== * + * Weld joint + * ========================== */ + +struct phys_weld_joint_def phys_weld_joint_def_init(void) +{ + struct phys_weld_joint_def def = ZI; + return def; +} + +struct phys_weld_joint phys_weld_joint_from_def(struct phys_weld_joint_def def) +{ + struct phys_weld_joint res = ZI; + res.e0 = def.e0; + res.e1 = def.e1; + res.xf0_to_xf1 = def.xf; + return res; +} + +void phys_prepare_weld_joints(struct phys_step_ctx *ctx) +{ + __prof; + struct sim_snapshot *ss = ctx->sim_step_ctx->world; + for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) { + struct sim_ent *joint_ent = &ss->ents[sim_ent_index]; + if (!sim_ent_should_simulate(joint_ent)) continue; + if (!sim_ent_has_prop(joint_ent, SEPROP_WELD_JOINT)) continue; + + /* TODO: Lookup and disable collision for any contacts between e0 & e1? */ + + struct phys_weld_joint *joint = &joint_ent->weld_joint_data; + struct sim_ent *e0 = sim_ent_from_id(ss, joint->e0); + struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1); + if (sim_ent_should_simulate(e0) && sim_ent_should_simulate(e1)) { + struct xform e0_xf = sim_ent_get_xform(e0); + struct xform e1_xf = sim_ent_get_xform(e1); + + 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); + } + joint->inv_m0 = inv_m0; + joint->inv_m1 = inv_m1; + joint->inv_i0 = inv_i0; + joint->inv_i1 = inv_i1; + +#if !SIM_PHYSICS_ENABLE_WARM_STARTING + joint->linear_impulse0 = V2(0, 0); + joint->linear_impulse1 = V2(0, 0); +#endif + } else { + /* Mark joint for removal */ + sim_ent_disable_prop(joint_ent, SEPROP_ACTIVE); + sim_ent_enable_prop(joint_ent, SEPROP_RELEASE); + } + } +} + +void phys_warm_start_weld_joints(struct phys_step_ctx *ctx) +{ + __prof; + struct sim_snapshot *ss = ctx->sim_step_ctx->world; + for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) { + struct sim_ent *joint_ent = &ss->ents[sim_ent_index]; + if (!sim_ent_should_simulate(joint_ent)) continue; + if (!sim_ent_has_prop(joint_ent, SEPROP_WELD_JOINT)) continue; + + struct phys_weld_joint *joint = &joint_ent->weld_joint_data; + +#if 0 + struct sim_ent *e0 = sim_ent_from_id(ss, joint->e0); + if (sim_ent_should_simulate(e0)) { + f32 inv_m = joint->inv_m0; + f32 inv_i = joint->inv_i0; + struct xform xf = sim_ent_get_xform(e1); + struct v2 vcp = v2_sub(xform_mul_v2(xf, joint->point_local_start), xf.og); + sim_ent_set_linear_velocity(ent, v2_add(ent->linear_velocity, v2_mul(joint->linear_impulse, inv_m))); + ent->angular_velocity += (v2_wedge(vcp, joint->linear_impulse) + joint->angular_impulse) * inv_i; + } +#endif + +#if 1 + struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1); + if (sim_ent_should_simulate(e1)) { + f32 inv_m = joint->inv_m1; + sim_ent_set_linear_velocity(e1, v2_add(e1->linear_velocity, v2_mul(joint->linear_impulse1, inv_m))); + } +#else + (UNUSED)joint; +#endif + } +} + +void phys_solve_weld_joints(struct phys_step_ctx *ctx, f32 dt) +{ + __prof; + struct sim_snapshot *ss = ctx->sim_step_ctx->world; + for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index) { + struct sim_ent *joint_ent = &ss->ents[sim_ent_index]; + if (!sim_ent_should_simulate(joint_ent)) continue; + if (!sim_ent_has_prop(joint_ent, SEPROP_WELD_JOINT)) continue; + + struct phys_weld_joint *joint = &joint_ent->weld_joint_data; + struct sim_ent *e0 = sim_ent_from_id(ss, joint->e0); + struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1); + if (sim_ent_should_simulate(e0) && sim_ent_should_simulate(e1)) { + struct math_spring_result softness = math_spring(50, 0.0f, dt); + + struct v2 v1 = e1->linear_velocity; + + struct xform xf0 = sim_ent_get_xform(e0); + struct xform xf1 = sim_ent_get_xform(e1); + + f32 inv_m1 = joint->inv_m1; + struct v2 target_p1 = xform_mul(xf0, joint->xf0_to_xf1).og; + + struct v2 separation = v2_sub(xf1.og, target_p1); + + f32 k = 1 / inv_m1; + + f32 bias_rate = softness.bias_rate; + f32 mass_scale = softness.mass_scale; + f32 impulse_scale = softness.impulse_scale; + + struct v2 bias = v2_mul(separation, bias_rate); + struct v2 vel = v1; + struct v2 b = v2_mul(v2_add(vel, bias), k); + + struct v2 impulse = v2_mul(b, -mass_scale); + impulse = v2_sub(impulse, v2_mul(joint->linear_impulse1, impulse_scale)); + + struct v2 old_impulse = joint->linear_impulse1; + joint->linear_impulse1 = v2_add(joint->linear_impulse1, impulse); + + impulse = v2_sub(joint->linear_impulse1, old_impulse); + + v1 = v2_add(v1, v2_mul(impulse, inv_m1)); + + sim_ent_set_linear_velocity(e1, v1); + } + } +} + /* ========================== * * Integration * ========================== */ @@ -1080,6 +1229,7 @@ void phys_step(struct phys_step_ctx *ctx, f32 timestep) phys_prepare_contacts(ctx, phys_iteration); phys_prepare_motor_joints(ctx); phys_prepare_mouse_joints(ctx); + phys_prepare_weld_joints(ctx); f32 substep_dt = step_dt / SIM_PHYSICS_SUBSTEPS; for (u32 i = 0; i < SIM_PHYSICS_SUBSTEPS; ++i) { @@ -1090,6 +1240,7 @@ void phys_step(struct phys_step_ctx *ctx, f32 timestep) phys_warm_start_contacts(ctx); phys_warm_start_motor_joints(ctx); phys_warm_start_mouse_joints(ctx); + phys_warm_start_weld_joints(ctx); #endif /* Solve */ @@ -1098,6 +1249,7 @@ void phys_step(struct phys_step_ctx *ctx, f32 timestep) #endif phys_solve_motor_joints(ctx, substep_dt); phys_solve_mouse_joints(ctx, substep_dt); + phys_solve_weld_joints(ctx, substep_dt); /* Integrate */ phys_integrate_velocities(ctx, substep_dt); diff --git a/src/phys.h b/src/phys.h index e65a8606..d3be95fb 100644 --- a/src/phys.h +++ b/src/phys.h @@ -172,6 +172,36 @@ void phys_prepare_mouse_joints(struct phys_step_ctx *ctx); void phys_warm_start_mouse_joints(struct phys_step_ctx *ctx); void phys_solve_mouse_joints(struct phys_step_ctx *ctx, f32 dt); +/* ========================== * + * Weld joint + * ========================== */ + +struct phys_weld_joint_def { + struct sim_ent_id e0; + struct sim_ent_id e1; + struct xform xf; +}; + +struct phys_weld_joint { + struct sim_ent_id e0; + struct sim_ent_id e1; + struct xform xf0_to_xf1; + + f32 inv_m0; + f32 inv_m1; + f32 inv_i0; + f32 inv_i1; + + struct v2 linear_impulse0; + struct v2 linear_impulse1; +}; + +struct phys_weld_joint_def phys_weld_joint_def_init(void); +struct phys_weld_joint phys_weld_joint_from_def(struct phys_weld_joint_def def); +void phys_prepare_weld_joints(struct phys_step_ctx *ctx); +void phys_warm_start_weld_joints(struct phys_step_ctx *ctx); +void phys_solve_weld_joints(struct phys_step_ctx *ctx, f32 dt); + /* ========================== * * Integration * ========================== */ diff --git a/src/sim_ent.h b/src/sim_ent.h index 3d1b8052..27993cbd 100644 --- a/src/sim_ent.h +++ b/src/sim_ent.h @@ -41,6 +41,7 @@ enum sim_ent_prop { SEPROP_CONTACT_CONSTRAINT, SEPROP_MOTOR_JOINT, SEPROP_MOUSE_JOINT, + SEPROP_WELD_JOINT, SEPROP_CAMERA, SEPROP_CAMERA_ACTIVE, @@ -193,23 +194,20 @@ struct sim_ent { struct space_entry_handle space_handle; /* ====================================================================== */ - /* Contact constraint */ + /* Constraints / joints */ /* SEPROP_CONSTRAINT_CONTACT */ struct phys_contact_constraint contact_constraint_data; - /* ====================================================================== */ - /* Motor joint */ - /* SEPROP_MOTOR_JOINT */ struct phys_motor_joint motor_joint_data; - /* ====================================================================== */ - /* Mouse joint */ - /* SEPROP_MOUSE_JOINT */ struct phys_mouse_joint mouse_joint_data; + /* SEPROP_WELD_JOINT */ + struct phys_weld_joint weld_joint_data; + /* ====================================================================== */ /* Control */ @@ -289,6 +287,7 @@ struct sim_ent { /* SEPROP_WEAPON_CHUCKER */ struct sim_ent_id chucker_zone; + struct sim_ent_id chucker_joint; /* ====================================================================== */ /* Chucker zone */ diff --git a/src/sim_step.c b/src/sim_step.c index 3f89c533..0799ec06 100644 --- a/src/sim_step.c +++ b/src/sim_step.c @@ -1035,7 +1035,26 @@ void sim_step(struct sim_step_ctx *ctx) struct sim_ent *zone = sim_ent_from_id(world, ent->chucker_zone); struct sim_ent *target = sim_ent_from_id(world, zone->chucker_zone_ent); if (sim_ent_is_valid_and_active(target) && zone->chucker_zone_ent_tick == world->tick - 1) { - sim_ent_enable_prop(target, SEPROP_RELEASE); + struct sim_ent *old_joint_ent = sim_ent_from_id(world, ent->chucker_joint); + if (!sim_ent_id_eq(old_joint_ent->weld_joint_data.e1, target->id)) { + if (old_joint_ent->valid) { + sim_ent_enable_prop(old_joint_ent, SEPROP_RELEASE); + } + struct sim_ent *joint_ent = sim_ent_alloc_sync_src(root); + sim_ent_enable_prop(joint_ent, SEPROP_ACTIVE); + + struct xform xf0 = sim_ent_get_xform(ent); + struct xform xf1 = sim_ent_get_xform(target); + struct xform xf0_to_xf1 = xform_mul(xform_invert(xf0), xf1); + + sim_ent_enable_prop(joint_ent, SEPROP_WELD_JOINT); + struct phys_weld_joint_def def = phys_weld_joint_def_init(); + def.e0 = ent->id; + def.e1 = target->id; + def.xf = xf0_to_xf1; + joint_ent->weld_joint_data = phys_weld_joint_from_def(def); + ent->chucker_joint = joint_ent->id; + } } } } @@ -1056,10 +1075,10 @@ void sim_step(struct sim_step_ctx *ctx) joint_ent->predictor = ent->predictor; joint_ent->mass_unscaled = F32_INFINITY; joint_ent->inertia_unscaled = F32_INFINITY; - sim_ent_enable_prop(joint_ent, SEPROP_MOTOR_JOINT); sim_ent_enable_prop(joint_ent, SEPROP_ACTIVE); ent->move_joint = joint_ent->id; + sim_ent_enable_prop(joint_ent, SEPROP_MOTOR_JOINT); struct phys_motor_joint_def def = phys_motor_joint_def_init(); def.e0 = joint_ent->id; /* Re-using joint entity as e0 */ def.e1 = ent->id; diff --git a/src/user.c b/src/user.c index e3b99cbe..c7f96196 100644 --- a/src/user.c +++ b/src/user.c @@ -1172,6 +1172,36 @@ INTERNAL void user_update(void) } #endif + /* Draw weld joint */ +#if 0 + if (sim_ent_has_prop(ent, SEPROP_WELD_JOINT)) { + struct sim_ent *e1 = sim_ent_from_id(G.ss_blended, ent->weld_joint_data.e1); + struct xform e1_xf = sim_ent_get_xform(e1); + + u32 color = COLOR_YELLOW; + f32 radius = 3; + struct v2 point = xform_mul_v2(e1_xf, ent->weld_joint_data.point_local_e1); + point = xform_mul_v2(G.world_to_ui_xf, point); + draw_circle(G.ui_cmd_buffer, point, radius, color, 10); + + DEBUGBREAKABLE; + } +#endif + + /* Draw mouse joint */ + if (sim_ent_has_prop(ent, SEPROP_MOUSE_JOINT)) { + struct sim_ent *target = sim_ent_from_id(G.ss_blended, ent->mouse_joint_data.target); + struct xform target_xf = sim_ent_get_xform(target); + + u32 color = COLOR_YELLOW; + f32 radius = 3; + struct v2 point = xform_mul_v2(target_xf, ent->mouse_joint_data.point_local_end); + point = xform_mul_v2(G.world_to_ui_xf, point); + draw_circle(G.ui_cmd_buffer, point, radius, color, 10); + + DEBUGBREAKABLE; + } + /* Draw collider */ if (ent->local_collider.count > 0) { struct collider_shape collider = ent->local_collider;