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

fix rotating shapes phasing through collisions

Browse Source
This commit is contained in:
jacob 2025-05-25 21:43:13 -05:00
parent 53d03f1684
commit 3d48c0c3de
4 changed files with 36 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/math.h
View File

@ -1270,10 +1270,10 @@ INLINE struct v2 math_poly_center(struct v2_array a)
* ========================== */ * ========================== */
/* https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf */ /* https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf */
struct math_spring_result { f32 bias_rate; f32 mass_scale; f32 impulse_scale; }; struct math_spring { f32 bias_rate; f32 mass_scale; f32 impulse_scale; };
INLINE struct math_spring_result math_spring(f32 hertz, f32 damping_ratio, f32 dt) INLINE struct math_spring math_spring_init(f32 hertz, f32 damping_ratio, f32 dt)
{ {
struct math_spring_result res; struct math_spring res;
if (hertz == 0.0f) { if (hertz == 0.0f) {
res.bias_rate = 0; res.bias_rate = 0;
res.mass_scale = 1; res.mass_scale = 1;

49
src/phys.c
View File

@ -173,8 +173,8 @@ void phys_create_and_update_contacts(struct phys_step_ctx *ctx, f32 elapsed_dt,
} }
/* Update points & separation */ /* Update points & separation */
contact->point_local_e0 = xform_invert_mul_v2(e0_xf, point); contact->vcp0 = v2_sub(point, e0_xf.og);
contact->point_local_e1 = xform_invert_mul_v2(e1_xf, point); contact->vcp1 = v2_sub(point, e1_xf.og);
contact->starting_separation = sep; contact->starting_separation = sep;
#if DEVELOPER #if DEVELOPER
@ -192,11 +192,11 @@ void phys_create_and_update_contacts(struct phys_step_ctx *ctx, f32 elapsed_dt,
data.dt = elapsed_dt; data.dt = elapsed_dt;
/* Calculate point */ /* Calculate point */
struct v2 point = collider_res.points[0].point; struct v2 midpoint = collider_res.points[0].point;
if (collider_res.num_points > 1) { if (collider_res.num_points > 1) {
point = v2_add(point, v2_mul(v2_sub(collider_res.points[1].point, point), 0.5f)); midpoint = v2_add(midpoint, v2_mul(v2_sub(collider_res.points[1].point, midpoint), 0.5f));
} }
data.point = point; data.point = midpoint;
/* Calculate relative velocity */ /* Calculate relative velocity */
struct v2 vrel; struct v2 vrel;
@ -205,8 +205,8 @@ void phys_create_and_update_contacts(struct phys_step_ctx *ctx, f32 elapsed_dt,
struct v2 v1 = e1->linear_velocity; struct v2 v1 = e1->linear_velocity;
f32 w0 = e0->angular_velocity; f32 w0 = e0->angular_velocity;
f32 w1 = e1->angular_velocity; f32 w1 = e1->angular_velocity;
struct v2 vcp0 = v2_sub(point, e0_xf.og); struct v2 vcp1 = v2_sub(midpoint, e1_xf.og);
struct v2 vcp1 = v2_sub(point, e1_xf.og); struct v2 vcp0 = v2_sub(midpoint, e0_xf.og);
struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0)); struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0));
struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1)); struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
vrel = v2_sub(vel0, vel1); vrel = v2_sub(vel0, vel1);
@ -325,9 +325,8 @@ void phys_prepare_contacts(struct phys_step_ctx *ctx, u64 phys_iteration)
/* Update / insert returned contacts */ /* Update / insert returned contacts */
for (u32 i = 0; i < num_points; ++i) { for (u32 i = 0; i < num_points; ++i) {
struct phys_contact_point *contact = &constraint->points[i]; struct phys_contact_point *contact = &constraint->points[i];
struct v2 vcp0 = contact->vcp0;
struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, contact->point_local_e0), e0_xf.og); struct v2 vcp1 = contact->vcp1;
struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, contact->point_local_e1), e1_xf.og);
/* Normal mass */ /* Normal mass */
{ {
@ -398,9 +397,6 @@ void phys_warm_start_contacts(struct phys_step_ctx *ctx)
struct sim_ent *e1 = sim_ent_from_id(ss, constraint->e1); struct sim_ent *e1 = sim_ent_from_id(ss, constraint->e1);
if (num_points > 0 && sim_ent_is_valid_and_active(e0) && sim_ent_is_valid_and_active(e1) && !constraint->skip_solve) { if (num_points > 0 && sim_ent_is_valid_and_active(e0) && sim_ent_is_valid_and_active(e1) && !constraint->skip_solve) {
struct xform e0_xf = sim_ent_get_xform(e0);
struct xform e1_xf = sim_ent_get_xform(e1);
f32 inv_m0 = constraint->inv_m0; f32 inv_m0 = constraint->inv_m0;
f32 inv_m1 = constraint->inv_m1; f32 inv_m1 = constraint->inv_m1;
f32 inv_i0 = constraint->inv_i0; f32 inv_i0 = constraint->inv_i0;
@ -417,8 +413,8 @@ void phys_warm_start_contacts(struct phys_step_ctx *ctx)
f32 inv_num_points = 1.f / num_points; f32 inv_num_points = 1.f / num_points;
for (u32 i = 0; i < num_points; ++i) { for (u32 i = 0; i < num_points; ++i) {
struct phys_contact_point *point = &constraint->points[i]; struct phys_contact_point *point = &constraint->points[i];
struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, point->point_local_e0), e0_xf.og); struct v2 vcp0 = point->vcp0;
struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, point->point_local_e1), e1_xf.og); struct v2 vcp1 = point->vcp1;
struct v2 impulse = v2_add(v2_mul(normal, point->normal_impulse), v2_mul(tangent, point->tangent_impulse)); struct v2 impulse = v2_add(v2_mul(normal, point->normal_impulse), v2_mul(tangent, point->tangent_impulse));
impulse = v2_mul(impulse, inv_num_points); impulse = v2_mul(impulse, inv_num_points);
@ -470,11 +466,12 @@ void phys_solve_contacts(struct phys_step_ctx *ctx, f32 dt, b32 apply_bias)
struct v2 normal = constraint->normal; struct v2 normal = constraint->normal;
for (u32 point_index = 0; point_index < num_points; ++point_index) { for (u32 point_index = 0; point_index < num_points; ++point_index) {
struct phys_contact_point *point = &constraint->points[point_index]; struct phys_contact_point *point = &constraint->points[point_index];
struct v2 p0 = xform_mul_v2(e0_xf, point->point_local_e0); struct v2 vcp0 = point->vcp0;
struct v2 p1 = xform_mul_v2(e1_xf, point->point_local_e1); struct v2 vcp1 = point->vcp1;
struct v2 vcp0 = v2_sub(p0, e0_xf.og); struct v2 p0 = v2_add(e0_xf.og, vcp0);
struct v2 vcp1 = v2_sub(p1, e1_xf.og); struct v2 p1 = v2_add(e1_xf.og, vcp1);
/* FIXME: Should separation use the rotated contact points? */
f32 separation = v2_dot(v2_sub(p1, p0), normal) + point->starting_separation; f32 separation = v2_dot(v2_sub(p1, p0), normal) + point->starting_separation;
f32 velocity_bias = 0.0f; f32 velocity_bias = 0.0f;
@ -486,7 +483,7 @@ void phys_solve_contacts(struct phys_step_ctx *ctx, f32 dt, b32 apply_bias)
velocity_bias = separation / dt; velocity_bias = separation / dt;
} else if (apply_bias) { } else if (apply_bias) {
/* Soft constraint */ /* Soft constraint */
struct math_spring_result softness = math_spring(CONTACT_SPRING_HZ, CONTACT_SPRING_DAMP, dt); struct math_spring softness = math_spring_init(CONTACT_SPRING_HZ, CONTACT_SPRING_DAMP, dt);
f32 pushout_velocity = constraint->pushout_velocity; f32 pushout_velocity = constraint->pushout_velocity;
mass_scale = softness.mass_scale; mass_scale = softness.mass_scale;
impulse_scale = softness.impulse_scale; impulse_scale = softness.impulse_scale;
@ -519,8 +516,8 @@ void phys_solve_contacts(struct phys_step_ctx *ctx, f32 dt, b32 apply_bias)
struct v2 tangent = v2_perp(normal); struct v2 tangent = v2_perp(normal);
for (u32 point_index = 0; point_index < num_points; ++point_index) { for (u32 point_index = 0; point_index < num_points; ++point_index) {
struct phys_contact_point *point = &constraint->points[point_index]; struct phys_contact_point *point = &constraint->points[point_index];
struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, point->point_local_e0), e0_xf.og); struct v2 vcp0 = point->vcp0;
struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, point->point_local_e1), e1_xf.og); struct v2 vcp1 = point->vcp1;
struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0)); struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0));
struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1)); struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1));
@ -865,7 +862,7 @@ void phys_solve_mouse_joints(struct phys_step_ctx *ctx, f32 dt)
/* Angular impulse */ /* Angular impulse */
{ {
struct math_spring_result softness = math_spring(joint->angular_spring_hz, joint->angular_spring_damp, dt); struct math_spring softness = math_spring_init(joint->angular_spring_hz, joint->angular_spring_damp, dt);
f32 mass_scale = softness.mass_scale; f32 mass_scale = softness.mass_scale;
f32 impulse_scale = softness.impulse_scale; f32 impulse_scale = softness.impulse_scale;
f32 impulse = mass_scale * (-w / inv_i) - impulse_scale * joint->angular_impulse; f32 impulse = mass_scale * (-w / inv_i) - impulse_scale * joint->angular_impulse;
@ -885,7 +882,7 @@ void phys_solve_mouse_joints(struct phys_step_ctx *ctx, f32 dt)
struct v2 vcp = v2_sub(point_start, xf.og); struct v2 vcp = v2_sub(point_start, xf.og);
struct v2 separation = v2_sub(point_start, point_end); 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 math_spring softness = math_spring_init(joint->linear_spring_hz, joint->linear_spring_damp, dt);
f32 bias_rate = softness.bias_rate; f32 bias_rate = softness.bias_rate;
f32 mass_scale = softness.mass_scale; f32 mass_scale = softness.mass_scale;
f32 impulse_scale = softness.impulse_scale; f32 impulse_scale = softness.impulse_scale;
@ -1050,7 +1047,7 @@ void phys_solve_weld_joints(struct phys_step_ctx *ctx, f32 dt)
/* Angular constraint */ /* Angular constraint */
{ {
struct math_spring_result softness = math_spring(joint->angular_spring_hz, joint->angular_spring_damp, dt); struct math_spring softness = math_spring_init(joint->angular_spring_hz, joint->angular_spring_damp, dt);
f32 inv_i1 = joint->inv_i1; f32 inv_i1 = joint->inv_i1;
f32 k = 1 / inv_i1; f32 k = 1 / inv_i1;
f32 separation = math_unwind_angle(xform_get_rotation(target_xf1) - xform_get_rotation(xf1)); f32 separation = math_unwind_angle(xform_get_rotation(target_xf1) - xform_get_rotation(xf1));
@ -1063,7 +1060,7 @@ void phys_solve_weld_joints(struct phys_step_ctx *ctx, f32 dt)
/* Linear constraint */ /* Linear constraint */
{ {
struct math_spring_result softness = math_spring(joint->linear_spring_hz, joint->linear_spring_damp, dt); struct math_spring softness = math_spring_init(joint->linear_spring_hz, joint->linear_spring_damp, dt);
f32 inv_m1 = joint->inv_m1; f32 inv_m1 = joint->inv_m1;
struct v2 separation = v2_sub(xf1.og, target_xf1.og); struct v2 separation = v2_sub(xf1.og, target_xf1.og);

12
src/phys.h
View File

@ -34,9 +34,15 @@ struct phys_step_ctx {
* ========================== */ * ========================== */
struct phys_contact_point { struct phys_contact_point {
/* Contact point in local space of each entity */ /* Contact point relative to the center of each entity.
struct v2 point_local_e0; *
struct v2 point_local_e1; * NOTE: We use fixed (non-rotated) points relative to the entities
* rather than points fully in local space because contact manifolds
* shouldn't really be affected by rotation accross substeps
* (imagine re-building the manifold of a rotated shape, it would still be
* on the same side of the shape that it originally occured on) */
struct v2 vcp0;
struct v2 vcp1;
u32 id; /* ID generated during clipping */ u32 id; /* ID generated during clipping */
f32 starting_separation; /* How far are original points along normal */ f32 starting_separation; /* How far are original points along normal */

5
src/user.c
View File

@ -355,7 +355,6 @@ INTERNAL void debug_draw_xform(struct xform xf, u32 color_x, u32 color_y)
//draw_quad(G.ui_cmd_buffer, quad, color); //draw_quad(G.ui_cmd_buffer, quad, color);
} }
/* TODO: remove this (testing) */
INTERNAL void debug_draw_movement(struct sim_ent *ent) INTERNAL void debug_draw_movement(struct sim_ent *ent)
{ {
f32 thickness = 2.f; f32 thickness = 2.f;
@ -1441,12 +1440,10 @@ INTERNAL void user_update(void)
/* Draw point */ /* Draw point */
{ {
//u32 color = contact.persisted ? RGBA_F(1, 1, 0, 0.50) : RGBA_F(1, 0, 0, 0.50);
u32 color = ALPHA_F(COLOR_YELLOW, 0.50); u32 color = ALPHA_F(COLOR_YELLOW, 0.50);
//struct v2 point = xform_mul_v2(e0_xf, contact.p0_local);
//struct v2 point = contact.p0_initial_world;
draw_circle(G.ui_cmd_buffer, xform_mul_v2(G.world_to_ui_xf, dbg_pt), radius, color, 10); draw_circle(G.ui_cmd_buffer, xform_mul_v2(G.world_to_ui_xf, dbg_pt), radius, color, 10);
} }
/* Draw normal */ /* Draw normal */
{ {
u32 color = COLOR_WHITE; u32 color = COLOR_WHITE;