diff --git a/src/entity.h b/src/entity.h index e88dde2a..d489daae 100644 --- a/src/entity.h +++ b/src/entity.h @@ -14,6 +14,7 @@ enum entity_prop { ENTITY_PROP_PHYSICAL, ENTITY_PROP_CONTACT_CONSTRAINT, ENTITY_PROP_MOTOR_JOINT, + ENTITY_PROP_MOUSE_JOINT, ENTITY_PROP_PLAYER_CONTROLLED, ENTITY_PROP_CAMERA, @@ -58,6 +59,7 @@ struct entity_store { /* TODO: Remove this */ #include "collider.h" +#include "math.h" struct contact_point { /* Contact point in local space of each entity */ @@ -89,6 +91,9 @@ struct contact_constraint { struct contact_point points[2]; u32 num_points; + struct math_spring_result softness; + f32 pushout_velocity; + /* TODO: Remove this (debugging) */ struct collider_collision_points_result res; struct xform dbg_xf0; @@ -146,6 +151,31 @@ INLINE struct motor_joint motor_joint_from_def(struct motor_joint_def def) +struct mouse_joint { + struct entity_handle target; + + struct v2 point_local_target; + struct v2 point_local_mouse; + + f32 max_force; + + f32 inv_m; + f32 inv_i; + + struct v2 linear_impulse; + f32 angular_impulse; + + struct xform linear_mass_xf; + + struct math_spring_result linear_softness; + struct math_spring_result angular_softness; +}; + + + + + + struct entity { @@ -199,6 +229,12 @@ struct entity { /* ENTITY_PROP_MOTOR_JOINT */ struct motor_joint motor_joint_data; + /* ====================================================================== */ + /* Mouse joint */ + + /* ENTITY_PROP_MOUSE_JOINT */ + struct mouse_joint mouse_joint_data; + diff --git a/src/game.c b/src/game.c index 4c3549bc..660beea9 100644 --- a/src/game.c +++ b/src/game.c @@ -138,44 +138,39 @@ INTERNAL void spawn_test_entities(f32 offset) struct entity *e = entity_alloc(root); - //struct v2 pos = V2(0.25, -10); - //struct v2 pos = V2(0.25, -7); - //struct v2 pos = V2(0.25, -5.27); - //struct v2 pos = V2(0.5, -1.5); struct v2 pos = V2(1, -1); - //struct v2 pos = V2(0.300121694803, -1.322724342346); - //struct v2 pos = V2(1.0295, -1); pos = v2_add(pos, V2(0, offset)); pos = v2_add(pos, V2(0, offset_all)); - //struct v2 size = V2(1, 1); - //struct v2 size = V2(0.25, 0.25); //struct v2 size = V2(0.5, 0.5); //struct v2 size = V2(0.5, 0.25); struct v2 size = V2(1.0, 1.0); - //struct v2 size = V2(1.5, 1.5); - //f32 r = PI; - f32 r = PI / 4; - //f32 r = PI / 3; - //f32 r = 0.05; - //f32 r = PI / 2; - //f32 r = PI; - //f32 r = 0; + + //f32 r = PI / 4; + f32 r = 0; + + if (!G.extra_spawn) { + e->sprite = sprite_tag_from_path(STR("res/graphics/tim.ase")); + } else { + e->sprite = sprite_tag_from_path(STR("res/graphics/box.ase")); + size = V2(0.5, 0.5); + } + + //e->sprite = sprite_tag_from_path(STR("res/graphics/box_rounded.ase")); + //e->sprite_span_name = STR("idle.unarmed"); + //e->sprite_span_name = STR("idle.one_handed"); + e->sprite_span_name = STR("idle.two_handed"); + struct xform xf = XFORM_TRS(.t = pos, .r = r, .s = size); //xf.bx.y = -1.f; entity_set_xform(e, xf); - e->sprite = sprite_tag_from_path(STR("res/graphics/tim.ase")); - //e->sprite = sprite_tag_from_path(STR("res/graphics/box.ase")); - //e->sprite = sprite_tag_from_path(STR("res/graphics/box_rounded.ase")); - //e->sprite_span_name = STR("idle.unarmed"); - //e->sprite_span_name = STR("idle.one_handed"); - e->sprite_span_name = STR("idle.two_handed"); - - entity_enable_prop(e, ENTITY_PROP_PLAYER_CONTROLLED); + if (!G.extra_spawn) { + entity_enable_prop(e, ENTITY_PROP_PLAYER_CONTROLLED); + } e->linear_ground_friction = 150; e->angular_ground_friction = 100; @@ -309,8 +304,8 @@ INTERNAL void spawn_test_entities(f32 offset) struct v2 size = V2(1, 1); #else //struct v2 size = V2(5000, 1); - //struct v2 size = V2(50, 1); - struct v2 size = V2(1, 1); + struct v2 size = V2(50, 1); + //struct v2 size = V2(1, 1); #endif //f32 rot = PI / 4; f32 rot = 0; @@ -369,7 +364,7 @@ INTERNAL void spawn_test_entities(f32 offset) * TESTING CONTACT CONSTRAINT * ========================== */ -INTERNAL void prepare_contacts(void) +INTERNAL void create_contacts(void) { /* TODO: Remove this */ /* FIXME: Dict has leaks: Entries never removed, even when constraint is no longer valid, or either entities are no longer valid. */ @@ -492,28 +487,6 @@ INTERNAL void prepare_contacts(void) if (res.num_points > 0) { struct contact_constraint *constraint = &constraint_ent->contact_constraint_data; - struct v2 normal = res.normal; - struct v2 tangent = v2_perp(normal); - - /* TODO: Cache this */ - /* Calculate masses */ - 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); - } - constraint->inv_m0 = inv_m0; - constraint->inv_m1 = inv_m1; - constraint->inv_i0 = inv_i0; - constraint->inv_i1 = inv_i1; - /* Delete old contacts that are no longer present */ for (u32 i = 0; i < constraint->num_points; ++i) { struct contact_point *old = &constraint->points[i]; @@ -547,58 +520,101 @@ INTERNAL void prepare_contacts(void) break; } } - if (contact) { - /* Update existing */ -#if !GAME_PHYSICS_ENABLE_WARM_STARTING - contact->normal_impulse = 0; - contact->tangent_impulse = 0; -#endif - } else { - /* Insert new */ + if (!contact) { + /* Insert */ contact = &constraint->points[constraint->num_points++]; MEMZERO_STRUCT(contact); contact->id = id; + f32 substep_dt = (1.f / ((f32)GAME_FPS * (f32)GAME_PHYSICS_SUBSTEPS)); + f32 damping_ratio = 10.0f; + f32 frequency = (1.f / substep_dt) / 8.f; + constraint->softness = math_spring(frequency, damping_ratio, substep_dt); + constraint->pushout_velocity = 3.0f; } + /* 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->starting_separation = sep; - /* TODO: Remove this (debugging) */ #if COLLIDER_DEBUG - { - contact->dbg_pt = point; - } + contact->dbg_pt = point; #endif - { - struct v2 vcp0 = v2_sub(point, e0_xf.og); - struct v2 vcp1 = v2_sub(point, e1_xf.og); - - /* Normal mass */ - { - f32 vcp0_wedge = v2_wedge(vcp0, normal); - f32 vcp1_wedge = v2_wedge(vcp1, normal); - f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge); - contact->inv_normal_mass = k > 0.0f ? 1.0f / k : 0.0f; - } - - /* Tangent mass */ - { - f32 vcp0_wedge = v2_wedge(vcp0, tangent); - f32 vcp1_wedge = v2_wedge(vcp1, tangent); - f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge); - contact->inv_tangent_mass = k > 0.0f ? 1.0f / k : 0.0f; - } - } } } else if (constraint_ent) { constraint_ent->contact_constraint_data.num_points = 0; -#if !COLLIDER_DEBUG - /* No longer colliding, delete constraint */ - fixed_dict_set(&dict_arena, &dict, constraint_key, NULL); - entity_enable_prop(constraint_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME); + } + } + } +} + +INTERNAL void prepare_contacts(void) +{ + struct entity_store *store = G.tick.entity_store; + for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { + struct entity *constraint_ent = &store->entities[entity_index]; + if (!entity_is_valid_and_active(constraint_ent)) continue; + if (!entity_has_prop(constraint_ent, ENTITY_PROP_CONTACT_CONSTRAINT)) continue; + + struct contact_constraint *constraint = &constraint_ent->contact_constraint_data; + + u32 num_points = constraint->num_points; + struct entity *e0 = entity_from_handle(store, constraint->e0); + struct entity *e1 = entity_from_handle(store, constraint->e1); + if (num_points > 0 && entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) { + struct v2 normal = constraint->normal; + struct v2 tangent = v2_perp(normal); + + struct xform e0_xf = entity_get_xform(e0); + struct xform e1_xf = entity_get_xform(e1); + + /* TODO: Cache this */ + /* Calculate masses */ + 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); + } + constraint->inv_m0 = inv_m0; + constraint->inv_m1 = inv_m1; + constraint->inv_i0 = inv_i0; + constraint->inv_i1 = inv_i1; + + /* Update / insert returned contacts */ + for (u32 i = 0; i < num_points; ++i) { + struct contact_point *contact = &constraint->points[i]; + + struct v2 vcp0 = v2_sub(xform_basis_mul_v2(e0_xf, contact->point_local_e0), e0_xf.og); + struct v2 vcp1 = v2_sub(xform_basis_mul_v2(e1_xf, contact->point_local_e1), e1_xf.og); + + /* Normal mass */ + { + f32 vcp0_wedge = v2_wedge(vcp0, normal); + f32 vcp1_wedge = v2_wedge(vcp1, normal); + f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge); + contact->inv_normal_mass = k > 0.0f ? 1.0f / k : 0.0f; + } + + /* Tangent mass */ + { + f32 vcp0_wedge = v2_wedge(vcp0, tangent); + f32 vcp1_wedge = v2_wedge(vcp1, tangent); + f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge); + contact->inv_tangent_mass = k > 0.0f ? 1.0f / k : 0.0f; + } + +#if !GAME_PHYSICS_ENABLE_WARM_STARTING + contact->normal_impulse = 0; + contact->tangent_impulse = 0; #endif } } @@ -659,24 +675,6 @@ INTERNAL void warm_start_contacts(void) } } -struct soft_result { f32 bias_rate; f32 mass_scale; f32 impulse_scale; }; -INTERNAL struct soft_result make_soft(f32 hertz, f32 zeta, f32 h) -{ - if (hertz == 0.0f) { - return (struct soft_result) { .mass_scale = 1.0f }; - } else { - f32 omega = 2.0f * PI * hertz; - f32 a1 = 2.0f * zeta + h * omega; - f32 a2 = h * omega * a1; - f32 a3 = 1.0f / (1.0f + a2); - return (struct soft_result) { - .bias_rate = omega / a1, - .mass_scale = a2 * a3, - .impulse_scale = 1.0f / (1.0f + a2) - }; - } -} - INTERNAL void solve_contacts(f32 dt, b32 apply_bias) { struct entity_store *store = G.tick.entity_store; @@ -725,17 +723,11 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) velocity_bias = separation / dt; } else if (apply_bias) { /* Soft constraint */ - - f32 contact_damping_ratio = 10.0f; - f32 contact_hertz = (1.f / dt) / 8; - - struct soft_result softness = make_soft(contact_hertz, contact_damping_ratio, dt); - - f32 contact_pushout_velocity = 3.0f; - velocity_bias = max_f32(softness.bias_rate * separation, -contact_pushout_velocity); - + struct math_spring_result softness = constraint->softness; + f32 pushout_velocity = constraint->pushout_velocity; mass_scale = softness.mass_scale; impulse_scale = softness.impulse_scale; + velocity_bias = max_f32(softness.bias_rate * separation, -pushout_velocity); } struct v2 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0)); @@ -811,8 +803,6 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) * TESTING MOTOR JOINT * ========================== */ -#if 1 - INTERNAL void prepare_motor_joints(void) { struct entity_store *store = G.tick.entity_store; @@ -868,9 +858,6 @@ INTERNAL void prepare_motor_joints(void) joint->linear_impulse = V2(0, 0); joint->angular_impulse = 0; #endif - } else { - entity_disable_prop(joint_ent, ENTITY_PROP_ACTIVE); - entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME); } } } @@ -979,20 +966,156 @@ INTERNAL void solve_motor_joints(f32 dt) e1->linear_velocity = v1; e1->angular_velocity = w1; } +} + + + + +#if 1 + +/* ========================== * + * TESTING MOUSE JOINT + * ========================== */ + +INTERNAL void create_mouse_joints(void) +{ +} + +INTERNAL void prepare_mouse_joints(void) +{ + struct entity_store *store = G.tick.entity_store; + for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { + struct entity *joint_ent = &store->entities[entity_index]; + if (!entity_is_valid_and_active(joint_ent)) continue; + if (!entity_has_prop(joint_ent, ENTITY_PROP_MOUSE_JOINT)) continue; + + struct mouse_joint *joint = &joint_ent->mouse_joint_data; + struct entity *ent = entity_from_handle(store, joint->target); + if (entity_is_valid_and_active(ent)) { + struct xform xf = entity_get_xform(ent); + + /* TODO: Cache this */ + /* Calculate masses */ + f32 inv_m; + f32 inv_i; + { + f32 scale = math_fabs(xform_get_determinant(xf)); + inv_m = 1.f / (ent->mass_unscaled * scale); + inv_i = 1.f / (ent->inertia_unscaled * scale); + } + joint->inv_m = inv_m; + joint->inv_i = inv_i; + + struct v2 vcp = v2_sub(xform_mul_v2(xf, joint->point_local_target), xf.og); + + struct xform linear_mass_xf; + 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; + linear_mass_xf.by.y = inv_m + inv_i * vcp.x * vcp.x; + joint->linear_mass_xf = xform_invert(linear_mass_xf); + +#if !GAME_PHYSICS_ENABLE_WARM_STARTING + joint->linear_impulse = V2(0, 0); + joint->angular_impulse = 0; +#endif + } + + } +} + +INTERNAL void warm_start_mouse_joints(void) +{ + struct entity_store *store = G.tick.entity_store; + for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { + struct entity *joint_ent = &store->entities[entity_index]; + if (!entity_is_valid_and_active(joint_ent)) continue; + if (!entity_has_prop(joint_ent, ENTITY_PROP_MOUSE_JOINT)) continue; + + struct mouse_joint *joint = &joint_ent->mouse_joint_data; + struct entity *ent = entity_from_handle(store, joint->target); + if (entity_is_valid_and_active(ent)) { + f32 inv_m = joint->inv_m; + f32 inv_i = joint->inv_i; + struct xform xf = entity_get_xform(ent); + struct v2 vcp = v2_sub(xform_mul_v2(xf, joint->point_local_target), xf.og); + ent->linear_velocity = v2_add(ent->linear_velocity, v2_mul(joint->linear_impulse, inv_m)); + ent->angular_velocity += (v2_wedge(joint->linear_impulse, vcp) + joint->angular_impulse) * inv_i; + } + } +} + +INTERNAL void solve_mouse_joints(f32 dt) +{ + struct entity_store *store = G.tick.entity_store; + for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { + struct entity *joint_ent = &store->entities[entity_index]; + if (!entity_is_valid_and_active(joint_ent)) continue; + if (!entity_has_prop(joint_ent, ENTITY_PROP_MOUSE_JOINT)) continue; + + struct mouse_joint *joint = &joint_ent->mouse_joint_data; + struct entity *ent = entity_from_handle(store, joint->target); + if (entity_is_valid_and_active(ent)) { + struct v2 v = ent->linear_velocity; + f32 w = ent->angular_velocity; + + f32 inv_m = joint->inv_m; + f32 inv_i = joint->inv_i; + + /* Angular impulse */ + { + struct math_spring_result softness = joint->angular_softness; + f32 mass_scale = softness.mass_scale; + f32 impulse_scale = softness.impulse_scale; + f32 impulse = mass_scale * (-w / inv_i) - impulse_scale * joint->angular_impulse; + joint->angular_impulse += impulse; + w += impulse * inv_i; + } + + /* Linear impulse */ + { + f32 max_impulse = joint->max_force / dt; + + struct xform xf = entity_get_xform(ent); + + struct v2 point_target = xform_mul_v2(xf, joint->point_local_target); + struct v2 point_mouse = xform_mul_v2(xf, joint->point_local_mouse); + + struct v2 vcp = v2_sub(point_target, xf.og); + struct v2 separation = v2_sub(point_mouse, point_target); + + struct math_spring_result softness = joint->linear_softness; + struct v2 bias = v2_mul(separation, softness.bias_rate); + f32 mass_scale = softness.mass_scale; + f32 impulse_scale = softness.impulse_scale; + + struct v2 vel = v2_add(v, v2_perp_mul(vcp, w)); + struct v2 b = v2_mul(xform_basis_mul_v2(joint->linear_mass_xf, v2_add(vel, bias)), mass_scale); + + struct v2 old_impulse = joint->linear_impulse; + struct v2 impulse = v2_add(v2_mul(joint->linear_impulse, impulse_scale), b); + joint->linear_impulse = v2_clamp_len(v2_add(joint->linear_impulse, impulse), max_impulse); + impulse = v2_sub(joint->linear_impulse, old_impulse); + + v = v2_add(v, v2_mul(impulse, inv_m)); + w += v2_wedge(impulse, vcp) * inv_i; + } + } + } } #else -INTERNAL void prepare_motor_joints(void) +INTERNAL void prepare_mouse_joints(void) { } -INTERNAL void warm_start_motor_joints(void) +INTERNAL void warm_start_mouse_joints(void) { } -INTERNAL void solve_motor_joints(f32 dt) +INTERNAL void solve_mouse_joints(f32 dt) { (UNUSED)dt; } @@ -1003,8 +1126,6 @@ INTERNAL void solve_motor_joints(f32 dt) - - /* ========================== * * TESTING PHYSICS INTEGRATION * ========================== */ @@ -1544,7 +1665,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) } entity_set_xform(joint_ent, XFORM_IDENT); /* Reset joint ent position */ - joint_ent->linear_velocity = v2_with_len(ent->control.move, ent->control_force_max_speed); + joint_ent->linear_velocity = v2_mul(v2_clamp_len(ent->control.move, 1), ent->control_force_max_speed); } } #endif @@ -1718,7 +1839,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * Physics * ========================== */ - + (UNUSED)create_contacts; (UNUSED)prepare_contacts; (UNUSED)warm_start_contacts; (UNUSED)solve_contacts; @@ -1727,26 +1848,36 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) (UNUSED)warm_start_motor_joints; (UNUSED)solve_motor_joints; + (UNUSED)create_mouse_joints; + (UNUSED)prepare_mouse_joints; + (UNUSED)warm_start_mouse_joints; + (UNUSED)solve_mouse_joints; + (UNUSED)integrate_velocities_from_forces; (UNUSED)integrate_positions_from_velocities; #if 1 { integrate_velocities_from_forces(dt); + create_contacts(); + create_mouse_joints(); + prepare_contacts(); prepare_motor_joints(); + prepare_mouse_joints(); f32 substep_dt = dt / GAME_PHYSICS_SUBSTEPS; for (u32 i = 0; i < GAME_PHYSICS_SUBSTEPS; ++i) { #if GAME_PHYSICS_ENABLE_WARM_STARTING warm_start_contacts(); warm_start_motor_joints(); + warm_start_mouse_joints(); #endif #if GAME_PHYSICS_ENABLE_COLLISION solve_contacts(substep_dt, true); #endif solve_motor_joints(substep_dt); - + solve_mouse_joints(substep_dt); integrate_positions_from_velocities(substep_dt); diff --git a/src/math.h b/src/math.h index 004393cf..0648e936 100644 --- a/src/math.h +++ b/src/math.h @@ -1193,4 +1193,29 @@ INLINE struct v2 math_poly_center(struct v2_array a) return v2_div(sum, a.count); } +/* ========================== * + * Other + * ========================== */ + +/* 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_result res; + if (hertz == 0.0f) { + res.bias_rate = 0; + res.mass_scale = 1; + res.impulse_scale = 0; + } else { + f32 angular_frequency = TAU * hertz; + f32 a = 2 * damping_ratio + angular_frequency * dt; + f32 b = angular_frequency * a * dt; + f32 c = 1 / (b + 1); + res.bias_rate = angular_frequency / a; + res.mass_scale = b * c; + res.impulse_scale = c; + } + return res; +} + #endif