jacob/power_play
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

weld joint angular

Browse Source
This commit is contained in:
jacob 2025-05-19 19:53:14 -05:00
parent fd365313b3
commit 9e8e800e9d
2 changed files with 62 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
src/phys.c
View File

@ -816,7 +816,7 @@ void phys_warm_start_mouse_joints(struct phys_step_ctx *ctx)
struct xform xf = sim_ent_get_xform(ent); struct xform xf = sim_ent_get_xform(ent);
struct v2 vcp = v2_sub(xform_mul_v2(xf, joint->point_local_start), xf.og); 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))); 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 phys_weld_joint_def_init(void)
{ {
struct phys_weld_joint_def def = ZI; 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; 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; struct phys_weld_joint res = ZI;
res.e0 = def.e0; res.e0 = def.e0;
res.e1 = def.e1; 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; res.xf0_to_xf1 = def.xf;
return res; return res;
} }
@ -947,6 +955,8 @@ void phys_prepare_weld_joints(struct phys_step_ctx *ctx)
#if !SIM_PHYSICS_ENABLE_WARM_STARTING #if !SIM_PHYSICS_ENABLE_WARM_STARTING
joint->linear_impulse0 = V2(0, 0); joint->linear_impulse0 = V2(0, 0);
joint->linear_impulse1 = V2(0, 0); joint->linear_impulse1 = V2(0, 0);
joint->angular_impulse0 = 0;
joint->angular_impulse1 = 0;
#endif #endif
} else { } else {
/* Mark joint for removal */ /* 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); struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1);
if (sim_ent_should_simulate(e1)) { if (sim_ent_should_simulate(e1)) {
f32 inv_m = joint->inv_m1; 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_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 #else
(UNUSED)joint; (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 *e0 = sim_ent_from_id(ss, joint->e0);
struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1); struct sim_ent *e1 = sim_ent_from_id(ss, joint->e1);
if (sim_ent_should_simulate(e0) && sim_ent_should_simulate(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 xf0 = sim_ent_get_xform(e0);
struct xform xf1 = sim_ent_get_xform(e1); struct xform xf1 = sim_ent_get_xform(e1);
f32 inv_m1 = joint->inv_m1; struct xform target_xf1 = xform_mul(xf0, joint->xf0_to_xf1);
struct v2 target_p1 = xform_mul(xf0, joint->xf0_to_xf1).og;
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; /* Linear constraint */
f32 mass_scale = softness.mass_scale; {
f32 impulse_scale = softness.impulse_scale; 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 separation = v2_sub(xf1.og, target_xf1.og);
struct v2 vel = v1;
struct v2 b = v2_mul(v2_add(vel, bias), k);
struct v2 impulse = v2_mul(b, -mass_scale); f32 k = 1 / inv_m1;
impulse = v2_sub(impulse, v2_mul(joint->linear_impulse1, impulse_scale));
struct v2 old_impulse = joint->linear_impulse1; struct v2 bias = v2_mul(separation, softness.bias_rate);
joint->linear_impulse1 = v2_add(joint->linear_impulse1, impulse); 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_linear_velocity(e1, v1);
sim_ent_set_angular_velocity(e1, w1);
} }
} }
} }

15
src/phys.h
View File

@ -179,7 +179,15 @@ void phys_solve_mouse_joints(struct phys_step_ctx *ctx, f32 dt);
struct phys_weld_joint_def { struct phys_weld_joint_def {
struct sim_ent_id e0; struct sim_ent_id e0;
struct sim_ent_id e1; 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; struct xform xf;
f32 linear_spring_hz;
f32 linear_spring_damp;
f32 angular_spring_hz;
f32 angular_spring_damp;
}; };
struct phys_weld_joint { struct phys_weld_joint {
@ -187,6 +195,11 @@ struct phys_weld_joint {
struct sim_ent_id e1; struct sim_ent_id e1;
struct xform xf0_to_xf1; 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_m0;
f32 inv_m1; f32 inv_m1;
f32 inv_i0; f32 inv_i0;
@ -194,6 +207,8 @@ struct phys_weld_joint {
struct v2 linear_impulse0; struct v2 linear_impulse0;
struct v2 linear_impulse1; struct v2 linear_impulse1;
f32 angular_impulse0;
f32 angular_impulse1;
}; };
struct phys_weld_joint_def phys_weld_joint_def_init(void); struct phys_weld_joint_def phys_weld_joint_def_init(void);