From ea56dab9bfc9200f92a73ee0e8e4a4d58e271a43 Mon Sep 17 00:00:00 2001 From: jacob Date: Wed, 11 Sep 2024 16:22:04 -0500 Subject: [PATCH] kinda working physics --- src/entity.c | 54 +++++++++++++++++- src/entity.h | 30 ++++------ src/game.c | 153 +++++++++++++++++++++------------------------------ 3 files changed, 125 insertions(+), 112 deletions(-) diff --git a/src/entity.c b/src/entity.c index aee2263e..739a3bf1 100644 --- a/src/entity.c +++ b/src/entity.c @@ -1,7 +1,7 @@ #include "entity.h" #include "math.h" -/* Offset in bytes from start of entities array to start of store struct */ +/* Offset in bytes from start of store struct to start of entities array (assume adjacently allocated) */ #define STORE_ENTITIES_OFFSET (sizeof(struct entity_store) + (sizeof(struct entity_store) % alignof(struct entity))) /* Accessed via entity_store_nil() */ @@ -298,6 +298,58 @@ void entity_set_local_xform(struct entity *ent, struct xform xf) } } +/* ========================== * + * Impulse + * ========================== */ + +void entity_apply_linear_impulse(struct entity *ent, struct v2 impulse, struct v2 point) +{ + struct xform xf = entity_get_xform(ent); + struct v2 center = xf.og; + f32 scale = math_fabs(xform_get_determinant(xf)); + f32 inv_mass = 1.f / (ent->mass_unscaled * scale); + f32 inv_inertia = 1.f / (ent->inertia_unscaled * scale); + + struct v2 vcp = v2_sub(point, center); + ent->linear_velocity = v2_add(ent->linear_velocity, v2_mul(impulse, inv_mass)); + ent->angular_velocity += -v2_wedge(impulse, vcp) * inv_inertia; +} + +#if 0 +void entity_apply_force(struct entity *ent, struct v2 impulse, struct v2 world_point) +{ +} +#endif + +void entity_apply_linear_impulse_to_center(struct entity *ent, struct v2 impulse) +{ + struct xform xf = entity_get_xform(ent); + f32 scale = math_fabs(xform_get_determinant(xf)); + f32 inv_mass = 1.f / (ent->mass_unscaled * scale); + + ent->linear_velocity = v2_add(ent->linear_velocity, v2_mul(impulse, inv_mass)); +} + +void entity_apply_force_to_center(struct entity *ent, struct v2 force) +{ + ent->force = v2_add(ent->force, force); +} + +void entity_apply_angular_impulse(struct entity *ent, f32 impulse) +{ + struct xform xf = entity_get_xform(ent); + f32 scale = math_fabs(xform_get_determinant(xf)); + f32 inv_inertia = 1.f / (ent->inertia_unscaled * scale); + + ent->angular_velocity += impulse * inv_inertia; +} + +#if 0 +void entity_apply_torque(struct entity *ent, f32 torque) +{ +} +#endif + /* ========================== * * Tree * ========================== */ diff --git a/src/entity.h b/src/entity.h index 2e570955..df1de502 100644 --- a/src/entity.h +++ b/src/entity.h @@ -12,10 +12,6 @@ enum entity_prop { ENTITY_PROP_RELEASE, ENTITY_PROP_PHYSICAL, - ENTITY_PROP_LINEAR_IMPULSE, - ENTITY_PROP_ANGULAR_IMPULSE, - ENTITY_PROP_FORCE, - ENTITY_PROP_TORQUE, ENTITY_PROP_PLAYER_CONTROLLED, ENTITY_PROP_CAMERA, @@ -104,6 +100,7 @@ struct entity { b32 solved; b32 test_torque_applied; + b32 test_collided; @@ -144,28 +141,13 @@ struct entity { /* ENTITY_PROP_PHYSICAL */ f32 mass_unscaled; /* Mass of entity in kg before any transformations */ - f32 inertia_unscaled; /* Inertia of entity in kgm2 before any transformations */ + f32 inertia_unscaled; /* Inertia of entity in kg*m^2 before any transformations */ f32 ground_friction; struct v2 linear_velocity; /* m/s */ f32 angular_velocity; /* rad/s */ - /* ====================================================================== */ - /* Force */ - - /* ENTITY_PROP_LINEAR_IMPULSE */ - /* ENTITY_PROP_FORCE */ - - /* Applies force to parent entity */ struct v2 force; - - /* ====================================================================== */ - /* Force */ - - /* ENTITY_PROP_ANGULAR_IMPULSE */ - /* ENTITY_PROP_TORQUE */ - - /* Applies torque to parent entity */ f32 torque; /* ====================================================================== */ @@ -321,6 +303,14 @@ struct xform entity_get_local_xform(struct entity *ent); void entity_set_xform(struct entity *ent, struct xform xf); void entity_set_local_xform(struct entity *ent, struct xform xf); +/* Impulse */ +void entity_apply_linear_impulse(struct entity *ent, struct v2 impulse, struct v2 world_point); +void entity_apply_force(struct entity *ent, struct v2 impulse, struct v2 world_point); +void entity_apply_linear_impulse_to_center(struct entity *ent, struct v2 impulse); +void entity_apply_force_to_center(struct entity *ent, struct v2 force); +void entity_apply_angular_impulse(struct entity *ent, f32 impulse); +void entity_apply_torque(struct entity *ent, f32 torque); + /* Query */ struct entity_store *entity_get_store(struct entity *ent); struct entity *entity_from_handle(struct entity_store *store, struct entity_handle handle); diff --git a/src/game.c b/src/game.c index 651becb0..bcbcc6ae 100644 --- a/src/game.c +++ b/src/game.c @@ -127,7 +127,8 @@ INTERNAL void spawn_test_entities(void) //struct v2 pos = V2(0.374142020941, -0.246118023992); /* Touching glitch spot */ struct v2 size = V2(1, 1); //f32 r = PI / 4; - f32 r = 0; + f32 r = PI / 3; + //f32 r = 0; f32 skew = 0; struct entity *e = entity_alloc(root); @@ -142,12 +143,14 @@ INTERNAL void spawn_test_entities(void) e->sprite_span_name = STR("idle.two_handed"); entity_enable_prop(e, ENTITY_PROP_PLAYER_CONTROLLED); - e->control_force = 4500; + //e->control_force = 4500; + e->control_force = 1200; e->control_torque = 10; e->control.focus = V2(0, -1); entity_enable_prop(e, ENTITY_PROP_PHYSICAL); - e->mass_unscaled = 70; + e->mass_unscaled = 100; + e->inertia_unscaled = 5; e->ground_friction = 1000; //entity_enable_prop(e, ENTITY_PROP_TEST); @@ -188,7 +191,13 @@ INTERNAL void spawn_test_entities(void) e->sprite = sprite_tag_from_path(STR("res/graphics/box.ase")); entity_enable_prop(e, ENTITY_PROP_PHYSICAL); - e->mass_unscaled = 70; +#if 0 + e->mass_unscaled = 300; + e->inertia_unscaled = 500; +#else + e->mass_unscaled = F32_INFINITY; + e->inertia_unscaled = F32_INFINITY; +#endif //e->ground_friction = 1000; entity_set_xform(e, XFORM_TRS(.t = pos, .s = size, .r = rot)); @@ -572,6 +581,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * Create forces from control move * ========================== */ +#if 0 for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { struct entity *ent = &store->entities[entity_index]; if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue; @@ -584,6 +594,18 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) activate_now(f); } } +#else + for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { + struct entity *ent = &store->entities[entity_index]; + if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue; + + if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) { + struct v2 move = ent->control.move; + struct v2 force = v2_mul(move, ent->control_force); + entity_apply_force_to_center(ent, force); + } + } +#endif /* ========================== * * Create forces from control focus (aim) @@ -711,6 +733,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) f32 diff = final_xf_angle - cur_angle; if (math_fabs(diff) > angle_error_allowed) { +#if 0 #if 0 /* Create force */ struct entity *f = entity_alloc(ent); @@ -727,11 +750,9 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) /* Create force */ if (!ent->test_torque_applied) { ent->test_torque_applied = true; - struct entity *f = entity_alloc(ent); - entity_enable_prop(f, ENTITY_PROP_ANGULAR_IMPULSE); - f->torque = 10; - activate_now(f); + entity_apply_angular_impulse(ent, 10); } +#endif #endif } } @@ -743,6 +764,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * Create ground friction force * ========================== */ +#if 0 /* TODO: Do this globally rather than creating entities for constant forces */ for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { struct entity *ent = &store->entities[entity_index]; @@ -801,13 +823,11 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) /* TODO: Remove this (gravity test) */ struct xform xf = entity_get_xform(ent); f32 mass = ent->mass_unscaled * math_fabs(xform_get_determinant(xf)); - struct entity *impulse = entity_alloc(ent); - entity_enable_prop(impulse, ENTITY_PROP_FORCE); - impulse->force = v2_mul(V2(0, 9.81), mass); - activate_now(impulse); + entity_apply_force_to_center(ent, V2(0, 9.81 * mass)); #endif } } +#endif /* ========================== * * Integrate forces @@ -823,51 +843,25 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) f32 mass = ent->mass_unscaled * det_abs; f32 inertia = ent->inertia_unscaled * det_abs; - /* Determine acceleration from forces and impulses */ - struct v2 tick_linear_acceleration = V2(0, 0); - f32 tick_angular_acceleration = 0; - for (struct entity *child = entity_from_handle(store, ent->first); child->valid; child = entity_from_handle(store, child->next)) { - if (entity_has_prop(child, ENTITY_PROP_LINEAR_IMPULSE)) { - tick_linear_acceleration = v2_add(tick_linear_acceleration, child->force); - entity_enable_prop(child, ENTITY_PROP_RELEASE); - } else if (entity_has_prop(child, ENTITY_PROP_FORCE)) { - tick_linear_acceleration = v2_add(tick_linear_acceleration, v2_mul(child->force, dt)); - entity_enable_prop(child, ENTITY_PROP_RELEASE); - } else if (entity_has_prop(child, ENTITY_PROP_ANGULAR_IMPULSE)) { - tick_angular_acceleration += child->torque; - entity_enable_prop(child, ENTITY_PROP_RELEASE); - } else if (entity_has_prop(child, ENTITY_PROP_TORQUE)) { - tick_angular_acceleration += child->torque * dt; - entity_enable_prop(child, ENTITY_PROP_RELEASE); - } - } - tick_linear_acceleration = v2_div(tick_linear_acceleration, mass); - tick_angular_acceleration = tick_angular_acceleration / inertia; + /* Determine force & torque acceleration */ + struct v2 tick_force_accel = v2_mul(v2_div(ent->force, mass), dt); + f32 tick_torque_accel = ent->torque / inertia * dt; /* Integrate */ - struct v2 tick_linear_velocity = v2_add(v2_mul(ent->linear_velocity, dt), v2_mul(tick_linear_acceleration, dt)); - f32 tick_angular_velocity = (ent->angular_velocity * dt) + (tick_angular_acceleration * dt); + struct v2 tick_linear_velocity = v2_add(v2_mul(ent->linear_velocity, dt), v2_mul(tick_force_accel, dt)); + f32 tick_angular_velocity = (ent->angular_velocity * dt) + (tick_torque_accel * dt); + xf.og = v2_add(xf.og, tick_linear_velocity); xf = xform_rotated(xf, tick_angular_velocity); + ent->linear_velocity = v2_div(tick_linear_velocity, dt); ent->angular_velocity = tick_angular_velocity / dt; + ent->force = V2(0, 0); + ent->torque = 0; + entity_set_xform(ent, xf); } - /* ========================== * - * Release impulses - * ========================== */ - - for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { - struct entity *ent = &store->entities[entity_index]; - if (!(ent->valid && entity_has_prop(ent, ENTITY_PROP_ACTIVE))) continue; - - /* TODO: Easier checking for single entity with multiple properties */ - if (entity_has_prop(ent, ENTITY_PROP_LINEAR_IMPULSE) || entity_has_prop(ent, ENTITY_PROP_ANGULAR_IMPULSE) || entity_has_prop(ent, ENTITY_PROP_FORCE) || entity_has_prop(ent, ENTITY_PROP_TORQUE)) { - entity_enable_prop(ent, ENTITY_PROP_RELEASE); - } - } - #if 1 /* ========================== * * Collision @@ -977,13 +971,14 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) struct v2 pen = v2_sub(p1, p0); struct v2 pen_norm = v2_norm(pen); -#if 0 - f32 bias_factor = 0.3; - f32 bias_slop = 0.05; - struct v2 bias = v2_sub(v2_mul(pen, bias_factor), v2_mul(pen_norm, bias_slop)); - if (v2_dot(pen, bias) < 0) { - bias = V2(0, 0); - } +#if 1 + f32 bias_factor = 10; + f32 bias_slop = 0.01; + + f32 bias = (v2_len(pen) * bias_factor) - bias_slop; + bias = max(bias, 0); +#else + f32 bias = 0; #endif @@ -998,24 +993,14 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) f32 vn = v2_dot(vrel, pen_norm); - - - - - //f32 ma = 1 / (vcp0 / v2_len(vcp0) - - - - struct v2 idk0 = v2_perp_mul(vcp0, v2_wedge(vcp0, pen_norm) * inv_i0); struct v2 idk1 = v2_perp_mul(vcp1, v2_wedge(vcp1, pen_norm) * inv_i1); f32 k = inv_m0 + inv_m1 + v2_dot(pen_norm, v2_add(idk0, idk1)); - /* (to be applied along n) */ - f32 j = vn / k; - //j = max_f32(j, 0); + f32 j = (vn + bias) / k; + j = max_f32(j, 0); @@ -1023,32 +1008,21 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) DEBUGBREAKABLE; } - + if (!e0->test_collided) { + e0->test_collided = true; + } struct v2 imp = v2_mul(pen_norm, j); (UNUSED)imp; - - - - + //imp = v2_mul(imp, dt); #if 1 - { - struct entity *impulse = entity_alloc(e0); - entity_enable_prop(impulse, ENTITY_PROP_LINEAR_IMPULSE); - impulse->force = v2_mul(imp, m0); - activate_now(impulse); - } - { - struct entity *impulse = entity_alloc(e1); - entity_enable_prop(impulse, ENTITY_PROP_LINEAR_IMPULSE); - impulse->force = v2_mul(v2_neg(imp), m1); - activate_now(impulse); - } + entity_apply_linear_impulse(e0, imp, p0); + entity_apply_linear_impulse(e1, v2_neg(imp), p1); #endif } @@ -1072,17 +1046,14 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) struct xform src_xf = entity_get_xform(src); struct v2 pos = xform_mul_v2(src_xf, ent->bullet_src_pos); - struct v2 velocity = xform_basis_mul_v2(src_xf, ent->bullet_src_dir); - velocity = v2_mul(v2_norm(velocity), ent->bullet_impulse); + struct v2 impulse = xform_basis_mul_v2(src_xf, ent->bullet_src_dir); + impulse = v2_mul(v2_norm(impulse), ent->bullet_impulse); - struct xform xf = XFORM_TRS(.t = pos, .r = v2_angle(velocity) + PI / 2); + struct xform xf = XFORM_TRS(.t = pos, .r = v2_angle(impulse) + PI / 2); entity_set_xform(ent, xf); entity_enable_prop(ent, ENTITY_PROP_PHYSICAL); - /* Create impulse */ - struct entity *impulse = entity_alloc(ent); - impulse->force = velocity; - entity_enable_prop(impulse, ENTITY_PROP_LINEAR_IMPULSE); + entity_apply_linear_impulse_to_center(src, impulse); } }