From 5dde85a04aa15897978897afd6607e7ffc90c70c Mon Sep 17 00:00:00 2001 From: jacob Date: Mon, 28 Oct 2024 15:12:23 -0500 Subject: [PATCH] motor joint warm starting --- src/config.h | 2 +- src/entity.h | 6 +- src/game.c | 185 ++++++++++++--------------------------------------- 3 files changed, 47 insertions(+), 146 deletions(-) diff --git a/src/config.h b/src/config.h index 3eb9323f..2d638874 100644 --- a/src/config.h +++ b/src/config.h @@ -33,7 +33,7 @@ #define GAME_TIMESCALE 1.0 #define GAME_PHYSICS_SUBSTEPS 4 -#define GAME_PHYSICS_ENABLE_WARM_STARTING 0 +#define GAME_PHYSICS_ENABLE_WARM_STARTING 1 #define GAME_PHYSICS_ENABLE_RELAXATION 1 #define USER_DRAW_MENKOWSKI 0 diff --git a/src/entity.h b/src/entity.h index a34b1b6a..f29de786 100644 --- a/src/entity.h +++ b/src/entity.h @@ -103,7 +103,7 @@ struct contact_constraint { struct motor_joint_def { struct entity_handle e0; struct entity_handle e1; - f32 correction_factor; + f32 correction_rate; f32 max_force; f32 max_torque; }; @@ -111,7 +111,7 @@ struct motor_joint_def { struct motor_joint { struct entity_handle e0; struct entity_handle e1; - f32 correction_factor; + f32 correction_rate; f32 max_force; f32 max_torque; @@ -135,7 +135,7 @@ INLINE struct motor_joint motor_joint_from_def(struct motor_joint_def def) struct motor_joint res = ZI; res.e0 = def.e0; res.e1 = def.e1; - res.correction_factor = clamp_f32(def.correction_factor, 0, 1); + res.correction_rate = clamp_f32(def.correction_rate, 0, 1); res.max_force = def.max_force; res.max_torque = def.max_torque; return res; diff --git a/src/game.c b/src/game.c index 86df0f2f..c0a913b4 100644 --- a/src/game.c +++ b/src/game.c @@ -184,7 +184,7 @@ INTERNAL void spawn_test_entities(f32 offset) //e->control_force = 5000; e->control_force = 250; #endif - e->control_torque = 10; + e->control_torque = 500; e->control.focus = V2(0, -1); entity_enable_prop(e, ENTITY_PROP_PHYSICAL); @@ -371,6 +371,7 @@ INTERNAL void prepare_contacts(void) struct entity *e0 = &store->entities[e0_index]; if (!entity_is_valid_and_active(e0)) continue; if (!entity_has_prop(e0, ENTITY_PROP_PHYSICAL)) continue; + if (e0->local_collider.count <= 0) continue; struct xform e0_xf = entity_get_xform(e0); struct collider_shape e0_collider = e0->local_collider; @@ -380,6 +381,7 @@ INTERNAL void prepare_contacts(void) if (e1 == e0) continue; if (!entity_is_valid_and_active(e1)) continue; if (!entity_has_prop(e1, ENTITY_PROP_PHYSICAL)) continue; + if (e1->local_collider.count <= 0) continue; /* TODO: Remove this (temporary stop to prevent double-constraint creation) */ if (e0_index >= e1_index) { @@ -426,7 +428,6 @@ INTERNAL void prepare_contacts(void) CT_ASSERT(ARRAY_COUNT(constraint_ent->contact_constraint_data.points) == 2); CT_ASSERT(ARRAY_COUNT(res.points) == 2); - /* TODO: Move this down */ if (res.num_points > 0 || COLLIDER_DEBUG) { if (!constraint_ent) { @@ -618,10 +619,8 @@ INTERNAL void warm_start_contacts(void) f32 inv_num_points = 1.f / num_points; for (u32 i = 0; i < num_points; ++i) { struct contact_point *point = &constraint->points[i]; - 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 = 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 impulse = v2_add(v2_mul(normal, point->normal_impulse), v2_mul(tangent, point->tangent_impulse)); impulse = v2_mul(impulse, inv_num_points); @@ -745,10 +744,8 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) struct v2 tangent = v2_perp(normal); for (u32 point_index = 0; point_index < num_points; ++point_index) { struct 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 = 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 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0)); struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1)); @@ -835,8 +832,8 @@ INTERNAL void prepare_motor_joints(void) joint->point_local_e0 = V2(0, 0); joint->point_local_e1 = V2(0, 0); - struct v2 vcp0 = v2_sub(xform_invert_mul_v2(e0_xf, joint->point_local_e0), e0_xf.og); - struct v2 vcp1 = v2_sub(xform_invert_mul_v2(e1_xf, joint->point_local_e1), e1_xf.og); + struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, joint->point_local_e0), e0_xf.og); + struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, joint->point_local_e1), e1_xf.og); struct xform linear_mass_xf; linear_mass_xf.bx.x = inv_m0 + inv_m1 + vcp0.y * vcp0.y * inv_i0 + vcp1.y * vcp1.y * inv_i1; @@ -860,6 +857,33 @@ INTERNAL void prepare_motor_joints(void) INTERNAL void warm_start_motor_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_MOTOR_JOINT)) continue; + + struct motor_joint *joint = &joint_ent->motor_joint_data; + + struct entity *e0 = entity_from_handle(store, joint->e0); + struct entity *e1 = entity_from_handle(store, joint->e1); + + struct xform e0_xf = entity_get_xform(e0); + struct xform e1_xf = entity_get_xform(e1); + + f32 inv_m0 = joint->inv_m0; + f32 inv_m1 = joint->inv_m1; + f32 inv_i0 = joint->inv_i0; + f32 inv_i1 = joint->inv_i1; + + struct v2 vcp0 = v2_sub(xform_mul_v2(e0_xf, joint->point_local_e0), e0_xf.og); + struct v2 vcp1 = v2_sub(xform_mul_v2(e1_xf, joint->point_local_e1), e1_xf.og); + + e0->linear_velocity = v2_sub(e0->linear_velocity, v2_mul(joint->linear_impulse, inv_m0)); + e1->linear_velocity = v2_add(e1->linear_velocity, v2_mul(joint->linear_impulse, inv_m1)); + e0->angular_velocity -= (v2_wedge(joint->linear_impulse, vcp0) + joint->angular_impulse) * inv_i0; + e1->angular_velocity += (v2_wedge(joint->linear_impulse, vcp1) + joint->angular_impulse) * inv_i1; + } } INTERNAL void solve_motor_joints(f32 dt) @@ -888,26 +912,16 @@ INTERNAL void solve_motor_joints(f32 dt) f32 w0 = e0->angular_velocity; f32 w1 = e1->angular_velocity; - f32 correction_rate = joint->correction_factor / dt; + f32 correction_rate = joint->correction_rate / dt; /* Angular constraint */ { f32 max_impulse = joint->max_torque * dt; -#if 0 f32 angular_separation = math_unwind_angle(xform_get_rotation(e1_xf) - xform_get_rotation(e0_xf)); f32 angular_bias = angular_separation * correction_rate; - f32 vel_diff = math_unwind_angle(w1 - w0); - f32 impulse = -joint->angular_mass * (vel_diff + angular_bias); -#else - f32 angular_separation = math_unwind_angle(xform_get_rotation(e1_xf) - xform_get_rotation(e0_xf)); - f32 angular_bias = angular_separation * correction_rate; - - //f32 vel_diff = w1 - w0; - f32 vel_diff = 0; - f32 impulse = -joint->angular_mass * (vel_diff + angular_bias); -#endif + f32 impulse = -joint->angular_mass * (w1 - w0 + angular_bias); f32 old_impulse = joint->angular_impulse; joint->angular_impulse = clamp_f32(joint->angular_impulse + impulse, -max_impulse, max_impulse); @@ -1504,71 +1518,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * ========================== */ #if GAME_PLAYER_AIM -#if 0 - for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { - struct entity *ent = &store->entities[entity_index]; - if (!entity_is_valid_and_active(ent)) continue; - - if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) { - struct xform xf = entity_get_xform(ent); - struct xform sprite_xf = xform_mul(xf, ent->sprite_local_xform); - f32 old_angle = xform_get_rotation(xf); - - /* Solve for final angle using law of sines */ - f32 final_xf_angle; - { - struct v2 ent_pos = xf.og; - struct v2 focus_pos = v2_add(ent_pos, ent->control.focus); - - struct v2 sprite_hold_pos; - struct v2 sprite_hold_dir; - { - struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, ent->sprite); - struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("attach.wep"), ent->animation_frame); - sprite_hold_pos = slice.center; - sprite_hold_dir = slice.dir; - } - - struct v2 hold_dir = xform_basis_mul_v2(sprite_xf, sprite_hold_dir); - struct v2 hold_pos = xform_mul_v2(sprite_xf, sprite_hold_pos); - if (v2_eq(hold_pos, ent_pos)) { - /* If hold pos is same as origin (E.G if pivot is being used as hold pos), then move hold pos forward a tad to avoid issue */ - sprite_hold_pos = v2_add(sprite_hold_pos, V2(0, -1)); - hold_pos = xform_mul_v2(sprite_xf, sprite_hold_pos); - } - - f32 forward_hold_angle_offset; - { - struct xform xf_unrotated = xform_basis_with_rotation_world(xf, 0); - struct v2 hold_pos_unrotated = xform_mul_v2(xf_unrotated, xform_mul_v2(ent->sprite_local_xform, sprite_hold_pos)); - forward_hold_angle_offset = v2_angle_from_dirs(V2(0, -1), v2_sub(hold_pos_unrotated, xf_unrotated.og)); - } - - struct v2 hold_ent_dir = v2_sub(ent_pos, hold_pos); - struct v2 focus_ent_dir = v2_sub(ent_pos, focus_pos); - - f32 hold_ent_len = v2_len(hold_ent_dir); - f32 focus_ent_len = v2_len(focus_ent_dir); - - f32 final_hold_angle_btw_ent_and_focus = v2_angle_from_dirs(hold_ent_dir, hold_dir); - f32 final_focus_angle_btw_ent_and_hold = math_asin((math_sin(final_hold_angle_btw_ent_and_focus) * hold_ent_len) / focus_ent_len); - f32 final_ent_angle_btw_focus_and_hold = PI - (final_focus_angle_btw_ent_and_hold + final_hold_angle_btw_ent_and_focus); - - final_xf_angle = v2_angle_from_dirs(V2(0, -1), v2_sub(focus_pos, ent_pos)) + final_ent_angle_btw_focus_and_hold - forward_hold_angle_offset; - } - - if (!F32_IS_NAN(final_xf_angle)) { - const f32 angle_error_allowed = 0.001; - f32 diff = math_unwind_angle(final_xf_angle - old_angle); - if (math_fabs(diff) > angle_error_allowed) { - xf = xform_basis_rotated_world(xf, diff); - } - } - - entity_set_xform(ent, xf); - } - } -#else for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { struct entity *ent = &store->entities[entity_index]; if (!entity_is_valid_and_active(ent)) continue; @@ -1581,17 +1530,17 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) struct entity *joint_ent = entity_from_handle(store, ent->aim_joint); if (!entity_is_valid_and_active(joint_ent)) { joint_ent = entity_alloc(root); + entity_enable_prop(joint_ent, ENTITY_PROP_PHYSICAL); entity_enable_prop(joint_ent, ENTITY_PROP_MOTOR_JOINT); entity_enable_prop(joint_ent, ENTITY_PROP_ACTIVE); struct motor_joint_def def = ZI; def.e0 = joint_ent->handle; /* Re-using joint entity as e0 */ def.e1 = ent->handle; - def.correction_factor = 0.5; + def.correction_rate = 1.0; def.max_force = 0; - def.max_torque = 2500; + def.max_torque = ent->control_torque; joint_ent->motor_joint_data = motor_joint_from_def(def); - ent->aim_joint = joint_ent->handle; } @@ -1638,26 +1587,17 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) new_angle = math_unwind_angle(v2_angle_from_dirs(V2(0, -1), v2_sub(focus_pos, ent_pos)) + final_ent_angle_btw_focus_and_hold - forward_hold_angle_offset); } -#if 1 + joint_ent->angular_velocity = 0; if (!F32_IS_NAN(new_angle)) { const f32 angle_error_allowed = 0.001; struct xform joint_xf = entity_get_xform(joint_ent); f32 diff = math_unwind_angle(new_angle - xform_get_rotation(joint_xf)); if (math_fabs(diff) > angle_error_allowed) { - struct xform joint_ent_xf = xform_basis_with_rotation_world(joint_xf, new_angle); - entity_set_xform(joint_ent, joint_ent_xf); + joint_ent->angular_velocity = diff / dt; } } -#endif - -#if 0 - struct xform joint_ent_xf = xform_basis_with_rotation_world(entity_get_xform(joint_ent), new_angle); - entity_set_xform(joint_ent, joint_ent_xf); -#endif - } } -#endif #endif /* ========================== * @@ -1806,7 +1746,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * Update camera position * ========================== */ -#if 0 for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { struct entity *ent = &store->entities[entity_index]; if (!entity_is_valid_and_active(ent)) continue; @@ -1843,44 +1782,6 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) ent->camera_applied_lerp_continuity_gen_plus_one = ent->camera_lerp_continuity_gen + 1; entity_set_xform(ent, xf); } -#else - for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { - struct entity *ent = &store->entities[entity_index]; - if (!entity_is_valid_and_active(ent)) continue; - - /* Camera follow */ - if (entity_has_prop(ent, ENTITY_PROP_CAMERA)) { - struct entity *follow = entity_from_handle(store, ent->camera_follow); - - struct xform xf = entity_get_xform(ent); - - f32 aspect_ratio = 1.0; - { - struct xform quad_xf = xform_mul(entity_get_xform(ent), ent->camera_quad_xform); - struct v2 camera_size = xform_get_scale(quad_xf); - if (!v2_is_zero(camera_size)) { - aspect_ratio = camera_size.x / camera_size.y; - } - } - f32 ratio_y = 0.33f; - f32 ratio_x = ratio_y / aspect_ratio; - struct v2 camera_focus_dir = v2_mul_v2(follow->control.focus, V2(ratio_x, ratio_y)); - struct v2 camera_focus_pos = v2_add(entity_get_xform(follow).og, camera_focus_dir); - ent->camera_xform_target = xf; - ent->camera_xform_target.og = camera_focus_pos; - - /* Lerp camera */ - if (ent->camera_applied_lerp_continuity_gen_plus_one == ent->camera_lerp_continuity_gen + 1) { - f32 t = 1 - math_pow(2.f, -20.f * (f32)G.tick.dt); - xf = xform_lerp(xf, ent->camera_xform_target, t); - } else { - /* Skip lerp */ - xf = ent->camera_xform_target; - } - ent->camera_applied_lerp_continuity_gen_plus_one = ent->camera_lerp_continuity_gen + 1; - entity_set_xform(ent, xf); - } -#endif } /* ========================== *