diff --git a/src/math.h b/src/math.h index 03fcc4a2..cc1ecff8 100644 --- a/src/math.h +++ b/src/math.h @@ -1270,10 +1270,10 @@ INLINE struct v2 math_poly_center(struct v2_array a) * ========================== */ /* https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf */ -struct math_spring_result { f32 bias_rate; f32 mass_scale; f32 impulse_scale; }; -INLINE struct math_spring_result math_spring(f32 hertz, f32 damping_ratio, f32 dt) +struct math_spring { f32 bias_rate; f32 mass_scale; f32 impulse_scale; }; +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) { res.bias_rate = 0; res.mass_scale = 1; diff --git a/src/phys.c b/src/phys.c index 8d784533..cc288c49 100644 --- a/src/phys.c +++ b/src/phys.c @@ -173,8 +173,8 @@ void phys_create_and_update_contacts(struct phys_step_ctx *ctx, f32 elapsed_dt, } /* Update points & separation */ - contact->point_local_e0 = xform_invert_mul_v2(e0_xf, point); - contact->point_local_e1 = xform_invert_mul_v2(e1_xf, point); + contact->vcp0 = v2_sub(point, e0_xf.og); + contact->vcp1 = v2_sub(point, e1_xf.og); contact->starting_separation = sep; #if DEVELOPER @@ -192,11 +192,11 @@ void phys_create_and_update_contacts(struct phys_step_ctx *ctx, f32 elapsed_dt, data.dt = elapsed_dt; /* Calculate point */ - struct v2 point = collider_res.points[0].point; + struct v2 midpoint = collider_res.points[0].point; 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 */ 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; f32 w0 = e0->angular_velocity; f32 w1 = e1->angular_velocity; - struct v2 vcp0 = v2_sub(point, e0_xf.og); - struct v2 vcp1 = v2_sub(point, e1_xf.og); + struct v2 vcp1 = v2_sub(midpoint, 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 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1)); 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 */ for (u32 i = 0; i < num_points; ++i) { struct phys_contact_point *contact = &constraint->points[i]; - - struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, contact->point_local_e0), e0_xf.og); - struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, contact->point_local_e1), e1_xf.og); + struct v2 vcp0 = contact->vcp0; + struct v2 vcp1 = contact->vcp1; /* 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); 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_m1 = constraint->inv_m1; 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; for (u32 i = 0; i < num_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 vcp1 = v2_sub(xform_mul_v2(e1_xf, point->point_local_e1), e1_xf.og); + struct v2 vcp0 = point->vcp0; + struct v2 vcp1 = point->vcp1; struct v2 impulse = v2_add(v2_mul(normal, point->normal_impulse), v2_mul(tangent, point->tangent_impulse)); 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; for (u32 point_index = 0; point_index < num_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 p1 = xform_mul_v2(e1_xf, point->point_local_e1); - struct v2 vcp0 = v2_sub(p0, e0_xf.og); - struct v2 vcp1 = v2_sub(p1, e1_xf.og); + struct v2 vcp0 = point->vcp0; + struct v2 vcp1 = point->vcp1; + struct v2 p0 = v2_add(e0_xf.og, vcp0); + 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 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; } else if (apply_bias) { /* 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; mass_scale = softness.mass_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); for (u32 point_index = 0; point_index < num_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 vcp1 = v2_sub(xform_mul_v2(e1_xf, point->point_local_e1), e1_xf.og); + struct v2 vcp0 = point->vcp0; + struct v2 vcp1 = point->vcp1; struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0)); 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 */ { - 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 impulse_scale = softness.impulse_scale; 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 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 mass_scale = softness.mass_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 */ { - 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 k = 1 / inv_i1; 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 */ { - 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; struct v2 separation = v2_sub(xf1.og, target_xf1.og); diff --git a/src/phys.h b/src/phys.h index d32a12c3..e11f602a 100644 --- a/src/phys.h +++ b/src/phys.h @@ -34,9 +34,15 @@ struct phys_step_ctx { * ========================== */ struct phys_contact_point { - /* Contact point in local space of each entity */ - struct v2 point_local_e0; - struct v2 point_local_e1; + /* Contact point relative to the center of each entity. + * + * 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 */ f32 starting_separation; /* How far are original points along normal */ diff --git a/src/user.c b/src/user.c index d6e5e6b9..c642b7a3 100644 --- a/src/user.c +++ b/src/user.c @@ -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); } -/* TODO: remove this (testing) */ INTERNAL void debug_draw_movement(struct sim_ent *ent) { f32 thickness = 2.f; @@ -1441,12 +1440,10 @@ INTERNAL void user_update(void) /* 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); - //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 normal */ { u32 color = COLOR_WHITE;