diff --git a/src/phys.c b/src/phys.c index 8c75d4a5..7ef0930b 100644 --- a/src/phys.c +++ b/src/phys.c @@ -816,7 +816,7 @@ void phys_warm_start_mouse_joints(struct phys_step_ctx *ctx) struct xform xf = sim_ent_get_xform(ent); 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; + sim_ent_set_angular_velocity(ent, ent->angular_velocity + ((v2_wedge(vcp, joint->linear_impulse) + joint->angular_impulse) * inv_i)); } } } @@ -897,6 +897,10 @@ void phys_solve_mouse_joints(struct phys_step_ctx *ctx, f32 dt) struct phys_weld_joint_def phys_weld_joint_def_init(void) { struct phys_weld_joint_def def = ZI; + def.linear_spring_hz = 50; + def.linear_spring_damp = 0; + def.angular_spring_hz = 50; + def.angular_spring_damp = 0; return def; } @@ -905,6 +909,10 @@ 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.linear_spring_hz = def.linear_spring_hz; + res.linear_spring_damp = def.linear_spring_damp; + res.angular_spring_hz = def.angular_spring_hz; + res.angular_spring_damp = def.angular_spring_damp; res.xf0_to_xf1 = def.xf; return res; } @@ -947,6 +955,8 @@ void phys_prepare_weld_joints(struct phys_step_ctx *ctx) #if !SIM_PHYSICS_ENABLE_WARM_STARTING joint->linear_impulse0 = V2(0, 0); joint->linear_impulse1 = V2(0, 0); + joint->angular_impulse0 = 0; + joint->angular_impulse1 = 0; #endif } else { /* Mark joint for removal */ @@ -983,7 +993,9 @@ void phys_warm_start_weld_joints(struct phys_step_ctx *ctx) struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1); if (sim_ent_should_simulate(e1)) { f32 inv_m = joint->inv_m1; + f32 inv_i = joint->inv_i1; sim_ent_set_linear_velocity(e1, v2_add(e1->linear_velocity, v2_mul(joint->linear_impulse1, inv_m))); + sim_ent_set_angular_velocity(e1, e1->angular_velocity + joint->angular_impulse1 * inv_i); } #else (UNUSED)joint; @@ -1004,39 +1016,53 @@ void phys_solve_weld_joints(struct phys_step_ctx *ctx, f32 dt) 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 xform target_xf1 = xform_mul(xf0, joint->xf0_to_xf1); - struct v2 separation = v2_sub(xf1.og, target_p1); + struct v2 v1 = e1->linear_velocity; + f32 w1 = e1->angular_velocity; - f32 k = 1 / inv_m1; + /* Angular constraint */ + { + struct math_spring_result softness = math_spring(joint->angular_spring_hz, joint->angular_spring_damp, dt); + f32 inv_i1 = joint->inv_i1; + f32 k = 1 / inv_i1; + f32 separation = math_unwind_angle(xform_get_rotation(target_xf1) - xform_get_rotation(xf1)); + f32 bias = -separation * softness.bias_rate; + f32 b = (w1 + bias) * k; + f32 impulse = -softness.mass_scale * b - joint->angular_impulse1 * softness.impulse_scale; + joint->angular_impulse1 += impulse; + w1 += impulse * inv_i1; + } - f32 bias_rate = softness.bias_rate; - f32 mass_scale = softness.mass_scale; - f32 impulse_scale = softness.impulse_scale; + /* Linear constraint */ + { + struct math_spring_result softness = math_spring(joint->linear_spring_hz, joint->linear_spring_damp, dt); + f32 inv_m1 = joint->inv_m1; - struct v2 bias = v2_mul(separation, bias_rate); - struct v2 vel = v1; - struct v2 b = v2_mul(v2_add(vel, bias), k); + struct v2 separation = v2_sub(xf1.og, target_xf1.og); - struct v2 impulse = v2_mul(b, -mass_scale); - impulse = v2_sub(impulse, v2_mul(joint->linear_impulse1, impulse_scale)); + f32 k = 1 / inv_m1; - struct v2 old_impulse = joint->linear_impulse1; - joint->linear_impulse1 = v2_add(joint->linear_impulse1, impulse); + struct v2 bias = v2_mul(separation, softness.bias_rate); + struct v2 b = v2_mul(v2_add(v1, bias), k); - impulse = v2_sub(joint->linear_impulse1, old_impulse); + struct v2 impulse = v2_mul(b, -softness.mass_scale); + impulse = v2_sub(impulse, v2_mul(joint->linear_impulse1, softness.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)); + } - v1 = v2_add(v1, v2_mul(impulse, inv_m1)); sim_ent_set_linear_velocity(e1, v1); + sim_ent_set_angular_velocity(e1, w1); } } } diff --git a/src/phys.h b/src/phys.h index d3be95fb..944867d7 100644 --- a/src/phys.h +++ b/src/phys.h @@ -179,7 +179,15 @@ void phys_solve_mouse_joints(struct phys_step_ctx *ctx, f32 dt); struct phys_weld_joint_def { struct sim_ent_id e0; struct sim_ent_id e1; + + /* The xform that transforms a point in e0's space into the desired e1 space + * (IE `xf` * V2(0, 0) should evaluate to the local point that e1's origin will lie) */ struct xform xf; + + f32 linear_spring_hz; + f32 linear_spring_damp; + f32 angular_spring_hz; + f32 angular_spring_damp; }; struct phys_weld_joint { @@ -187,6 +195,11 @@ struct phys_weld_joint { struct sim_ent_id e1; struct xform xf0_to_xf1; + f32 linear_spring_hz; + f32 linear_spring_damp; + f32 angular_spring_hz; + f32 angular_spring_damp; + f32 inv_m0; f32 inv_m1; f32 inv_i0; @@ -194,6 +207,8 @@ struct phys_weld_joint { struct v2 linear_impulse0; struct v2 linear_impulse1; + f32 angular_impulse0; + f32 angular_impulse1; }; struct phys_weld_joint_def phys_weld_joint_def_init(void);