diff --git a/src/app.c b/src/app.c index bcb3d8d3..876b6aae 100644 --- a/src/app.c +++ b/src/app.c @@ -20,6 +20,7 @@ #include "draw.h" #include "math.h" #include "renderer.h" +#include "phys.h" struct exit_callback { app_exit_callback_func *func; @@ -213,7 +214,8 @@ void app_entry_point(void) struct mixer_startup_receipt mixer_sr = mixer_startup(); struct sound_startup_receipt sound_sr = sound_startup(&work_sr, &asset_cache_sr, &resource_sr); struct draw_startup_receipt draw_sr = draw_startup(&renderer_sr, &font_sr); - struct game_startup_receipt game_sr = game_startup(&mixer_sr, &sprite_sr, &sound_sr); + struct phys_startup_receipt phys_sr = phys_startup(); + struct game_startup_receipt game_sr = game_startup(&mixer_sr, &sprite_sr, &sound_sr, &phys_sr); struct user_startup_receipt user_sr = user_startup(&work_sr, &renderer_sr, &font_sr, &sprite_sr, &draw_sr, &game_sr, &asset_cache_sr, &mixer_sr, &window); struct playback_startup_receipt playback_sr = playback_startup(&mixer_sr); diff --git a/src/collider.c b/src/collider.c index 0f9911b1..eddb7d78 100644 --- a/src/collider.c +++ b/src/collider.c @@ -852,6 +852,88 @@ struct collider_closest_points_result collider_closest_points(struct collider_sh return res; } +/* ========================== * + * Time of impact + * ========================== */ + + /* Takes 2 shapes and their xforms at t=0 and t=1. + * Returns time of impact in range [0, 1]. */ +f32 collider_time_of_impact(struct collider_shape *c0, struct collider_shape *c1, + struct xform xf0_t0, struct xform xf1_t0, + struct xform xf0_t1, struct xform xf1_t1, + f32 tolerance, u32 max_iterations) +{ + f32 t0 = 0; + f32 t1 = 1; + f32 t0_sep = 0; + f32 t1_sep = 0; + f32 t = 0; + f32 t_sep = F32_INFINITY; + + /* Find direction p0 -> p1 at t=0 */ + struct v2 dir; + struct v2 dir_neg; + { + struct collider_closest_points_result closest_points_res = collider_closest_points(c0, c1, xf0_t0, xf1_t0); + if (closest_points_res.colliding) { + /* Shapes are penetrating at t=0 */ + return 0; + } + dir = v2_sub(closest_points_res.p1, closest_points_res.p0); + t0_sep = v2_len(dir); + dir = v2_div(dir, t0_sep); /* Normalize */ + dir_neg = v2_neg(dir); + } + + { + struct v2 p0 = collider_support_point(c0, xf0_t1, dir); + struct v2 p1 = collider_support_point(c1, xf1_t1, dir_neg); + t1_sep = v2_dot(dir, v2_sub(p1, p0)); + if (t1_sep > 0) { + /* Shapes are not penetrating at t=1 */ + return 1; + } + } + + u32 iteration = 0; + while (math_fabs(t_sep) > tolerance) { + if (iteration >= max_iterations) { + break; + } + + /* Use mix of bisection & false position method to find root + * (as described in https://box2d.org/files/ErinCatto_ContinuousCollision_GDC2013.pdf) */ + if (iteration & 1) { + /* Bisect */ + t = (t1 + t0) / 2.0; + } else { + /* False position (fastest for linear case) */ + f32 m = (t1_sep - t0_sep) / (t1 - t0); + t = (-t1_sep / m) + t1; + } + + struct xform xf0 = xform_lerp(xf0_t0, xf0_t1, t); + struct xform xf1 = xform_lerp(xf1_t0, xf1_t1, t); + + struct v2 p0 = collider_support_point(c0, xf0, dir); + struct v2 p1 = collider_support_point(c1, xf1, dir_neg); + t_sep = v2_dot(dir, v2_sub(p1, p0)); + + /* Update bracket */ + if (t_sep > 0) { + t0 = t; + t0_sep = t_sep; + } else { + t1 = t; + t1_sep = t_sep; + } + + ++iteration; + } + + return t; +} + /* ========================== * * Debug functions * TODO: Remove these diff --git a/src/collider.h b/src/collider.h index c8f3a2fd..e4e79682 100644 --- a/src/collider.h +++ b/src/collider.h @@ -5,13 +5,6 @@ extern u32 collider_debug_steps; #endif -struct v2 collider_support_point(struct collider_shape *a, struct xform xf, struct v2 dir); - -#if 0 -/* Returns simple true or false indicating shape collision */ -b32 collider_collision_boolean(struct collider_shape *shape0, struct collider_shape *shape1); -#endif - struct collider_menkowski_point { struct v2 p; /* Menkowski difference point */ struct v2 s0; /* Support point of first shape in dir */ @@ -47,8 +40,6 @@ struct collider_collision_points_result { struct v2 a0, b0, a1, b1; /* Clipping faces */ }; -struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1); - struct collider_closest_points_result { struct v2 p0, p1; b32 colliding; @@ -59,8 +50,14 @@ struct collider_closest_points_result { struct collider_prototype prototype; }; +struct v2 collider_support_point(struct collider_shape *a, struct xform xf, struct v2 dir); + +struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1); + struct collider_closest_points_result collider_closest_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1); +f32 collider_time_of_impact(struct collider_shape *c0, struct collider_shape *c1, struct xform xf0_t0, struct xform xf1_t0, struct xform xf0_t1, struct xform xf1_t1, f32 tolerance, u32 max_iterations); + struct v2_array menkowski(struct arena *arena, struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1, u32 detail); struct v2_array cloud(struct arena *arena, struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1); diff --git a/src/common.h b/src/common.h index 92cd0dcd..4f99ca38 100644 --- a/src/common.h +++ b/src/common.h @@ -491,6 +491,11 @@ struct renderer_handle { u64 v[1]; }; +struct entity_handle { + u64 idx; + u64 gen; +}; + /* ========================== * * Tag structs * ========================== */ diff --git a/src/entity.c b/src/entity.c index 5160c433..75932fbe 100644 --- a/src/entity.c +++ b/src/entity.c @@ -397,3 +397,125 @@ void entity_unlink_parent(struct entity *ent) ent->prev = entity_nil_handle(); ent->next = entity_nil_handle(); } + +/* ========================== * + * Entity lookup + * ========================== */ + +struct entity_lookup entity_lookup_alloc(u64 num_buckets) +{ + ASSERT(num_buckets > 0); + struct entity_lookup l = ZI; + l.arena = arena_alloc(GIGABYTE(64)); + l.buckets = arena_push_array_zero(&l.arena, struct entity_lookup_bucket, num_buckets); + l.num_buckets = num_buckets; + return l; +} + +void entity_lookup_release(struct entity_lookup *l) +{ + arena_release(&l->arena); +} + +struct entity_lookup_entry *entity_lookup_get(struct entity_lookup *l, struct entity_lookup_key key) +{ + u64 index = key.hash % l->num_buckets; + struct entity_lookup_bucket *bucket = &l->buckets[index]; + struct entity_lookup_entry *res = NULL; + for (struct entity_lookup_entry *e = bucket->first; e; e = e->next) { + if (e->key.hash == key.hash) { + res = e; + break; + } + } + return res; +} + +void entity_lookup_set(struct entity_lookup *l, struct entity_lookup_key key, struct entity_handle handle) +{ + u64 index = key.hash % l->num_buckets; + struct entity_lookup_bucket *bucket = &l->buckets[index]; + + struct entity_lookup_entry *prev = NULL; + struct entity_lookup_entry **slot = &bucket->first; + while (*slot) { + if ((*slot)->key.hash == key.hash) { + break; + } + prev = *slot; + slot = &(*slot)->next; + } + + struct entity_lookup_entry *entry = *slot; + if (entry) { + /* Set existing entry */ + entry->entity = handle; + } else { + /* Allocate entry */ + if (l->first_free_entry) { + entry = l->first_free_entry; + l->first_free_entry->prev = NULL; + l->first_free_entry = entry->next; + } else { + entry = arena_push(&l->arena, struct entity_lookup_entry); + } + MEMZERO_STRUCT(entry); + + entry->key = key; + entry->entity = handle; + if (prev) { + entry->prev = prev; + prev->next = entry; + } + + bucket->last = entry; + *slot = entry; + } +} + +void entity_lookup_remove(struct entity_lookup *l, struct entity_lookup_entry *entry) +{ + struct entity_lookup_bucket *bucket = &l->buckets[entry->key.hash % l->num_buckets]; + struct entity_lookup_entry *prev = entry->prev; + struct entity_lookup_entry *next = entry->next; + + if (prev) { + prev->next = next; + } else { + bucket->first = next; + } + + if (next) { + next->prev = prev; + } else { + bucket->last = prev; + } + + if (l->first_free_entry) { + l->first_free_entry->prev = entry; + } + entry->next = l->first_free_entry; + entry->prev = NULL; + l->first_free_entry = entry; +} + +struct entity_lookup_key entity_lookup_key_from_two_handles(struct entity_handle h0, struct entity_handle h1) +{ + struct entity_lookup_key key = ZI; + struct buffer b0 = BUFFER_FROM_STRUCT(&h0); + struct buffer b1 = BUFFER_FROM_STRUCT(&h1); + key.hash = hash_fnv64(HASH_FNV64_BASIS, b0); + key.hash = hash_fnv64(key.hash, b1); + return key; +} + +/* ========================== * + * Activate + * ========================== */ + +void entity_activate(struct entity *ent, u64 current_tick_id) +{ + entity_enable_prop(ent, ENTITY_PROP_ACTIVE); + ent->activation_tick = current_tick_id; + ++ent->continuity_gen; +} diff --git a/src/entity.h b/src/entity.h index 02622d1d..206bf7ab 100644 --- a/src/entity.h +++ b/src/entity.h @@ -3,6 +3,7 @@ #include "sprite.h" #include "mixer.h" +#include "phys.h" enum entity_prop { ENTITY_PROP_NONE, @@ -44,11 +45,6 @@ enum entity_prop { ENTITY_PROP_COUNT }; -struct entity_handle { - u64 idx; - u64 gen; -}; - struct entity_store { struct arena arena; u64 allocated; @@ -58,154 +54,29 @@ struct entity_store { struct entity *entities; }; - - - - - - -/* TODO: Remove this */ -#include "collider.h" -#include "math.h" - -struct contact_point { - /* Contact point in local space of each entity */ - struct v2 point_local_e0; - struct v2 point_local_e1; - - u32 id; /* ID generated during clipping */ - f32 starting_separation; /* How far are original points along normal */ - f32 normal_impulse; /* Accumulated impulse along normal */ - f32 tangent_impulse; /* Accumulated impulse along tangent */ - - f32 inv_normal_mass; - f32 inv_tangent_mass; - - /* Debugging */ - struct v2 dbg_pt; +struct entity_lookup_key { + u64 hash; }; -struct contact_constraint { - u64 last_updated_tick; /* To avoid checking collisions for the same constraint twice in one tick */ - b32 skip_solve; - struct entity_handle e0; - struct entity_handle e1; - f32 inv_m0; - f32 inv_m1; - f32 inv_i0; - f32 inv_i1; - - struct v2 normal; /* Normal vector of collision from e0 -> e1 */ - u64 last_iteration; - struct contact_point points[2]; - u32 num_points; - - f32 friction; - - struct math_spring_result softness; - f32 pushout_velocity; +struct entity_lookup_entry { + struct entity_lookup_key key; + struct entity_handle entity; + struct entity_lookup_entry *next; + struct entity_lookup_entry *prev; }; -struct collision_debug { - struct entity_handle e0; - struct entity_handle e1; - struct collider_collision_points_result res; - - struct contact_point points[2]; - u32 num_points; - - struct v2 closest0; - struct v2 closest1; - - struct xform xf0; - struct xform xf1; +struct entity_lookup_bucket { + struct entity_lookup_entry *first; + struct entity_lookup_entry *last; }; - - - - - -struct motor_joint_def { - struct entity_handle e0; - struct entity_handle e1; - f32 correction_rate; - f32 max_force; - f32 max_torque; +struct entity_lookup { + struct arena arena; + struct entity_lookup_bucket *buckets; + u64 num_buckets; + struct entity_lookup_entry *first_free_entry; }; -struct motor_joint { - struct entity_handle e0; - struct entity_handle e1; - f32 correction_rate; - f32 max_force; - f32 max_torque; - - f32 inv_m0; - f32 inv_m1; - f32 inv_i0; - f32 inv_i1; - - struct v2 linear_impulse; - f32 angular_impulse; - - struct v2 point_local_e0; - struct v2 point_local_e1; - - struct xform linear_mass_xf; - f32 angular_mass; -}; - -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_rate = clamp_f32(def.correction_rate, 0, 1); - res.max_force = def.max_force; - res.max_torque = def.max_torque; - return res; -} - - - - - - -struct mouse_joint { - struct entity_handle target; - struct v2 point_local_start; - struct v2 point_local_end; - struct math_spring_result linear_softness; - struct math_spring_result angular_softness; - f32 max_force; - - f32 inv_m; - f32 inv_i; - - struct v2 linear_impulse; - f32 angular_impulse; - - struct xform linear_mass_xf; -}; - - - -struct hit_event { - struct entity_handle e0; - struct entity_handle e1; - struct v2 point; - struct v2 normal; - struct v2 vrel; /* Relative velocity */ -}; - - - - - - - - struct entity { /* ====================================================================== */ /* Metadata */ @@ -229,50 +100,38 @@ struct entity { struct entity_handle first; struct entity_handle last; - - - - - - - - /* TODO: Remove this (testing) */ - i32 colliding; - struct collision_debug collision_debug_data; - - /* ====================================================================== */ /* Collider */ struct collider_shape local_collider; +#if COLLIDER_DEBUG + i32 colliding; + struct phys_collision_debug collision_debug_data; +#endif + /* ====================================================================== */ /* Contact constraint */ /* ENTITY_PROP_CONSTRAINT_CONTACT */ - struct contact_constraint contact_constraint_data; + struct phys_contact_constraint contact_constraint_data; /* ====================================================================== */ /* Motor joint */ /* ENTITY_PROP_MOTOR_JOINT */ - struct motor_joint motor_joint_data; + struct phys_motor_joint motor_joint_data; /* ====================================================================== */ /* Mouse joint */ /* ENTITY_PROP_MOUSE_JOINT */ - struct mouse_joint mouse_joint_data; + struct phys_mouse_joint mouse_joint_data; /* ====================================================================== */ /* Hit event */ - struct hit_event hit_event; - - - - - + struct phys_hit_event hit_event; /* ====================================================================== */ /* Activation */ @@ -503,4 +362,15 @@ struct entity *entity_find_first_match_all(struct entity_store *store, struct en void entity_link_parent(struct entity *parent, struct entity *child); void entity_unlink_parent(struct entity *ent); +/* Lookup */ +struct entity_lookup entity_lookup_alloc(u64 num_buckets); +void entity_lookup_release(struct entity_lookup *l); +struct entity_lookup_entry *entity_lookup_get(struct entity_lookup *l, struct entity_lookup_key key); +void entity_lookup_set(struct entity_lookup *l, struct entity_lookup_key key, struct entity_handle handle); +void entity_lookup_remove(struct entity_lookup *l, struct entity_lookup_entry *entry); +struct entity_lookup_key entity_lookup_key_from_two_handles(struct entity_handle h0, struct entity_handle h1); + +/* Activate */ +void entity_activate(struct entity *ent, u64 current_tick_id); + #endif diff --git a/src/game.c b/src/game.c index 32193233..7059f61d 100644 --- a/src/game.c +++ b/src/game.c @@ -10,31 +10,9 @@ #include "atomic.h" #include "app.h" #include "log.h" +#include "phys.h" #include "collider.h" -struct entity_lookup_key { - u64 hash; -}; - -struct entity_lookup_entry { - struct entity_lookup_key key; - struct entity_handle entity; - struct entity_lookup_entry *next; - struct entity_lookup_entry *prev; -}; - -struct entity_lookup_bucket { - struct entity_lookup_entry *first; - struct entity_lookup_entry *last; -}; - -struct entity_lookup { - struct arena arena; - struct entity_lookup_bucket *buckets; - u64 num_buckets; - struct entity_lookup_entry *first_free_entry; -}; - GLOBAL struct { struct atomic_i32 game_thread_shutdown; struct sys_thread game_thread; @@ -54,12 +32,6 @@ GLOBAL struct { struct arena game_cmds_arena; struct entity *root; - /* Constants */ - struct math_spring_result contact_softness; - struct math_spring_result mouse_joint_linear_softness; - struct math_spring_result mouse_joint_angular_softness; - f32 mouse_joint_max_force; - /* Bookkeeping structures */ struct entity_lookup contact_lookup; #if COLLIDER_DEBUG @@ -84,28 +56,13 @@ INTERNAL void reset_world(void); struct game_startup_receipt game_startup(struct mixer_startup_receipt *mixer_sr, struct sprite_startup_receipt *sheet_sr, - struct sound_startup_receipt *sound_sr) + struct sound_startup_receipt *sound_sr, + struct phys_startup_receipt *phys_sr) { (UNUSED)mixer_sr; (UNUSED)sheet_sr; (UNUSED)sound_sr; - - /* Initialize constants */ - { - const f32 substep_dt = (1.f / ((f32)GAME_FPS * (f32)GAME_PHYSICS_SUBSTEPS)); - - const f32 contact_frequency = (1.f / substep_dt) / 8.f; - const f32 contact_damping_ratio = 10; - G.contact_softness = math_spring(contact_frequency, contact_damping_ratio, substep_dt); - - const f32 mouse_joint_linear_frequency = 5.0; - const f32 mouse_joint_linear_damping_ratio = 0.7; - const f32 mouse_joint_angular_frequency = 0.5f; - const f32 mouse_joint_angular_damping_ratio = 0.1f; - G.mouse_joint_max_force = 1000; - G.mouse_joint_linear_softness = math_spring(mouse_joint_linear_frequency, mouse_joint_linear_damping_ratio, substep_dt); - G.mouse_joint_angular_softness = math_spring(mouse_joint_angular_frequency, mouse_joint_angular_damping_ratio, substep_dt); - } + (UNUSED)phys_sr; /* Initialize game cmd storage */ G.game_cmds_mutex = sys_mutex_alloc(); @@ -161,128 +118,6 @@ INTERNAL struct game_cmd_array pop_cmds(struct arena *arena) return array; } -/* ========================== * - * Activate - * ========================== */ - -INTERNAL void activate_now(struct entity *ent) -{ - entity_enable_prop(ent, ENTITY_PROP_ACTIVE); - ent->activation_tick = G.tick.tick_id; - ++ent->continuity_gen; -} - -/* ========================== * - * Entity lookup - * ========================== */ - -INTERNAL struct entity_lookup entity_lookup_alloc(u64 num_buckets) -{ - ASSERT(num_buckets > 0); - struct entity_lookup l = ZI; - l.arena = arena_alloc(GIGABYTE(64)); - l.buckets = arena_push_array_zero(&l.arena, struct entity_lookup_bucket, num_buckets); - l.num_buckets = num_buckets; - return l; -} - -INTERNAL void entity_lookup_release(struct entity_lookup *l) -{ - arena_release(&l->arena); -} - -INTERNAL struct entity_lookup_entry *entity_lookup_get(struct entity_lookup *l, struct entity_lookup_key key) -{ - u64 index = key.hash % l->num_buckets; - struct entity_lookup_bucket *bucket = &l->buckets[index]; - struct entity_lookup_entry *res = NULL; - for (struct entity_lookup_entry *e = bucket->first; e; e = e->next) { - if (e->key.hash == key.hash) { - res = e; - break; - } - } - return res; -} - -INTERNAL void entity_lookup_set(struct entity_lookup *l, struct entity_lookup_key key, struct entity_handle handle) -{ - u64 index = key.hash % l->num_buckets; - struct entity_lookup_bucket *bucket = &l->buckets[index]; - - struct entity_lookup_entry *prev = NULL; - struct entity_lookup_entry **slot = &bucket->first; - while (*slot) { - if ((*slot)->key.hash == key.hash) { - break; - } - prev = *slot; - slot = &(*slot)->next; - } - - struct entity_lookup_entry *entry = *slot; - if (entry) { - /* Set existing entry */ - entry->entity = handle; - } else { - /* Allocate entry */ - if (l->first_free_entry) { - entry = l->first_free_entry; - l->first_free_entry->prev = NULL; - l->first_free_entry = entry->next; - } else { - entry = arena_push(&l->arena, struct entity_lookup_entry); - } - MEMZERO_STRUCT(entry); - - entry->key = key; - entry->entity = handle; - if (prev) { - entry->prev = prev; - prev->next = entry; - } - - bucket->last = entry; - *slot = entry; - } -} - -INTERNAL void entity_lookup_remove(struct entity_lookup *l, struct entity_lookup_entry *entry) -{ - struct entity_lookup_bucket *bucket = &l->buckets[entry->key.hash % l->num_buckets]; - struct entity_lookup_entry *prev = entry->prev; - struct entity_lookup_entry *next = entry->next; - - if (prev) { - prev->next = next; - } else { - bucket->first = next; - } - - if (next) { - next->prev = prev; - } else { - bucket->last = prev; - } - - if (l->first_free_entry) { - l->first_free_entry->prev = entry; - } - entry->next = l->first_free_entry; - entry->prev = NULL; - l->first_free_entry = entry; -} - -INTERNAL struct entity_lookup_key entity_lookup_key_from_two_handles(struct entity_handle h0, struct entity_handle h1) -{ - struct entity_lookup_key key = ZI; - struct buffer b0 = BUFFER_FROM_STRUCT(&h0); - struct buffer b1 = BUFFER_FROM_STRUCT(&h1); - key.hash = hash_fnv64(HASH_FNV64_BASIS, b0); - key.hash = hash_fnv64(key.hash, b1); - return key; -} - /* ========================== * * Reset * ========================== */ @@ -454,1200 +289,6 @@ INTERNAL void spawn_test_entities(void) G.extra_spawn = true; } - - - - -/* ========================== * - * TESTING CONTACT CONSTRAINT - * ========================== */ - -INTERNAL void create_contacts(void) -{ - struct entity_store *store = G.tick.entity_store; - struct entity *root = G.root; - - for (u64 check0_index = 0; check0_index < store->reserved; ++check0_index) { - struct entity *check0 = &store->entities[check0_index]; - if (!entity_is_valid_and_active(check0)) continue; - if (!(entity_has_prop(check0, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(check0, ENTITY_PROP_PHYSICAL_KINEMATIC))) continue; - if (check0->local_collider.count <= 0) continue; - - struct xform check0_xf = entity_get_xform(check0); - struct collider_shape check0_collider = check0->local_collider; - - for (u64 check1_index = 0; check1_index < store->reserved; ++check1_index) { - struct entity *check1 = &store->entities[check1_index]; - if (check1 == check0) continue; - if (!entity_is_valid_and_active(check1)) continue; - if (!(entity_has_prop(check1, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(check1, ENTITY_PROP_PHYSICAL_KINEMATIC))) continue; - if (check1->local_collider.count <= 0) continue; - - /* Deterministic entity order based on index */ - struct entity *e0; - struct entity *e1; - struct xform e0_xf; - struct xform e1_xf; - struct collider_shape e0_collider; - struct collider_shape e1_collider; - if (check0_index < check1_index) { - e0 = check0; - e1 = check1; - e0_xf = check0_xf; - e1_xf = entity_get_xform(check1); - e0_collider = check0_collider; - e1_collider = check1->local_collider; - } else { - e0 = check1; - e1 = check0; - e0_xf = entity_get_xform(check1); - e1_xf = check0_xf; - e0_collider = check1->local_collider; - e1_collider = check0_collider; - } - - struct entity_lookup_key key = entity_lookup_key_from_two_handles(e0->handle, e1->handle); - struct entity_lookup_entry *entry = entity_lookup_get(&G.contact_lookup, key); - - struct entity *constraint_ent = entity_nil(); - if (entry) { - constraint_ent = entity_from_handle(store, entry->entity); - if (entity_is_valid_and_active(constraint_ent)) { - if (constraint_ent->contact_constraint_data.last_iteration >= G.tick.tick_id) { - /* Already processed constraint this iteration */ - continue; - } else { - ++constraint_ent->contact_constraint_data.last_iteration; - } - } else { - /* Constraint ent no longer valid, delete entry */ - entity_lookup_remove(&G.contact_lookup, entry); - entry = NULL; - } - } - - /* Calculate collision */ - struct collider_collision_points_result collider_res = collider_collision_points(&e0_collider, &e1_collider, e0_xf, e1_xf); - - /* Parts of algorithm are hard-coded to support 2 contact points */ - CT_ASSERT(ARRAY_COUNT(constraint_ent->contact_constraint_data.points) == 2); - CT_ASSERT(ARRAY_COUNT(collider_res.points) == 2); - - struct contact_constraint *constraint = NULL; - if (collider_res.num_points > 0) { - if (!entity_is_valid_and_active(constraint_ent)) { - /* Create hit event */ - { - struct entity *event = entity_alloc(root); - entity_enable_prop(event, ENTITY_PROP_HIT_EVENT); - event->hit_event.e0 = e0->handle; - event->hit_event.e1 = e1->handle; - event->hit_event.normal = collider_res.normal; - entity_enable_prop(event, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); - entity_enable_prop(event, ENTITY_PROP_ACTIVE); - - /* Calculate point */ - struct v2 point = 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)); - } - event->hit_event.point = point; - - /* Calculate relative velocity */ - struct v2 vrel; - { - struct v2 v0 = e0->linear_velocity; - 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 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); - } - event->hit_event.vrel = vrel; - } - - /* Create constraint */ - { - constraint_ent = entity_alloc(root); - constraint_ent->contact_constraint_data.e1 = e1->handle; - constraint_ent->contact_constraint_data.e0 = e0->handle; - constraint_ent->contact_constraint_data.skip_solve = entity_has_prop(e0, ENTITY_PROP_SENSOR) || entity_has_prop(e1, ENTITY_PROP_SENSOR) - || !(entity_has_prop(e0, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(e1, ENTITY_PROP_PHYSICAL_DYNAMIC)); - entity_enable_prop(constraint_ent, ENTITY_PROP_ACTIVE); - - /* TODO: Should we recalculate normal as more contact points are added? */ - entity_enable_prop(constraint_ent, ENTITY_PROP_CONTACT_CONSTRAINT); - activate_now(constraint_ent); - ASSERT(!entry); /* Existing entry should never be present here */ - entity_lookup_set(&G.contact_lookup, key, constraint_ent->handle); - } - } - constraint = &constraint_ent->contact_constraint_data; - constraint->normal = collider_res.normal; - constraint->friction = math_sqrt(e0->friction * e1->friction); - - /* Delete old contacts that are no longer present */ - for (u32 i = 0; i < constraint->num_points; ++i) { - struct contact_point *old = &constraint->points[i]; - u32 id = old->id; - b32 found = false; - for (u32 j = 0; j < collider_res.num_points; ++j) { - if (collider_res.points[j].id == id) { - found = true; - break; - } - } - if (!found) { - /* Delete contact by replacing with last in array */ - *old = constraint->points[--constraint->num_points]; - --i; - } - } - - /* Update / insert returned contacts */ - for (u32 i = 0; i < collider_res.num_points; ++i) { - struct collider_collision_point *res_point = &collider_res.points[i]; - struct v2 point = res_point->point; - f32 sep = res_point->separation; - u32 id = res_point->id; - struct contact_point *contact = NULL; - /* Match */ - for (u32 j = 0; j < constraint->num_points; ++j) { - struct contact_point *t = &constraint->points[j]; - if (t->id == id) { - contact = t; - break; - } - } - if (!contact) { - /* Insert */ - contact = &constraint->points[constraint->num_points++]; - MEMZERO_STRUCT(contact); - contact->id = id; - constraint->softness = G.contact_softness; - 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; - -#if COLLIDER_DEBUG - contact->dbg_pt = point; -#endif - } - } else if (constraint_ent->valid) { - constraint_ent->contact_constraint_data.num_points = 0; - } - - /* TODO: Remove this (debugging) */ -#if COLLIDER_DEBUG - { - struct entity *dbg_ent = entity_nil(); - struct entity_lookup_entry *dbg_entry = entity_lookup_get(&G.collision_debug_lookup, key); - if (dbg_entry) { - dbg_ent = entity_from_handle(store, dbg_entry->entity); - } - - if (!dbg_ent->valid) { - /* FIXME: Entity never released */ - dbg_ent = entity_alloc(root); - entity_enable_prop(dbg_ent, ENTITY_PROP_COLLISION_DEBUG); - entity_lookup_set(&G.collision_debug_lookup, key, dbg_ent->handle); - } - - struct collision_debug *dbg = &dbg_ent->collision_debug_data; - - if (dbg->res.num_points == 0) { - if (collider_res.num_points > 0) { - ++e0->colliding; - ++e1->colliding; - } - } else { - if (collider_res.num_points == 0) { - --e0->colliding; - --e1->colliding; - } - } - dbg->e0 = e0->handle; - dbg->e1 = e1->handle; - dbg->res = collider_res; - - if (constraint) { - MEMCPY(dbg->points, constraint->points, sizeof(dbg->points)); - dbg->num_points = constraint->num_points; - } else { - dbg->num_points = 0; - } - - dbg->xf0 = e0_xf; - dbg->xf1 = e1_xf; - - /* Update closest points */ - { - struct collider_closest_points_result closest_points_res = collider_closest_points(&e0_collider, &e1_collider, e0_xf, e1_xf); - dbg->closest0 = closest_points_res.p0; - dbg->closest1 = closest_points_res.p1; - } - } -#endif - } - } -} - -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; - - if (entity_has_prop(e0, ENTITY_PROP_PHYSICAL_KINEMATIC)) { - constraint->inv_m0 = 0; - constraint->inv_i0 = 0; - } - if (entity_has_prop(e1, ENTITY_PROP_PHYSICAL_KINEMATIC)) { - constraint->inv_m1 = 0; - constraint->inv_i1 = 0; - } - - /* 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_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); - - /* 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 - } - } else { - /* Mark constraint for removal */ - entity_disable_prop(constraint_ent, ENTITY_PROP_ACTIVE); - entity_enable_prop(constraint_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); - /* Remove from lookup */ - struct entity_lookup_key key = entity_lookup_key_from_two_handles(constraint->e0, constraint->e1); - struct entity_lookup_entry *entry = entity_lookup_get(&G.contact_lookup, key); - if (entry) { - entity_lookup_remove(&G.contact_lookup, entry); - } else { - ASSERT(false); /* This should always exist */ - } - } - } - -#if COLLIDER_DEBUG - for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { - struct entity *dbg_ent = &store->entities[entity_index]; - if (!entity_is_valid_and_active(dbg_ent)) continue; - if (!entity_has_prop(dbg_ent, ENTITY_PROP_COLLISION_DEBUG)) continue; - - struct collision_debug *dbg = &dbg_ent->collision_debug_data; - struct entity *e0 = entity_from_handle(store, dbg->e0); - struct entity *e1 = entity_from_handle(store, dbg->e1); - - - if (!entity_is_valid_and_active(e0) || !entity_is_valid_and_active(e1) - || !(entity_has_prop(e0, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(e0, ENTITY_PROP_PHYSICAL_KINEMATIC)) - || !(entity_has_prop(e1, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(e1, ENTITY_PROP_PHYSICAL_KINEMATIC))) { - /* Mark dbg ent for removal */ - entity_disable_prop(dbg_ent, ENTITY_PROP_ACTIVE); - entity_enable_prop(dbg_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); - - /* Remove from lookup */ - struct entity_lookup_key key = entity_lookup_key_from_two_handles(dbg->e0, dbg->e1); - struct entity_lookup_entry *entry = entity_lookup_get(&G.collision_debug_lookup, key); - - if (e0->valid) { - --e0->colliding; - } - if (e1->valid) { - --e1->colliding; - } - - if (entry) { - entity_lookup_remove(&G.collision_debug_lookup, entry); - } else { - ASSERT(false); /* This should always exist */ - } - } - } -#endif -} - -INTERNAL void warm_start_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) && !constraint->skip_solve) { - struct xform e0_xf = entity_get_xform(e0); - struct xform e1_xf = entity_get_xform(e1); - - f32 inv_m0 = constraint->inv_m0; - f32 inv_m1 = constraint->inv_m1; - f32 inv_i0 = constraint->inv_i0; - f32 inv_i1 = constraint->inv_i1; - - struct v2 v0 = e0->linear_velocity; - struct v2 v1 = e1->linear_velocity; - f32 w0 = e0->angular_velocity; - f32 w1 = e1->angular_velocity; - - /* Warm start */ - struct v2 normal = constraint->normal; - struct v2 tangent = v2_perp(normal); - 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 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); - - v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); - v1 = v2_add(v1, v2_mul(impulse, inv_m1)); - w0 -= v2_wedge(vcp0, impulse) * inv_i0; - w1 += v2_wedge(vcp1, impulse) * inv_i1; - } - - e0->linear_velocity = v0; - e0->angular_velocity = w0; - e1->linear_velocity = v1; - e1->angular_velocity = w1; - } - } -} - -INTERNAL void solve_contacts(f32 dt, b32 apply_bias) -{ - 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; - - struct entity *e0 = entity_from_handle(store, constraint->e0); - struct entity *e1 = entity_from_handle(store, constraint->e1); - - struct v2 v0 = e0->linear_velocity; - struct v2 v1 = e1->linear_velocity; - f32 w0 = e0->angular_velocity; - f32 w1 = e1->angular_velocity; - - u32 num_points = constraint->num_points; - if (num_points > 0 && entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1) && !constraint->skip_solve) { - struct xform e0_xf = entity_get_xform(e0); - struct xform e1_xf = entity_get_xform(e1); - - f32 inv_m0 = constraint->inv_m0; - f32 inv_m1 = constraint->inv_m1; - f32 inv_i0 = constraint->inv_i0; - f32 inv_i1 = constraint->inv_i1; - - /* Normal impulse */ - struct v2 normal = constraint->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); - - f32 separation = v2_dot(v2_sub(p1, p0), normal) + point->starting_separation; - - f32 velocity_bias = 0.0f; - f32 mass_scale = 1.0f; - f32 impulse_scale = 0.0f; - - if (separation > 0.0f) { - /* Speculative */ - velocity_bias = separation / dt; - } else if (apply_bias) { - /* Soft constraint */ - 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)); - struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1)); - struct v2 vrel = v2_sub(vel0, vel1); - - f32 k = point->inv_normal_mass; - - /* (to be applied along n) */ - f32 vn = v2_dot(vrel, normal); - f32 j = ((k * mass_scale) * (vn - velocity_bias)) - (point->normal_impulse * impulse_scale); - - f32 old_impulse = point->normal_impulse; - f32 new_impulse = max_f32(old_impulse + j, 0); - f32 delta = new_impulse - old_impulse; - point->normal_impulse = new_impulse; - - struct v2 impulse = v2_mul(normal, delta); - v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); - v1 = v2_add(v1, v2_mul(impulse, inv_m1)); - w0 -= v2_wedge(vcp0, impulse) * inv_i0; - w1 += v2_wedge(vcp1, impulse) * inv_i1; - } - - /* Tangent impulse */ - 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 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)); - struct v2 vrel = v2_sub(vel0, vel1); - - f32 k = point->inv_tangent_mass; - - /* (to be applied along t) */ - f32 vt = v2_dot(vrel, tangent); - f32 j = vt * k; - - f32 max_friction = constraint->friction * point->normal_impulse; - f32 old_impulse = point->tangent_impulse; - f32 new_impulse = clamp_f32(old_impulse + j, -max_friction, max_friction); - f32 delta = new_impulse - old_impulse; - point->tangent_impulse = new_impulse; - - struct v2 impulse = v2_mul(tangent, delta); - v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); - v1 = v2_add(v1, v2_mul(impulse, inv_m1)); - w0 -= v2_wedge(vcp0, impulse) * inv_i0; - w1 += v2_wedge(vcp1, impulse) * inv_i1; - } - - e0->linear_velocity = v0; - e0->angular_velocity = w0; - e1->linear_velocity = v1; - e1->angular_velocity = w1; - } - } -} - - - - -/* ========================== * - * TESTING MOTOR JOINT - * ========================== */ - -INTERNAL void prepare_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); - - if (entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) { - 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); - } - joint->inv_m0 = inv_m0; - joint->inv_m1 = inv_m1; - joint->inv_i0 = inv_i0; - joint->inv_i1 = inv_i1; - - joint->point_local_e0 = V2(0, 0); - joint->point_local_e1 = V2(0, 0); - - 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; - linear_mass_xf.bx.y = -vcp0.y * vcp0.x * inv_i0 - vcp1.y * vcp1.x * inv_i1; - linear_mass_xf.by.x = linear_mass_xf.bx.y; - linear_mass_xf.by.y = inv_m0 + inv_m1 + vcp0.x * vcp0.x * inv_i0 + vcp1.x * vcp1.x * inv_i1; - joint->linear_mass_xf = xform_invert(linear_mass_xf); - - joint->angular_mass = 1.f / (inv_i0 + inv_i1); - -#if !GAME_PHYSICS_ENABLE_WARM_STARTING - joint->linear_impulse = V2(0, 0); - joint->angular_impulse = 0; -#endif - } else { - /* Mark joint for removal */ - entity_disable_prop(joint_ent, ENTITY_PROP_ACTIVE); - entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); - } - } -} - -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(vcp0, joint->linear_impulse) + joint->angular_impulse) * inv_i0; - e1->angular_velocity += (v2_wedge(vcp1, joint->linear_impulse) + joint->angular_impulse) * inv_i1; - } -} - -INTERNAL void solve_motor_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_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 v0 = e0->linear_velocity; - struct v2 v1 = e1->linear_velocity; - f32 w0 = e0->angular_velocity; - f32 w1 = e1->angular_velocity; - - f32 correction_rate = joint->correction_rate / dt; - - /* Angular constraint */ - { - f32 max_impulse = joint->max_torque * dt; - - f32 angular_separation = math_unwind_angle(xform_get_rotation(e1_xf) - xform_get_rotation(e0_xf)); - f32 angular_bias = angular_separation * correction_rate; - - 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); - impulse = joint->angular_impulse - old_impulse; - - w0 -= impulse * inv_i0; - w1 += impulse * inv_i1; - } - - /* Linear constraint */ - { - 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); - - f32 max_impulse = joint->max_force * dt; - - struct v2 linear_separation = v2_sub(v2_add(e1_xf.og, vcp1), v2_add(e0_xf.og, vcp0)); - struct v2 linear_bias = v2_mul(linear_separation, correction_rate); - - struct v2 vrel = v2_sub(v2_add(v1, v2_perp_mul(vcp1, w1)), v2_add(v0, v2_perp_mul(vcp0, w0))); - struct v2 impulse = v2_neg(xform_basis_mul_v2(joint->linear_mass_xf, v2_add(vrel, linear_bias))); - - struct v2 old_impulse = joint->linear_impulse; - joint->linear_impulse = v2_clamp_len(v2_add(joint->linear_impulse, impulse), max_impulse); - impulse = v2_sub(joint->linear_impulse, old_impulse); - - v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); - v1 = v2_add(v1, v2_mul(impulse, inv_m1)); - w0 -= v2_wedge(vcp0, impulse) * inv_i0; - w1 += v2_wedge(vcp1, impulse) * inv_i1; - } - - e0->linear_velocity = v0; - e0->angular_velocity = w0; - e1->linear_velocity = v1; - e1->angular_velocity = w1; - } -} - -#if 1 - -/* ========================== * - * TESTING MOUSE JOINT - * ========================== */ - -INTERNAL void create_mouse_joints(struct game_cmd_array game_cmds) -{ - struct entity_store *store = G.tick.entity_store; - struct entity *root = G.root; - - b32 start_dragging = false; - b32 stop_dragging = false; - for (u64 i = 0; i < game_cmds.count; ++i) { - struct game_cmd cmd = game_cmds.cmds[i]; - b32 start = cmd.state == GAME_CMD_STATE_START; - b32 stop = cmd.state == GAME_CMD_STATE_STOP; - switch (cmd.kind) { - case GAME_CMD_KIND_DRAG_OBJECT: - { - if (start) { - start_dragging = true; - } else if (stop) { - stop_dragging = true; - } - } break; - default: break; - } - } - - struct v2 cursor = G.user_cursor; - - struct entity *joint_ent = entity_find_first_match_one(store, ENTITY_PROP_MOUSE_JOINT); - struct entity *target_ent = entity_from_handle(store, joint_ent->mouse_joint_data.target); - - if (start_dragging) { - struct xform mouse_xf = xform_from_pos(cursor); - struct collider_shape mouse_shape = ZI; - mouse_shape.points[0] = V2(0, 0); - mouse_shape.count = 1; - - 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_PHYSICAL_DYNAMIC)) continue; - - struct collider_shape ent_collider = ent->local_collider; - if (ent_collider.count > 0) { - struct xform ent_xf = entity_get_xform(ent); - /* TODO: Can just use boolean GJK */ - struct collider_collision_points_result res = collider_collision_points(&ent_collider, &mouse_shape, ent_xf, mouse_xf); - if (res.num_points > 0) { - target_ent = ent; - break; - } - } - } - } else if (stop_dragging) { - target_ent = entity_nil(); - } - - if (entity_is_valid_and_active(target_ent)) { - if (!entity_is_valid_and_active(joint_ent)) { - joint_ent = entity_alloc(root); - joint_ent->mass_unscaled = F32_INFINITY; - joint_ent->inertia_unscaled = F32_INFINITY; - entity_enable_prop(joint_ent, ENTITY_PROP_MOUSE_JOINT); - entity_enable_prop(joint_ent, ENTITY_PROP_ACTIVE); - } - - struct mouse_joint *joint = &joint_ent->mouse_joint_data; - - struct xform xf = entity_get_xform(target_ent); - f32 mass = target_ent->mass_unscaled * math_fabs(xform_get_determinant(xf)); - - if (!entity_handle_eq(joint->target, target_ent->handle)) { - joint->point_local_start = xform_invert_mul_v2(xf, cursor); - joint->target = target_ent->handle; - } - joint->point_local_end = xform_invert_mul_v2(xf, cursor); - - joint->linear_softness = G.mouse_joint_linear_softness; - joint->angular_softness = G.mouse_joint_angular_softness; - joint->max_force = G.mouse_joint_max_force * mass; - } else { - if (entity_is_valid_and_active(joint_ent)) { - joint_ent->mouse_joint_data.target = entity_nil_handle(); - } - } -} - -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_start), 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 - } else { - /* Mark joint for removal */ - entity_disable_prop(joint_ent, ENTITY_PROP_ACTIVE); - entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); - } - } -} - -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_start), xf.og); - ent->linear_velocity = v2_add(ent->linear_velocity, v2_mul(joint->linear_impulse, inv_m)); - ent->angular_velocity += (v2_wedge(vcp, joint->linear_impulse) + 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_start = xform_mul_v2(xf, joint->point_local_start); - struct v2 point_end = xform_mul_v2(xf, joint->point_local_end); - - struct v2 vcp = v2_sub(point_start, xf.og); - struct v2 separation = v2_sub(point_start, point_end); - - 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; - -#if 0 - struct v2 vel = v2_add(v, v2_perp_mul(vcp, w)); - struct v2 b = v2_mul(xform_basis_mul_v2(xform_invert(joint->linear_mass_xf), v2_add(vel, bias)), mass_scale); - - struct v2 old_impulse = joint->linear_impulse; - struct v2 impulse = v2_sub(v2_mul(old_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); -#else - struct v2 vel = v2_add(v, v2_perp_mul(vcp, w)); - struct v2 b = xform_basis_mul_v2(joint->linear_mass_xf, v2_add(vel, bias)); - - struct v2 impulse; - impulse.x = -mass_scale * b.x - impulse_scale * joint->linear_impulse.x; - impulse.y = -mass_scale * b.y - impulse_scale * joint->linear_impulse.y; - - struct v2 old_impulse = joint->linear_impulse; - joint->linear_impulse.x += impulse.x; - joint->linear_impulse.y += impulse.y; - - joint->linear_impulse = v2_clamp_len(joint->linear_impulse, max_impulse); - - impulse.x = joint->linear_impulse.x - old_impulse.x; - impulse.y = joint->linear_impulse.y - old_impulse.y; -#endif - - v = v2_add(v, v2_mul(impulse, inv_m)); - w += v2_wedge(vcp, impulse) * inv_i; - } - - ent->linear_velocity = v; - ent->angular_velocity = w; - } - } -} - -#else - -INTERNAL void prepare_mouse_joints(void) -{ -} - -INTERNAL void warm_start_mouse_joints(void) -{ -} - -INTERNAL void solve_mouse_joints(f32 dt) -{ - (UNUSED)dt; -} - -#endif - - - - - -/* ========================== * - * TESTING PHYSICS INTEGRATION - * ========================== */ - -INTERNAL void integrate_velocities_from_forces(f32 dt) -{ - struct entity_store *store = G.tick.entity_store; - 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_PHYSICAL_DYNAMIC) && !entity_has_prop(ent, ENTITY_PROP_PHYSICAL_KINEMATIC)) continue; - - struct xform xf = entity_get_xform(ent); - f32 det_abs = math_fabs(xform_get_determinant(xf)); - f32 mass = ent->mass_unscaled * det_abs; - f32 inertia = ent->inertia_unscaled * det_abs; - - /* Determine force & torque acceleration */ - struct v2 force_accel = v2_mul(v2_div(ent->force, mass), dt); - f32 torque_accel = (ent->torque / inertia) * dt; - - /* Integrate & clamp */ - ent->linear_velocity = v2_clamp_len(v2_add(ent->linear_velocity, force_accel), GAME_MAX_LINEAR_VELOCITY); - ent->angular_velocity = clamp_f32(ent->angular_velocity + torque_accel, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY); - - /* Reset forces */ - ent->force = V2(0, 0); - ent->torque = 0; - } -} - -INTERNAL void integrate_positions_from_velocities(f32 dt) -{ - struct entity_store *store = G.tick.entity_store; - - 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_PHYSICAL_DYNAMIC) && !entity_has_prop(ent, ENTITY_PROP_PHYSICAL_KINEMATIC)) continue; - - /* Clamp velocities */ - ent->linear_velocity = v2_clamp_len(ent->linear_velocity, GAME_MAX_LINEAR_VELOCITY); - ent->angular_velocity = clamp_f32(ent->angular_velocity, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY); - - struct v2 tick_linear_velocity = v2_mul(ent->linear_velocity, dt); - f32 tick_angular_velocity = ent->angular_velocity * dt; - - struct xform xf = entity_get_xform(ent); - xf.og = v2_add(xf.og, tick_linear_velocity); - xf = xform_basis_rotated_world(xf, tick_angular_velocity); - entity_set_xform(ent, xf); - } -} - - - - - - - - - - -/* ========================== * - * TESTING TOI - * ========================== */ - -/* Takes 2 shapes and their xforms at t=0 and t=1. - * Returns time of impact in range [0, 1]. */ -INTERNAL f32 toi(struct collider_shape *c0, struct collider_shape *c1, - struct xform xf0_t0, struct xform xf1_t0, - struct xform xf0_t1, struct xform xf1_t1, - f32 tolerance, u32 max_iterations) -{ - f32 t0 = 0; - f32 t1 = 1; - f32 t0_sep = 0; - f32 t1_sep = 0; - f32 t = 0; - f32 t_sep = F32_INFINITY; - - /* Find direction p0 -> p1 at t=0 */ - struct v2 dir; - struct v2 dir_neg; - { - struct collider_closest_points_result closest_points_res = collider_closest_points(c0, c1, xf0_t0, xf1_t0); - if (closest_points_res.colliding) { - /* Shapes are penetrating at t=0 */ - return 0; - } - dir = v2_sub(closest_points_res.p1, closest_points_res.p0); - t0_sep = v2_len(dir); - dir = v2_div(dir, t0_sep); /* Normalize */ - dir_neg = v2_neg(dir); - } - - { - struct v2 p0 = collider_support_point(c0, xf0_t1, dir); - struct v2 p1 = collider_support_point(c1, xf1_t1, dir_neg); - t1_sep = v2_dot(dir, v2_sub(p1, p0)); - if (t1_sep > 0) { - /* Shapes are not penetrating at t=1 */ - return 1; - } - } - - u32 iteration = 0; - while (math_fabs(t_sep) > tolerance) { - if (iteration >= max_iterations) { - /* Not necessarily an error, but this should rarely occur and - * should be investigated if it does. If determined not an error then - * this assert should be removed or max_iterations / tolerance - * adjusted at call site. */ - ASSERT(false); - break; - } - - /* Use mix of bisection & false position method to find root - * (as described in https://box2d.org/files/ErinCatto_ContinuousCollision_GDC2013.pdf) */ - if (iteration & 1) { - /* Bisect */ - t = (t1 + t0) / 2.0; - } else { - /* False position (fastest for linear case) */ - f32 m = (t1_sep - t0_sep) / (t1 - t0); - t = (-t1_sep / m) + t1; - } - - struct xform xf0 = xform_lerp(xf0_t0, xf0_t1, t); - struct xform xf1 = xform_lerp(xf1_t0, xf1_t1, t); - - struct v2 p0 = collider_support_point(c0, xf0, dir); - struct v2 p1 = collider_support_point(c1, xf1, dir_neg); - t_sep = v2_dot(dir, v2_sub(p1, p0)); - - /* Update bracket */ - if (t_sep > 0) { - t0 = t; - t0_sep = t_sep; - } else { - t1 = t; - t1_sep = t_sep; - } - - ++iteration; - } - - return t; -} - -INTERNAL f32 determine_earliest_toi(f32 dt, f32 tolerance, u32 max_iterations) -{ - __prof; - f32 smallest_t = 1; - - struct entity_store *store = G.tick.entity_store; - - for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) { - struct entity *e0 = &store->entities[e0_index]; - if (!entity_is_valid_and_active(e0)) continue; - if (!(entity_has_prop(e0, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(e0, ENTITY_PROP_PHYSICAL_KINEMATIC))) continue; - if (e0->local_collider.count <= 0) continue; - - b32 e0_is_bullet = entity_has_prop(e0, ENTITY_PROP_BULLET); - - struct collider_shape e0_collider = e0->local_collider; - struct xform e0_xf_t0 = entity_get_xform(e0); - struct xform e0_xf_t1 = e0_xf_t0; - { - /* Calculate xform at t=1 */ - struct v2 linear_velocity = v2_clamp_len(e0->linear_velocity, GAME_MAX_LINEAR_VELOCITY); - f32 angular_velocity = clamp_f32(e0->angular_velocity, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY); - struct v2 tick_linear_velocity = v2_mul(linear_velocity, dt); - f32 tick_angular_velocity = angular_velocity * dt; - e0_xf_t1.og = v2_add(e0_xf_t1.og, tick_linear_velocity); - e0_xf_t1 = xform_basis_rotated_world(e0_xf_t1, tick_angular_velocity); - } - - /* Start e1 index at e0 index + 1 to prevent redundant checks */ - for (u64 e1_index = e0_index + 1; e1_index < store->reserved; ++e1_index) { - struct entity *e1 = &store->entities[e1_index]; - if (e1 == e0) continue; - if (!entity_is_valid_and_active(e1)) continue; - if (!(entity_has_prop(e1, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(e1, ENTITY_PROP_PHYSICAL_KINEMATIC))) continue; - if (e1->local_collider.count <= 0) continue; - - b32 e1_is_bullet = entity_has_prop(e1, ENTITY_PROP_BULLET); - - /* Skip check if neither e0 or e1 are bullets */ - if (!e0_is_bullet && !e1_is_bullet) continue; - - struct collider_shape e1_collider = e1->local_collider; - struct xform e1_xf_t0 = entity_get_xform(e1); - struct xform e1_xf_t1 = e1_xf_t0; - { - /* Calculate xform at t=1 */ - struct v2 linear_velocity = v2_clamp_len(e1->linear_velocity, GAME_MAX_LINEAR_VELOCITY); - f32 angular_velocity = clamp_f32(e1->angular_velocity, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY); - struct v2 tick_linear_velocity = v2_mul(linear_velocity, dt); - f32 tick_angular_velocity = angular_velocity * dt; - e1_xf_t1.og = v2_add(e1_xf_t1.og, tick_linear_velocity); - e1_xf_t1 = xform_basis_rotated_world(e1_xf_t1, tick_angular_velocity); - } - - f32 t = toi(&e0_collider, &e1_collider, e0_xf_t0, e1_xf_t0, e0_xf_t1, e1_xf_t1, tolerance, max_iterations); - if (t != 0 && t < smallest_t) { - smallest_t = t; - } - } - } - - return smallest_t; -} - /* ========================== * * Release entities * ========================== */ @@ -1791,7 +432,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) if (!entity_has_prop(ent, ENTITY_PROP_ACTIVE)) { u64 atick = ent->activation_tick; if (atick != 0 || G.tick.tick_id >= atick) { - activate_now(ent); + entity_activate(ent, G.tick.tick_id); } } } @@ -2140,7 +781,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) entity_enable_prop(joint_ent, ENTITY_PROP_ACTIVE); ent->move_joint = joint_ent->handle; - struct motor_joint_def def = ZI; + struct phys_motor_joint_def def = ZI; def.e0 = joint_ent->handle; /* Re-using joint entity as e0 */ def.e1 = ent->handle; def.correction_rate = 0; @@ -2179,7 +820,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) entity_enable_prop(joint_ent, ENTITY_PROP_ACTIVE); ent->aim_joint = joint_ent->handle; - struct motor_joint_def def = ZI; + struct phys_motor_joint_def def = ZI; def.e0 = joint_ent->handle; /* Re-using joint entity as e0 */ def.e1 = ent->handle; def.correction_rate = 0.1; @@ -2256,7 +897,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) struct entity *joint_ent = entity_from_handle(store, ent->ground_friction_joint); - struct motor_joint_def def = ZI; + struct phys_motor_joint_def def = ZI; def.e0 = root->handle; def.e1 = ent->handle; def.correction_rate = 0; @@ -2277,73 +918,27 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * Physics * ========================== */ - (UNUSED)create_contacts; - (UNUSED)prepare_contacts; - (UNUSED)warm_start_contacts; - (UNUSED)solve_contacts; - - (UNUSED)prepare_motor_joints; - (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 - { - f32 remaining_dt = dt; - integrate_velocities_from_forces(dt); - while (remaining_dt > 0) { - f32 earliest_toi = 1; + /* Mouse drag */ + b32 dbg_start_dragging = false; + b32 dbg_stop_dragging = false; + for (u64 i = 0; i < game_cmds.count; ++i) { + struct game_cmd cmd = game_cmds.cmds[i]; + b32 start = cmd.state == GAME_CMD_STATE_START; + b32 stop = cmd.state == GAME_CMD_STATE_STOP; + switch (cmd.kind) { + case GAME_CMD_KIND_DRAG_OBJECT: { -#if GAME_PHYSICS_ENABLE_TOI - const f32 min_toi = 0.000001f; - const f32 tolerance = 0.00001f; - const u32 max_iterations = 16; - earliest_toi = max_f32(determine_earliest_toi(remaining_dt, tolerance, max_iterations), min_toi); -#else - (UNUSED)toi; - (UNUSED)determine_earliest_toi; -#endif - } - - f32 step_dt = remaining_dt * earliest_toi; - - create_contacts(); - create_mouse_joints(game_cmds); - - prepare_contacts(); - prepare_motor_joints(); - prepare_mouse_joints(); - - f32 substep_dt = step_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); - - #if GAME_PHYSICS_ENABLE_COLLISION && GAME_PHYSICS_ENABLE_RELAXATION - solve_contacts(substep_dt, false); /* Relaxation */ - #endif - } - remaining_dt -= step_dt; + if (start) { + dbg_start_dragging = true; + } else if (stop) { + dbg_stop_dragging = true; + } + } break; + default: break; } } -#endif + + phys_step(store, dt, &G.contact_lookup, &G.collision_debug_lookup, G.tick.tick_id, G.user_cursor, dbg_start_dragging, dbg_stop_dragging); /* ========================== * * Respond to hit events @@ -2354,7 +949,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) if (!entity_is_valid_and_active(ent)) continue; if (!entity_has_prop(ent, ENTITY_PROP_HIT_EVENT)) continue; - struct hit_event *event = &ent->hit_event; + struct phys_hit_event *event = &ent->hit_event; struct entity *e0 = entity_from_handle(store, event->e0); struct entity *e1 = entity_from_handle(store, event->e1); diff --git a/src/game.h b/src/game.h index 97ade7df..905c9dd8 100644 --- a/src/game.h +++ b/src/game.h @@ -5,6 +5,7 @@ struct world; struct mixer_startup_receipt; struct sprite_startup_receipt; struct sound_startup_receipt; +struct phys_startup_receipt; enum game_cmd_state { GAME_CMD_STATE_STOP = -1, @@ -53,7 +54,8 @@ struct game_cmd_array { struct game_startup_receipt { i32 _; }; struct game_startup_receipt game_startup(struct mixer_startup_receipt *mixer_sr, struct sprite_startup_receipt *sheet_sr, - struct sound_startup_receipt *sound_sr); + struct sound_startup_receipt *sound_sr, + struct phys_startup_receipt *phys_sr); void game_get_latest_tick(struct world *dest); u64 game_get_latest_tick_id(void); diff --git a/src/phys.c b/src/phys.c new file mode 100644 index 00000000..c8f7dc57 --- /dev/null +++ b/src/phys.c @@ -0,0 +1,1134 @@ +#include "phys.h" +#include "entity.h" + +GLOBAL struct { + /* Constants */ + struct math_spring_result contact_softness; + struct math_spring_result mouse_joint_linear_softness; + struct math_spring_result mouse_joint_angular_softness; + f32 mouse_joint_max_force; +} G = ZI, DEBUG_ALIAS(G, G_phys); + +/* ========================== * + * Startup + * ========================== */ + +struct phys_startup_receipt phys_startup(void) +{ + /* Initialize constants */ + const f32 substep_dt = (1.f / ((f32)GAME_FPS * (f32)GAME_PHYSICS_SUBSTEPS)); + + const f32 contact_frequency = (1.f / substep_dt) / 8.f; + const f32 contact_damping_ratio = 10; + G.contact_softness = math_spring(contact_frequency, contact_damping_ratio, substep_dt); + + const f32 mouse_joint_linear_frequency = 5.0; + const f32 mouse_joint_linear_damping_ratio = 0.7; + const f32 mouse_joint_angular_frequency = 0.5f; + const f32 mouse_joint_angular_damping_ratio = 0.1f; + G.mouse_joint_max_force = 1000; + G.mouse_joint_linear_softness = math_spring(mouse_joint_linear_frequency, mouse_joint_linear_damping_ratio, substep_dt); + G.mouse_joint_angular_softness = math_spring(mouse_joint_angular_frequency, mouse_joint_angular_damping_ratio, substep_dt); + + return (struct phys_startup_receipt) { 0 }; +} + +/* ========================== * + * Contact + * ========================== */ + +void phys_create_contacts(struct entity_store *store, struct entity_lookup *contact_lookup, struct entity_lookup *debug_lookup, u64 tick_id) +{ + struct entity *root = entity_from_handle(store, store->root); + + for (u64 check0_index = 0; check0_index < store->reserved; ++check0_index) { + struct entity *check0 = &store->entities[check0_index]; + if (!entity_is_valid_and_active(check0)) continue; + if (!(entity_has_prop(check0, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(check0, ENTITY_PROP_PHYSICAL_KINEMATIC))) continue; + if (check0->local_collider.count <= 0) continue; + + struct xform check0_xf = entity_get_xform(check0); + struct collider_shape check0_collider = check0->local_collider; + + for (u64 check1_index = 0; check1_index < store->reserved; ++check1_index) { + struct entity *check1 = &store->entities[check1_index]; + if (check1 == check0) continue; + if (!entity_is_valid_and_active(check1)) continue; + if (!(entity_has_prop(check1, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(check1, ENTITY_PROP_PHYSICAL_KINEMATIC))) continue; + if (check1->local_collider.count <= 0) continue; + + /* Deterministic entity order based on index */ + struct entity *e0; + struct entity *e1; + struct xform e0_xf; + struct xform e1_xf; + struct collider_shape e0_collider; + struct collider_shape e1_collider; + if (check0_index < check1_index) { + e0 = check0; + e1 = check1; + e0_xf = check0_xf; + e1_xf = entity_get_xform(check1); + e0_collider = check0_collider; + e1_collider = check1->local_collider; + } else { + e0 = check1; + e1 = check0; + e0_xf = entity_get_xform(check1); + e1_xf = check0_xf; + e0_collider = check1->local_collider; + e1_collider = check0_collider; + } + + struct entity_lookup_key key = entity_lookup_key_from_two_handles(e0->handle, e1->handle); + struct entity_lookup_entry *entry = entity_lookup_get(contact_lookup, key); + + struct entity *constraint_ent = entity_nil(); + if (entry) { + constraint_ent = entity_from_handle(store, entry->entity); + if (entity_is_valid_and_active(constraint_ent)) { + if (constraint_ent->contact_constraint_data.last_iteration >= tick_id) { + /* Already processed constraint this iteration */ + continue; + } else { + ++constraint_ent->contact_constraint_data.last_iteration; + } + } else { + /* Constraint ent no longer valid, delete entry */ + entity_lookup_remove(contact_lookup, entry); + entry = NULL; + } + } + + /* Calculate collision */ + struct collider_collision_points_result collider_res = collider_collision_points(&e0_collider, &e1_collider, e0_xf, e1_xf); + + /* Parts of algorithm are hard-coded to support 2 contact points */ + CT_ASSERT(ARRAY_COUNT(constraint_ent->contact_constraint_data.points) == 2); + CT_ASSERT(ARRAY_COUNT(collider_res.points) == 2); + + struct phys_contact_constraint *constraint = NULL; + if (collider_res.num_points > 0) { + if (!entity_is_valid_and_active(constraint_ent)) { + /* Create hit event */ + { + struct entity *event = entity_alloc(root); + entity_enable_prop(event, ENTITY_PROP_HIT_EVENT); + event->hit_event.e0 = e0->handle; + event->hit_event.e1 = e1->handle; + event->hit_event.normal = collider_res.normal; + entity_enable_prop(event, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); + entity_enable_prop(event, ENTITY_PROP_ACTIVE); + + /* Calculate point */ + struct v2 point = 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)); + } + event->hit_event.point = point; + + /* Calculate relative velocity */ + struct v2 vrel; + { + struct v2 v0 = e0->linear_velocity; + 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 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); + } + event->hit_event.vrel = vrel; + } + + /* Create constraint */ + { + constraint_ent = entity_alloc(root); + constraint_ent->contact_constraint_data.e1 = e1->handle; + constraint_ent->contact_constraint_data.e0 = e0->handle; + constraint_ent->contact_constraint_data.skip_solve = entity_has_prop(e0, ENTITY_PROP_SENSOR) || entity_has_prop(e1, ENTITY_PROP_SENSOR) + || !(entity_has_prop(e0, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(e1, ENTITY_PROP_PHYSICAL_DYNAMIC)); + entity_enable_prop(constraint_ent, ENTITY_PROP_ACTIVE); + + /* TODO: Should we recalculate normal as more contact points are added? */ + entity_enable_prop(constraint_ent, ENTITY_PROP_CONTACT_CONSTRAINT); + entity_activate(constraint_ent, tick_id); + ASSERT(!entry); /* Existing entry should never be present here */ + entity_lookup_set(contact_lookup, key, constraint_ent->handle); + } + } + constraint = &constraint_ent->contact_constraint_data; + constraint->normal = collider_res.normal; + constraint->friction = math_sqrt(e0->friction * e1->friction); + + /* Delete old contacts that are no longer present */ + for (u32 i = 0; i < constraint->num_points; ++i) { + struct phys_contact_point *old = &constraint->points[i]; + u32 id = old->id; + b32 found = false; + for (u32 j = 0; j < collider_res.num_points; ++j) { + if (collider_res.points[j].id == id) { + found = true; + break; + } + } + if (!found) { + /* Delete contact by replacing with last in array */ + *old = constraint->points[--constraint->num_points]; + --i; + } + } + + /* Update / insert returned contacts */ + for (u32 i = 0; i < collider_res.num_points; ++i) { + struct collider_collision_point *res_point = &collider_res.points[i]; + struct v2 point = res_point->point; + f32 sep = res_point->separation; + u32 id = res_point->id; + struct phys_contact_point *contact = NULL; + /* Match */ + for (u32 j = 0; j < constraint->num_points; ++j) { + struct phys_contact_point *t = &constraint->points[j]; + if (t->id == id) { + contact = t; + break; + } + } + if (!contact) { + /* Insert */ + contact = &constraint->points[constraint->num_points++]; + MEMZERO_STRUCT(contact); + contact->id = id; + constraint->softness = G.contact_softness; + 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; + +#if COLLIDER_DEBUG + contact->dbg_pt = point; +#endif + } + } else if (constraint_ent->valid) { + constraint_ent->contact_constraint_data.num_points = 0; + } + + /* TODO: Remove this (debugging) */ +#if COLLIDER_DEBUG + { + struct entity *dbg_ent = entity_nil(); + struct entity_lookup_entry *dbg_entry = entity_lookup_get(debug_lookup, key); + if (dbg_entry) { + dbg_ent = entity_from_handle(store, dbg_entry->entity); + } + + if (!dbg_ent->valid) { + /* FIXME: Entity never released */ + dbg_ent = entity_alloc(root); + entity_enable_prop(dbg_ent, ENTITY_PROP_COLLISION_DEBUG); + entity_lookup_set(debug_lookup, key, dbg_ent->handle); + } + + struct phys_collision_debug *dbg = &dbg_ent->collision_debug_data; + + if (dbg->res.num_points == 0) { + if (collider_res.num_points > 0) { + ++e0->colliding; + ++e1->colliding; + } + } else { + if (collider_res.num_points == 0) { + --e0->colliding; + --e1->colliding; + } + } + dbg->e0 = e0->handle; + dbg->e1 = e1->handle; + dbg->res = collider_res; + + if (constraint) { + MEMCPY(dbg->points, constraint->points, sizeof(dbg->points)); + dbg->num_points = constraint->num_points; + } else { + dbg->num_points = 0; + } + + dbg->xf0 = e0_xf; + dbg->xf1 = e1_xf; + + /* Update closest points */ + { + struct collider_closest_points_result closest_points_res = collider_closest_points(&e0_collider, &e1_collider, e0_xf, e1_xf); + dbg->closest0 = closest_points_res.p0; + dbg->closest1 = closest_points_res.p1; + } + } +#endif + } + } +} + +void phys_prepare_contacts(struct entity_store *store, struct entity_lookup *contact_lookup, struct entity_lookup *debug_lookup) +{ + 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 phys_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; + + if (entity_has_prop(e0, ENTITY_PROP_PHYSICAL_KINEMATIC)) { + constraint->inv_m0 = 0; + constraint->inv_i0 = 0; + } + if (entity_has_prop(e1, ENTITY_PROP_PHYSICAL_KINEMATIC)) { + constraint->inv_m1 = 0; + constraint->inv_i1 = 0; + } + + /* 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); + + /* 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 + } + } else { + /* Mark constraint for removal */ + entity_disable_prop(constraint_ent, ENTITY_PROP_ACTIVE); + entity_enable_prop(constraint_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); + /* Remove from lookup */ + struct entity_lookup_key key = entity_lookup_key_from_two_handles(constraint->e0, constraint->e1); + struct entity_lookup_entry *entry = entity_lookup_get(contact_lookup, key); + if (entry) { + entity_lookup_remove(contact_lookup, entry); + } else { + ASSERT(false); /* This should always exist */ + } + } + } + +#if COLLIDER_DEBUG + for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { + struct entity *dbg_ent = &store->entities[entity_index]; + if (!entity_is_valid_and_active(dbg_ent)) continue; + if (!entity_has_prop(dbg_ent, ENTITY_PROP_COLLISION_DEBUG)) continue; + + struct phys_collision_debug *dbg = &dbg_ent->collision_debug_data; + struct entity *e0 = entity_from_handle(store, dbg->e0); + struct entity *e1 = entity_from_handle(store, dbg->e1); + + + if (!entity_is_valid_and_active(e0) || !entity_is_valid_and_active(e1) + || !(entity_has_prop(e0, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(e0, ENTITY_PROP_PHYSICAL_KINEMATIC)) + || !(entity_has_prop(e1, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(e1, ENTITY_PROP_PHYSICAL_KINEMATIC))) { + /* Mark dbg ent for removal */ + entity_disable_prop(dbg_ent, ENTITY_PROP_ACTIVE); + entity_enable_prop(dbg_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); + + /* Remove from lookup */ + struct entity_lookup_key key = entity_lookup_key_from_two_handles(dbg->e0, dbg->e1); + struct entity_lookup_entry *entry = entity_lookup_get(debug_lookup, key); + + if (e0->valid) { + --e0->colliding; + } + if (e1->valid) { + --e1->colliding; + } + + if (entry) { + entity_lookup_remove(debug_lookup, entry); + } else { + ASSERT(false); /* This should always exist */ + } + } + } +#endif +} + +void phys_warm_start_contacts(struct entity_store *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 phys_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) && !constraint->skip_solve) { + struct xform e0_xf = entity_get_xform(e0); + struct xform e1_xf = entity_get_xform(e1); + + f32 inv_m0 = constraint->inv_m0; + f32 inv_m1 = constraint->inv_m1; + f32 inv_i0 = constraint->inv_i0; + f32 inv_i1 = constraint->inv_i1; + + struct v2 v0 = e0->linear_velocity; + struct v2 v1 = e1->linear_velocity; + f32 w0 = e0->angular_velocity; + f32 w1 = e1->angular_velocity; + + /* Warm start */ + struct v2 normal = constraint->normal; + struct v2 tangent = v2_perp(normal); + 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 impulse = v2_add(v2_mul(normal, point->normal_impulse), v2_mul(tangent, point->tangent_impulse)); + impulse = v2_mul(impulse, inv_num_points); + + v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); + v1 = v2_add(v1, v2_mul(impulse, inv_m1)); + w0 -= v2_wedge(vcp0, impulse) * inv_i0; + w1 += v2_wedge(vcp1, impulse) * inv_i1; + } + + e0->linear_velocity = v0; + e0->angular_velocity = w0; + e1->linear_velocity = v1; + e1->angular_velocity = w1; + } + } +} + +void phys_solve_contacts(struct entity_store *store, f32 dt, b32 apply_bias) +{ + 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 phys_contact_constraint *constraint = &constraint_ent->contact_constraint_data; + + struct entity *e0 = entity_from_handle(store, constraint->e0); + struct entity *e1 = entity_from_handle(store, constraint->e1); + + struct v2 v0 = e0->linear_velocity; + struct v2 v1 = e1->linear_velocity; + f32 w0 = e0->angular_velocity; + f32 w1 = e1->angular_velocity; + + u32 num_points = constraint->num_points; + if (num_points > 0 && entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1) && !constraint->skip_solve) { + struct xform e0_xf = entity_get_xform(e0); + struct xform e1_xf = entity_get_xform(e1); + + f32 inv_m0 = constraint->inv_m0; + f32 inv_m1 = constraint->inv_m1; + f32 inv_i0 = constraint->inv_i0; + f32 inv_i1 = constraint->inv_i1; + + /* Normal impulse */ + 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); + + f32 separation = v2_dot(v2_sub(p1, p0), normal) + point->starting_separation; + + f32 velocity_bias = 0.0f; + f32 mass_scale = 1.0f; + f32 impulse_scale = 0.0f; + + if (separation > 0.0f) { + /* Speculative */ + velocity_bias = separation / dt; + } else if (apply_bias) { + /* Soft constraint */ + 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)); + struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1)); + struct v2 vrel = v2_sub(vel0, vel1); + + f32 k = point->inv_normal_mass; + + /* (to be applied along n) */ + f32 vn = v2_dot(vrel, normal); + f32 j = ((k * mass_scale) * (vn - velocity_bias)) - (point->normal_impulse * impulse_scale); + + f32 old_impulse = point->normal_impulse; + f32 new_impulse = max_f32(old_impulse + j, 0); + f32 delta = new_impulse - old_impulse; + point->normal_impulse = new_impulse; + + struct v2 impulse = v2_mul(normal, delta); + v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); + v1 = v2_add(v1, v2_mul(impulse, inv_m1)); + w0 -= v2_wedge(vcp0, impulse) * inv_i0; + w1 += v2_wedge(vcp1, impulse) * inv_i1; + } + + /* Tangent impulse */ + 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 vel0 = v2_add(v0, v2_perp_mul(vcp0, w0)); + struct v2 vel1 = v2_add(v1, v2_perp_mul(vcp1, w1)); + struct v2 vrel = v2_sub(vel0, vel1); + + f32 k = point->inv_tangent_mass; + + /* (to be applied along t) */ + f32 vt = v2_dot(vrel, tangent); + f32 j = vt * k; + + f32 max_friction = constraint->friction * point->normal_impulse; + f32 old_impulse = point->tangent_impulse; + f32 new_impulse = clamp_f32(old_impulse + j, -max_friction, max_friction); + f32 delta = new_impulse - old_impulse; + point->tangent_impulse = new_impulse; + + struct v2 impulse = v2_mul(tangent, delta); + v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); + v1 = v2_add(v1, v2_mul(impulse, inv_m1)); + w0 -= v2_wedge(vcp0, impulse) * inv_i0; + w1 += v2_wedge(vcp1, impulse) * inv_i1; + } + + e0->linear_velocity = v0; + e0->angular_velocity = w0; + e1->linear_velocity = v1; + e1->angular_velocity = w1; + } + } +} + +/* ========================== * + * Motor joint + * ========================== */ + +struct phys_motor_joint motor_joint_from_def(struct phys_motor_joint_def def) +{ + struct phys_motor_joint res = ZI; + res.e0 = def.e0; + res.e1 = def.e1; + res.correction_rate = clamp_f32(def.correction_rate, 0, 1); + res.max_force = def.max_force; + res.max_torque = def.max_torque; + return res; +} + +void phys_prepare_motor_joints(struct entity_store *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 phys_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); + + if (entity_is_valid_and_active(e0) && entity_is_valid_and_active(e1)) { + 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); + } + joint->inv_m0 = inv_m0; + joint->inv_m1 = inv_m1; + joint->inv_i0 = inv_i0; + joint->inv_i1 = inv_i1; + + joint->point_local_e0 = V2(0, 0); + joint->point_local_e1 = V2(0, 0); + + 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; + linear_mass_xf.bx.y = -vcp0.y * vcp0.x * inv_i0 - vcp1.y * vcp1.x * inv_i1; + linear_mass_xf.by.x = linear_mass_xf.bx.y; + linear_mass_xf.by.y = inv_m0 + inv_m1 + vcp0.x * vcp0.x * inv_i0 + vcp1.x * vcp1.x * inv_i1; + joint->linear_mass_xf = xform_invert(linear_mass_xf); + + joint->angular_mass = 1.f / (inv_i0 + inv_i1); + +#if !GAME_PHYSICS_ENABLE_WARM_STARTING + joint->linear_impulse = V2(0, 0); + joint->angular_impulse = 0; +#endif + } else { + /* Mark joint for removal */ + entity_disable_prop(joint_ent, ENTITY_PROP_ACTIVE); + entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); + } + } +} + +void phys_warm_start_motor_joints(struct entity_store *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 phys_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(vcp0, joint->linear_impulse) + joint->angular_impulse) * inv_i0; + e1->angular_velocity += (v2_wedge(vcp1, joint->linear_impulse) + joint->angular_impulse) * inv_i1; + } +} + +void phys_solve_motor_joints(struct entity_store *store, f32 dt) +{ + 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 phys_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 v0 = e0->linear_velocity; + struct v2 v1 = e1->linear_velocity; + f32 w0 = e0->angular_velocity; + f32 w1 = e1->angular_velocity; + + f32 correction_rate = joint->correction_rate / dt; + + /* Angular constraint */ + { + f32 max_impulse = joint->max_torque * dt; + + f32 angular_separation = math_unwind_angle(xform_get_rotation(e1_xf) - xform_get_rotation(e0_xf)); + f32 angular_bias = angular_separation * correction_rate; + + 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); + impulse = joint->angular_impulse - old_impulse; + + w0 -= impulse * inv_i0; + w1 += impulse * inv_i1; + } + + /* Linear constraint */ + { + 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); + + f32 max_impulse = joint->max_force * dt; + + struct v2 linear_separation = v2_sub(v2_add(e1_xf.og, vcp1), v2_add(e0_xf.og, vcp0)); + struct v2 linear_bias = v2_mul(linear_separation, correction_rate); + + struct v2 vrel = v2_sub(v2_add(v1, v2_perp_mul(vcp1, w1)), v2_add(v0, v2_perp_mul(vcp0, w0))); + struct v2 impulse = v2_neg(xform_basis_mul_v2(joint->linear_mass_xf, v2_add(vrel, linear_bias))); + + struct v2 old_impulse = joint->linear_impulse; + joint->linear_impulse = v2_clamp_len(v2_add(joint->linear_impulse, impulse), max_impulse); + impulse = v2_sub(joint->linear_impulse, old_impulse); + + v0 = v2_sub(v0, v2_mul(impulse, inv_m0)); + v1 = v2_add(v1, v2_mul(impulse, inv_m1)); + w0 -= v2_wedge(vcp0, impulse) * inv_i0; + w1 += v2_wedge(vcp1, impulse) * inv_i1; + } + + e0->linear_velocity = v0; + e0->angular_velocity = w0; + e1->linear_velocity = v1; + e1->angular_velocity = w1; + } +} + +/* ========================== * + * Mouse joint + * ========================== */ + +void phys_create_mouse_joints(struct entity_store *store, struct v2 cursor, b32 start_dragging, b32 stop_dragging) +{ + struct entity *root = entity_from_handle(store, store->root); + + struct entity *joint_ent = entity_find_first_match_one(store, ENTITY_PROP_MOUSE_JOINT); + struct entity *target_ent = entity_from_handle(store, joint_ent->mouse_joint_data.target); + + if (start_dragging) { + struct xform mouse_xf = xform_from_pos(cursor); + struct collider_shape mouse_shape = ZI; + mouse_shape.points[0] = V2(0, 0); + mouse_shape.count = 1; + + 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_PHYSICAL_DYNAMIC)) continue; + + struct collider_shape ent_collider = ent->local_collider; + if (ent_collider.count > 0) { + struct xform ent_xf = entity_get_xform(ent); + /* TODO: Can just use boolean GJK */ + struct collider_collision_points_result res = collider_collision_points(&ent_collider, &mouse_shape, ent_xf, mouse_xf); + if (res.num_points > 0) { + target_ent = ent; + break; + } + } + } + } else if (stop_dragging) { + target_ent = entity_nil(); + } + + if (entity_is_valid_and_active(target_ent)) { + if (!entity_is_valid_and_active(joint_ent)) { + joint_ent = entity_alloc(root); + joint_ent->mass_unscaled = F32_INFINITY; + joint_ent->inertia_unscaled = F32_INFINITY; + entity_enable_prop(joint_ent, ENTITY_PROP_MOUSE_JOINT); + entity_enable_prop(joint_ent, ENTITY_PROP_ACTIVE); + } + + struct phys_mouse_joint *joint = &joint_ent->mouse_joint_data; + + struct xform xf = entity_get_xform(target_ent); + f32 mass = target_ent->mass_unscaled * math_fabs(xform_get_determinant(xf)); + + if (!entity_handle_eq(joint->target, target_ent->handle)) { + joint->point_local_start = xform_invert_mul_v2(xf, cursor); + joint->target = target_ent->handle; + } + joint->point_local_end = xform_invert_mul_v2(xf, cursor); + + joint->linear_softness = G.mouse_joint_linear_softness; + joint->angular_softness = G.mouse_joint_angular_softness; + joint->max_force = G.mouse_joint_max_force * mass; + } else { + if (entity_is_valid_and_active(joint_ent)) { + joint_ent->mouse_joint_data.target = entity_nil_handle(); + } + } +} + +void phys_prepare_mouse_joints(struct entity_store *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 phys_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_start), 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 + } else { + /* Mark joint for removal */ + entity_disable_prop(joint_ent, ENTITY_PROP_ACTIVE); + entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_BEFORE_PUBLISH); + } + } +} + +void phys_warm_start_mouse_joints(struct entity_store *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 phys_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_start), xf.og); + ent->linear_velocity = v2_add(ent->linear_velocity, v2_mul(joint->linear_impulse, inv_m)); + ent->angular_velocity += (v2_wedge(vcp, joint->linear_impulse) + joint->angular_impulse) * inv_i; + } + } +} + +void phys_solve_mouse_joints(struct entity_store *store, f32 dt) +{ + 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 phys_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_start = xform_mul_v2(xf, joint->point_local_start); + struct v2 point_end = xform_mul_v2(xf, joint->point_local_end); + + struct v2 vcp = v2_sub(point_start, xf.og); + struct v2 separation = v2_sub(point_start, point_end); + + 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; + +#if 0 + struct v2 vel = v2_add(v, v2_perp_mul(vcp, w)); + struct v2 b = v2_mul(xform_basis_mul_v2(xform_invert(joint->linear_mass_xf), v2_add(vel, bias)), mass_scale); + + struct v2 old_impulse = joint->linear_impulse; + struct v2 impulse = v2_sub(v2_mul(old_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); +#else + struct v2 vel = v2_add(v, v2_perp_mul(vcp, w)); + struct v2 b = xform_basis_mul_v2(joint->linear_mass_xf, v2_add(vel, bias)); + + struct v2 impulse; + impulse.x = -mass_scale * b.x - impulse_scale * joint->linear_impulse.x; + impulse.y = -mass_scale * b.y - impulse_scale * joint->linear_impulse.y; + + struct v2 old_impulse = joint->linear_impulse; + joint->linear_impulse.x += impulse.x; + joint->linear_impulse.y += impulse.y; + + joint->linear_impulse = v2_clamp_len(joint->linear_impulse, max_impulse); + + impulse.x = joint->linear_impulse.x - old_impulse.x; + impulse.y = joint->linear_impulse.y - old_impulse.y; +#endif + + v = v2_add(v, v2_mul(impulse, inv_m)); + w += v2_wedge(vcp, impulse) * inv_i; + } + + ent->linear_velocity = v; + ent->angular_velocity = w; + } + } +} + +/* ========================== * + * Earliest time of impact + * ========================== */ + +f32 phys_determine_earliest_toi_for_bullets(struct entity_store *store, f32 step_dt, f32 tolerance, u32 max_iterations) +{ + __prof; + f32 smallest_t = 1; + + for (u64 e0_index = 0; e0_index < store->reserved; ++e0_index) { + struct entity *e0 = &store->entities[e0_index]; + if (!entity_is_valid_and_active(e0)) continue; + if (!(entity_has_prop(e0, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(e0, ENTITY_PROP_PHYSICAL_KINEMATIC))) continue; + if (e0->local_collider.count <= 0) continue; + + b32 e0_is_bullet = entity_has_prop(e0, ENTITY_PROP_BULLET); + + struct collider_shape e0_collider = e0->local_collider; + struct xform e0_xf_t0 = entity_get_xform(e0); + struct xform e0_xf_t1 = e0_xf_t0; + { + /* Calculate xform at t=1 */ + struct v2 linear_velocity = v2_clamp_len(e0->linear_velocity, GAME_MAX_LINEAR_VELOCITY); + f32 angular_velocity = clamp_f32(e0->angular_velocity, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY); + struct v2 tick_linear_velocity = v2_mul(linear_velocity, step_dt); + f32 tick_angular_velocity = angular_velocity * step_dt; + e0_xf_t1.og = v2_add(e0_xf_t1.og, tick_linear_velocity); + e0_xf_t1 = xform_basis_rotated_world(e0_xf_t1, tick_angular_velocity); + } + + /* Start e1 index at e0 index + 1 to prevent redundant checks */ + for (u64 e1_index = e0_index + 1; e1_index < store->reserved; ++e1_index) { + struct entity *e1 = &store->entities[e1_index]; + if (e1 == e0) continue; + if (!entity_is_valid_and_active(e1)) continue; + if (!(entity_has_prop(e1, ENTITY_PROP_PHYSICAL_DYNAMIC) || entity_has_prop(e1, ENTITY_PROP_PHYSICAL_KINEMATIC))) continue; + if (e1->local_collider.count <= 0) continue; + + b32 e1_is_bullet = entity_has_prop(e1, ENTITY_PROP_BULLET); + + /* Skip check if neither e0 or e1 are bullets */ + if (!e0_is_bullet && !e1_is_bullet) continue; + + struct collider_shape e1_collider = e1->local_collider; + struct xform e1_xf_t0 = entity_get_xform(e1); + struct xform e1_xf_t1 = e1_xf_t0; + { + /* Calculate xform at t=1 */ + struct v2 linear_velocity = v2_clamp_len(e1->linear_velocity, GAME_MAX_LINEAR_VELOCITY); + f32 angular_velocity = clamp_f32(e1->angular_velocity, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY); + struct v2 tick_linear_velocity = v2_mul(linear_velocity, step_dt); + f32 tick_angular_velocity = angular_velocity * step_dt; + e1_xf_t1.og = v2_add(e1_xf_t1.og, tick_linear_velocity); + e1_xf_t1 = xform_basis_rotated_world(e1_xf_t1, tick_angular_velocity); + } + + f32 t = collider_time_of_impact(&e0_collider, &e1_collider, e0_xf_t0, e1_xf_t0, e0_xf_t1, e1_xf_t1, tolerance, max_iterations); + if (t != 0 && t < smallest_t) { + smallest_t = t; + } + } + } + + return smallest_t; +} + +/* ========================== * + * Integration + * ========================== */ + +void phys_integrate_forces(struct entity_store *store, f32 dt) +{ + 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_PHYSICAL_DYNAMIC) && !entity_has_prop(ent, ENTITY_PROP_PHYSICAL_KINEMATIC)) continue; + + struct xform xf = entity_get_xform(ent); + f32 det_abs = math_fabs(xform_get_determinant(xf)); + f32 mass = ent->mass_unscaled * det_abs; + f32 inertia = ent->inertia_unscaled * det_abs; + + /* Determine force & torque acceleration */ + struct v2 force_accel = v2_mul(v2_div(ent->force, mass), dt); + f32 torque_accel = (ent->torque / inertia) * dt; + + /* Integrate & clamp */ + ent->linear_velocity = v2_clamp_len(v2_add(ent->linear_velocity, force_accel), GAME_MAX_LINEAR_VELOCITY); + ent->angular_velocity = clamp_f32(ent->angular_velocity + torque_accel, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY); + + /* Reset forces */ + ent->force = V2(0, 0); + ent->torque = 0; + } +} + +void phys_integrate_velocities(struct entity_store *store, f32 dt) +{ + 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_PHYSICAL_DYNAMIC) && !entity_has_prop(ent, ENTITY_PROP_PHYSICAL_KINEMATIC)) continue; + + /* Clamp velocities */ + ent->linear_velocity = v2_clamp_len(ent->linear_velocity, GAME_MAX_LINEAR_VELOCITY); + ent->angular_velocity = clamp_f32(ent->angular_velocity, -GAME_MAX_ANGULAR_VELOCITY, GAME_MAX_ANGULAR_VELOCITY); + + struct v2 tick_linear_velocity = v2_mul(ent->linear_velocity, dt); + f32 tick_angular_velocity = ent->angular_velocity * dt; + + struct xform xf = entity_get_xform(ent); + xf.og = v2_add(xf.og, tick_linear_velocity); + xf = xform_basis_rotated_world(xf, tick_angular_velocity); + entity_set_xform(ent, xf); + } +} + +/* ========================== * + * Step + * ========================== */ + +void phys_step(struct entity_store *store, f32 step_dt, struct entity_lookup *contact_lookup, struct entity_lookup *collision_debug_lookup, u64 tick_id, struct v2 dbg_cursor_pos, b32 dbg_start_dragging, b32 dbg_stop_dragging) +{ + f32 remaining_dt = step_dt; + + phys_integrate_forces(store, remaining_dt); + while (remaining_dt > 0) { + /* TOI */ + { +#if GAME_PHYSICS_ENABLE_TOI + const f32 min_toi = 0.000001f; + const f32 tolerance = 0.00001f; + const u32 max_iterations = 16; + f32 earliest_toi = max_f32(phys_determine_earliest_toi_for_bullets(store, remaining_dt, tolerance, max_iterations), min_toi); + remaining_dt = remaining_dt * earliest_toi; +#else + (UNUSED)toi; + (UNUSED)determine_earliest_toi_for_bullets; +#endif + } + + phys_create_contacts(store, contact_lookup, collision_debug_lookup, tick_id); + phys_create_mouse_joints(store, dbg_cursor_pos, dbg_start_dragging, dbg_stop_dragging); + + phys_prepare_contacts(store, contact_lookup, collision_debug_lookup); + phys_prepare_motor_joints(store); + phys_prepare_mouse_joints(store); + + f32 substep_dt = remaining_dt / GAME_PHYSICS_SUBSTEPS; + for (u32 i = 0; i < GAME_PHYSICS_SUBSTEPS; ++i) { +#if GAME_PHYSICS_ENABLE_WARM_STARTING + phys_warm_start_contacts(store); + phys_warm_start_motor_joints(store); + phys_warm_start_mouse_joints(store); +#endif + +#if GAME_PHYSICS_ENABLE_COLLISION + phys_solve_contacts(store, substep_dt, true); +#endif + phys_solve_motor_joints(store, substep_dt); + phys_solve_mouse_joints(store, substep_dt); + + phys_integrate_velocities(store, substep_dt); + +#if GAME_PHYSICS_ENABLE_COLLISION && GAME_PHYSICS_ENABLE_RELAXATION + phys_solve_contacts(store, substep_dt, false); /* Relaxation */ +#endif + } + remaining_dt -= remaining_dt; + } +} diff --git a/src/phys.h b/src/phys.h new file mode 100644 index 00000000..7cea432a --- /dev/null +++ b/src/phys.h @@ -0,0 +1,171 @@ +#ifndef PHYS_H +#define PHYS_H + +#include "collider.h" +#include "math.h" + +struct entity_store; +struct entity_lookup; + +struct phys_hit_event { + struct entity_handle e0; + struct entity_handle e1; + struct v2 point; + struct v2 normal; + struct v2 vrel; /* Relative velocity */ +}; + +/* ========================== * + * Startup + * ========================== */ + +struct phys_startup_receipt { i32 _; }; +struct phys_startup_receipt phys_startup(void); + +/* ========================== * + * Contact + * ========================== */ + +struct phys_contact_point { + /* Contact point in local space of each entity */ + struct v2 point_local_e0; + struct v2 point_local_e1; + + u32 id; /* ID generated during clipping */ + f32 starting_separation; /* How far are original points along normal */ + f32 normal_impulse; /* Accumulated impulse along normal */ + f32 tangent_impulse; /* Accumulated impulse along tangent */ + + f32 inv_normal_mass; + f32 inv_tangent_mass; + + /* Debugging */ + struct v2 dbg_pt; +}; + +struct phys_contact_constraint { + u64 last_updated_tick; /* To avoid checking collisions for the same constraint twice in one tick */ + b32 skip_solve; + struct entity_handle e0; + struct entity_handle e1; + f32 inv_m0; + f32 inv_m1; + f32 inv_i0; + f32 inv_i1; + + struct v2 normal; /* Normal vector of collision from e0 -> e1 */ + u64 last_iteration; + struct phys_contact_point points[2]; + u32 num_points; + + f32 friction; + + struct math_spring_result softness; + f32 pushout_velocity; +}; + +struct phys_collision_debug { + struct entity_handle e0; + struct entity_handle e1; + struct collider_collision_points_result res; + + struct phys_contact_point points[2]; + u32 num_points; + + struct v2 closest0; + struct v2 closest1; + + struct xform xf0; + struct xform xf1; +}; + +void phys_create_contacts(struct entity_store *store, struct entity_lookup *contact_lookup, struct entity_lookup *debug_lookup, u64 tick_id); +void phys_prepare_contacts(struct entity_store *store, struct entity_lookup *contact_lookup, struct entity_lookup *debug_lookup); +void phys_warm_start_contacts(struct entity_store *store); +void phys_solve_contacts(struct entity_store *store, f32 dt, b32 apply_bias); + +/* ========================== * + * Motor joint + * ========================== */ + +struct phys_motor_joint_def { + struct entity_handle e0; + struct entity_handle e1; + f32 correction_rate; + f32 max_force; + f32 max_torque; +}; + +struct phys_motor_joint { + struct entity_handle e0; + struct entity_handle e1; + f32 correction_rate; + f32 max_force; + f32 max_torque; + + f32 inv_m0; + f32 inv_m1; + f32 inv_i0; + f32 inv_i1; + + struct v2 linear_impulse; + f32 angular_impulse; + + struct v2 point_local_e0; + struct v2 point_local_e1; + + struct xform linear_mass_xf; + f32 angular_mass; +}; + +struct phys_motor_joint motor_joint_from_def(struct phys_motor_joint_def def); +void phys_prepare_motor_joints(struct entity_store *store); +void phys_warm_start_motor_joints(struct entity_store *store); +void phys_solve_motor_joints(struct entity_store *store, f32 dt); + +/* ========================== * + * Mouse joint + * ========================== */ + +struct phys_mouse_joint { + struct entity_handle target; + struct v2 point_local_start; + struct v2 point_local_end; + struct math_spring_result linear_softness; + struct math_spring_result angular_softness; + f32 max_force; + + f32 inv_m; + f32 inv_i; + + struct v2 linear_impulse; + f32 angular_impulse; + + struct xform linear_mass_xf; +}; + +void phys_create_mouse_joints(struct entity_store *store, struct v2 cursor, b32 start_dragging, b32 stop_dragging); +void phys_prepare_mouse_joints(struct entity_store *store); +void phys_warm_start_mouse_joints(struct entity_store *store); +void phys_solve_mouse_joints(struct entity_store *store, f32 dt); + +/* ========================== * + * Earliest time of impact + * ========================== */ + +f32 phys_determine_earliest_toi_for_bullets(struct entity_store *store, f32 step_dt, f32 tolerance, u32 max_iterations); + +/* ========================== * + * Integration + * ========================== */ + +void phys_integrate_forces(struct entity_store *store, f32 dt); +void phys_integrate_velocities(struct entity_store *store, f32 dt); + +/* ========================== * + * Step + * ========================== */ + +void phys_step(struct entity_store *store, f32 step_dt, struct entity_lookup *contact_lookup, struct entity_lookup *collision_debug_lookup, u64 tick_id, struct v2 dbg_cursor_pos, b32 dbg_start_dragging, b32 dbg_stop_dragging); + +#endif diff --git a/src/user.c b/src/user.c index e0b4765b..8041dcab 100644 --- a/src/user.c +++ b/src/user.c @@ -1072,11 +1072,11 @@ INTERNAL void user_update(void) /* Draw constraint */ #if 1 if (entity_has_prop(ent, ENTITY_PROP_COLLISION_DEBUG)) { - struct collision_debug *data = &ent->collision_debug_data; + struct phys_collision_debug *data = &ent->collision_debug_data; struct collider_collision_points_result collider_res = data->res; - //struct contact_constraint *data = &entity_from_handle(store, data->contact_constraint_ent)->contact_constraint_data; - //struct contact_constraint *data = &data->contact_constraint_data; + //struct phys_contact_constraint *data = &entity_from_handle(store, data->contact_constraint_ent)->contact_constraint_data; + //struct phys_contact_constraint *data = &data->contact_constraint_data; struct entity *e0 = entity_from_handle(store, data->e0); struct entity *e1 = entity_from_handle(store, data->e1); struct collider_shape e0_collider = e0->local_collider; @@ -1216,7 +1216,7 @@ INTERNAL void user_update(void) { f32 radius = 5; for (u32 i = 0; i < data->num_points; ++i) { - struct contact_point point = data->points[i]; + struct phys_contact_point point = data->points[i]; #if 0 struct v2 p0 = xform_mul_v2(e0_xf, contact.point_local_e0); struct v2 p1 = xform_mul_v2(e1_xf, contact.point_local_e1);