From 4e920e071dcd11e184012b59bbc9945ecd1de3c2 Mon Sep 17 00:00:00 2001 From: jacob Date: Mon, 28 Oct 2024 14:33:26 -0500 Subject: [PATCH] motor joint --- src/collider.c | 562 +--------------------------------------- src/common.h | 5 +- src/config.h | 7 +- src/entity.h | 67 ++++- src/game.c | 678 +++++++++++++++++++++++++++---------------------- src/math.h | 13 +- src/user.c | 14 +- 7 files changed, 460 insertions(+), 886 deletions(-) diff --git a/src/collider.c b/src/collider.c index 7498d04e..286efdba 100644 --- a/src/collider.c +++ b/src/collider.c @@ -7,7 +7,6 @@ u32 collider_debug_steps = U32_MAX; //u32 collider_debug_steps = 1000000; //u32 collider_debug_steps = 50; -#endif INTERNAL void _dbgbreakable(void) { @@ -21,6 +20,9 @@ INTERNAL void _dbgbreakable(void) } else if (dbg_step >= collider_debug_steps - 1) { \ _dbgbreakable(); \ } +#else +#define DBGSTEP +#endif struct v2 collider_support_point(struct collider_shape *a, struct xform xf, struct v2 dir) { @@ -558,15 +560,14 @@ struct collider_collision_points_result collider_collision_points(struct collide b32 ignore_a = false; b32 ignore_b = false; -#if 1 if (v2_len_sq(v2_sub(contact_b, contact_a)) < (0.005f * 0.005f)) { + /* Merge contacts */ if (a_sep > b_sep) { ignore_a = true; } else { ignore_b = true; } } -#endif if (a_sep < tolerance && !ignore_a) { struct collider_collision_point *point = &points[num_points++]; @@ -597,7 +598,9 @@ struct collider_collision_points_result collider_collision_points(struct collide } res.solved = true; -abort: +#if COLLIDER_DEBUG + abort: +#endif u32 len = min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); for (u32 i = 0; i < len; ++i) { res.prototype.points[i] = proto[i]; @@ -726,554 +729,3 @@ b32 collider_collision_boolean(struct collider_shape *shape0, struct collider_sh return false; } #endif - - - - - - - -#if 0 -struct collider_collision_points_result collider_collision_points(struct collider_shape *shape0, struct collider_shape *shape1, struct xform xf0, struct xform xf1) -{ - struct temp_arena scratch = scratch_begin_no_conflict(); /* TODO: Only begin scratch for EPA */ - struct collider_collision_points_result res = ZI; - - struct v2 *points0 = shape0->points; - struct v2 *points1 = shape1->points; - u32 count0 = shape0->count; - u32 count1 = shape1->count; - f32 radius0 = shape0->radius; - f32 radius1 = shape1->radius; - (UNUSED)radius0; - (UNUSED)radius1; - - /* TODO: Parameterize */ - const f32 tolerance = 0.005f; /* How close can shapes be before collision is considered */ - const f32 min_unique_pt_dist_sq = 0.0001f * 0.0001f; - const u32 max_epa_iterations = 64; /* To prevent extremely large prototypes when origin is in exact center of rounded feature */ - - b32 colliding = false; - b32 simplex_is_closest_edge = false; - - struct collider_simplex s = ZI; - struct v2 *proto = NULL; - u32 proto_count = 0; - - struct v2 normal = ZI; - struct collider_collision_point points[2] = ZI; - u32 num_points = 0; - - struct v2 dir = ZI; - struct v2 m = ZI; - -#if COLLIDER_DEBUG - u32 dbg_step = 0; -#endif - - /* ========================== * - * GJK - * - * Determine encapsulating simplex if colliding, or closest edge / point to - * origin on simplex (for check if shape distances are within tolerance) - * ========================== */ - { - /* First point is support point in shape's general directions to eachother */ - dir = v2_sub(xf1.og, xf0.og); - if (v2_is_zero(dir)) dir = V2(1, 0); - s.a = menkowski_point(shape0, shape1, xf0, xf1, dir); - s.len = 1; - - struct v2 removed_a = ZI; - struct v2 removed_b = ZI; - u32 num_removed = 0; - while (true) { - if (s.len == 1) { - /* Second point is support point towards origin */ - dir = v2_neg(s.a); - - DBGSTEP; - m = menkowski_point(shape0, shape1, xf0, xf1, dir); - /* Check that new point is far enough away from existing point */ - if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq) { - simplex_is_closest_edge = true; - break; - } - s.b = s.a; - s.a = m; - s.len = 2; - - /* Third point is support point in direction of line normal towards origin */ - dir = v2_perp_towards_dir(v2_sub(s.b, s.a), v2_neg(s.a)); - } - - { - DBGSTEP; - m = menkowski_point(shape0, shape1, xf0, xf1, dir); - /* Check that new point is far enough away from existing points */ - if (v2_len_sq(v2_sub(m, s.a)) < min_unique_pt_dist_sq || - v2_len_sq(v2_sub(m, s.b)) < min_unique_pt_dist_sq || - ( - (num_removed >= 1) && ( - (v2_len_sq(v2_sub(m, removed_a)) < min_unique_pt_dist_sq) || - (num_removed >= 2 && v2_len_sq(v2_sub(m, removed_b)) < min_unique_pt_dist_sq)) - ) || - math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_sub(m, s.a))) < min_unique_pt_dist_sq) { - simplex_is_closest_edge = true; - break; - } - s.c = s.b; - s.b = s.a; - s.a = m; - s.len = 3; - - if (math_fabs(v2_wedge(v2_sub(s.b, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq || - math_fabs(v2_wedge(v2_sub(s.c, s.a), v2_neg(s.a))) <= min_unique_pt_dist_sq || - math_fabs(v2_wedge(v2_sub(s.c, s.b), v2_neg(s.b))) <= min_unique_pt_dist_sq) { - /* Simplex lies on origin */ - colliding = true; - break; - } - } - - /* Determine region of the simplex in which the origin lies */ - DBGSTEP; - struct v2 vab = v2_sub(s.b, s.a); - struct v2 vac = v2_sub(s.c, s.a); - struct v2 vbc = v2_sub(s.c, s.b); - - struct v2 rab_dir = v2_perp_towards_dir(vab, v2_neg(vac)); - struct v2 rac_dir = v2_perp_towards_dir(vac, v2_neg(vab)); - struct v2 rbc_dir = v2_perp_towards_dir(vbc, vab); - - f32 rab_dot = v2_dot(rab_dir, v2_neg(s.a)); - f32 rac_dot = v2_dot(rac_dir, v2_neg(s.a)); - f32 rbc_dot = v2_dot(rbc_dir, v2_neg(s.b)); - - f32 vab_dot = v2_dot(vab, v2_neg(s.a)) / v2_len_sq(vab); - f32 vac_dot = v2_dot(vac, v2_neg(s.a)) / v2_len_sq(vac); - f32 vbc_dot = v2_dot(vbc, v2_neg(s.b)) / v2_len_sq(vbc); - - if (rab_dot >= 0 && vab_dot >= 0 && vab_dot <= 1) { - /* Region ab, remove c */ - num_removed = 1; - removed_a = s.c; - s.len = 2; - dir = rab_dir; /* Next third point is in direction of region ab */ - } else if (rac_dot >= 0 && vac_dot >= 0 && vac_dot <= 1) { - /* Region ac, remove b */ - num_removed = 1; - removed_a = s.b; - s.len = 2; - s.b = s.c; - dir = rac_dir; /* Next third point is in direction of region ac */ - } else if (rbc_dot >= 0 && vbc_dot >= 0 && vbc_dot <= 1) { - /* Region bc, remove a */ - num_removed = 1; - removed_a = s.a; - s.len = 2; - s.a = s.b; - s.b = s.c; - dir = rbc_dir; /* Next third point is in direction of region bc */ - } else if (vab_dot <= 0 && vac_dot <= 0) { - /* Region a, remove bc */ - num_removed = 2; - removed_a = s.b; - removed_b = s.c; - s.len = 1; - } else if (vab_dot >= 1 && vbc_dot <= 0) { - /* Region b, remove ac */ - num_removed = 2; - removed_a = s.a; - removed_b = s.c; - s.len = 1; - s.a = s.b; - } else if (vac_dot >= 1 && vbc_dot >= 1) { - /* Region c, remove ab */ - num_removed = 2; - removed_a = s.a; - removed_b = s.b; - s.len = 1; - s.a = s.c; - } else { - /* No region, must be in simplex */ - colliding = true; - break; - } - } - } - - if (colliding) { - /* ========================== * - * Epa (to find collision normal from inside shape) - * ========================== */ - - const f32 epa_normal_epsilon_sq = 0.001f * 0.001f; - - proto = arena_dry_push(scratch.arena, struct v2); - proto_count = 0; - { - ASSERT(s.len == 3); - struct v2 *tmp = arena_push_array(scratch.arena, struct v2, 3); - tmp[0] = s.a; - tmp[1] = s.b; - tmp[2] = s.c; - proto_count = 3; - } - - u32 epa_iterations = 0; - while (colliding) { - ++epa_iterations; - f32 pen_len_sq = F32_INFINITY; - - /* Find dir from origin to closest edge */ - /* FIXME: Winding order of ps & pe index */ - u32 pen_ps_index = 0; - u32 pen_pe_index = 0; - struct v2 pen = ZI; - for (u32 i = 0; i < proto_count; ++i) { - u32 ps_index = i; - u32 pe_index = (i < proto_count - 1) ? (i + 1) : 0; - struct v2 ps = proto[ps_index]; - struct v2 pe = proto[pe_index]; - - struct v2 vse = v2_sub(pe, ps); - struct v2 vso = v2_neg(ps); - - struct v2 vsd = v2_mul(vse, (v2_dot(vso, vse) / v2_len_sq(vse))); - struct v2 pd = v2_add(ps, vsd); - - f32 pd_len_sq = v2_len_sq(pd); - if (pd_len_sq < pen_len_sq) { - pen_ps_index = ps_index; - pen_pe_index = pe_index; - pen_len_sq = pd_len_sq; - pen = pd; - } - } - - /* TODO: Remove this (debugging) */ - s.a = proto[pen_ps_index]; - s.b = proto[pen_pe_index]; - s.len = 2; - - /* Find new point in dir */ - DBGSTEP; - { - /* TODO: If winding order is guaranteed then this can become v2_perp_left/right? */ - struct v2 a = proto[pen_ps_index]; - struct v2 b = proto[pen_pe_index]; - struct v2 vab = v2_sub(b, a); - if (pen_len_sq < epa_normal_epsilon_sq) { - /* Next point is in direction of line normal pointing outwards from simplex */ - struct v2 n = proto[(pen_pe_index < proto_count - 1) ? (pen_pe_index + 1) : 0]; /* Next point along prototype after edge */ - dir = v2_perp_towards_dir(vab, v2_sub(a, n)); - } else { - dir = v2_perp_towards_dir(vab, a); - } - - } - m = menkowski_point(shape0, shape1, xf0, xf1, dir); - - /* Check unique */ - /* TODO: Better */ - { - b32 unique = true; - for (u32 i = 0; i < proto_count; ++i) { - struct v2 edge_start = proto[i]; - struct v2 edge_end = i < proto_count - 1 ? proto[i + 1] : proto[0]; - struct v2 vsm = v2_sub(m, edge_start); - if (v2_len_sq(vsm) < min_unique_pt_dist_sq || - math_fabs(v2_wedge(v2_sub(edge_end, edge_start), vsm)) < min_unique_pt_dist_sq) { - unique = false; - break; - } - } - if (!unique || epa_iterations >= max_epa_iterations) { - res.path = 1; - if (pen_len_sq < epa_normal_epsilon_sq) { - normal = v2_norm(dir); - } else { - normal = v2_norm(pen); - } - break; - } - } - - /* Insert point into prototype */ - /* FIXME: Preserve winding order */ - arena_push(scratch.arena, struct collider_menkowski_point); - ++proto_count; - for (u32 i = proto_count - 1; i > pen_pe_index; --i) { - u32 shift_from = (i > 0) ? i - 1 : proto_count - 1; - u32 shift_to = i; - proto[shift_to] = proto[shift_from]; - } - proto[pen_pe_index] = m; - } - } else if (simplex_is_closest_edge) { - if (s.len == 1) { - struct v2 p = v2_neg(s.a); - if (v2_len_sq(p) <= (tolerance * tolerance)) { - res.path = 2; - normal = v2_norm(dir); - colliding = true; - } - } else { - /* Shapes are not overlapping (origin is outside of simplex). Project - * origin to determine if distance is within tolerance. */ - ASSERT(s.len == 2); - struct v2 vab = v2_sub(s.b, s.a); - struct v2 vao = v2_neg(s.a); - f32 ratio = clamp_f32(v2_dot(vab, vao) / v2_dot(vab, vab), 0, 1); - struct v2 p = v2_add(s.a, v2_mul(vab, ratio)); - if (v2_len_sq(p) <= (tolerance * tolerance)) { - res.path = 2; - normal = v2_norm(dir); - colliding = true; - } - } - } - - if (colliding) { - /* ========================== * - * Clip to determine final points - * ========================== */ - - /* Max vertices must be < 16 to fit in 4 bit ids */ - CT_ASSERT(ARRAY_COUNT(shape0->points) <= 16); - - DBGSTEP; - { - //const f32 wedge_epsilon = 0.001f; - const f32 wedge_epsilon = 0.1f; - - /* shape0 a -> b winding = clockwise */ - u32 id_a0; - u32 id_b0; - struct v2 a0; - struct v2 b0; - - /* shape1 a -> b winding = counterclockwise */ - u32 id_a1; - u32 id_b1; - struct v2 a1; - struct v2 b1; - { - u32 p_i = collider_support_point_index(shape0, xf0, normal); - u32 a_i = (p_i > 0) ? (p_i - 1) : (count0 - 1); - u32 b_i = ((p_i + 1) < count0) ? (p_i + 1) : 0; - - struct v2 p = xform_mul_v2(xf0, points0[p_i]); - struct v2 a = xform_mul_v2(xf0, points0[a_i]); - struct v2 b = xform_mul_v2(xf0, points0[b_i]); - - struct v2 vap = v2_sub(p, a); - struct v2 vpb = v2_sub(b, p); - - /* Swap a & b depending on winding order */ - if (v2_wedge(vap, vpb) < 0) { - u32 tmp_u32 = a_i; - a_i = b_i; - b_i = tmp_u32; - struct v2 tmp_v2 = a; - a = b; - b = tmp_v2; - tmp_v2 = vap; - vap = v2_neg(vpb); - vpb = v2_neg(tmp_v2); - } - - f32 vap_wedge = v2_wedge(vap, normal); - f32 vpb_wedge = v2_wedge(vpb, normal); - if (vap_wedge < (vpb_wedge + wedge_epsilon)) { - id_a0 = a_i; - id_b0 = p_i; - a0 = a; - b0 = p; - } else { - id_a0 = p_i; - id_b0 = b_i; - a0 = p; - b0 = b; - } - } - { - struct v2 neg_normal = v2_neg(normal); - - u32 p_i = collider_support_point_index(shape1, xf1, neg_normal); - u32 a_i = ((p_i + 1) < count1) ? (p_i + 1) : 0; - u32 b_i = (p_i > 0) ? (p_i - 1) : (count1 - 1); - - struct v2 p = xform_mul_v2(xf1, points1[p_i]); - struct v2 a = xform_mul_v2(xf1, points1[a_i]); - struct v2 b = xform_mul_v2(xf1, points1[b_i]); - - struct v2 vap = v2_sub(p, a); - struct v2 vpb = v2_sub(b, p); - - /* Swap a & b depending on winding order */ - if (v2_wedge(vap, vpb) > 0) { - u32 tmp_u32 = a_i; - a_i = b_i; - b_i = tmp_u32; - struct v2 tmp_v2 = a; - a = b; - b = tmp_v2; - tmp_v2 = vap; - vap = v2_neg(vpb); - vpb = v2_neg(tmp_v2); - } - - f32 vap_wedge = v2_wedge(vap, normal); - f32 vpb_wedge = v2_wedge(vpb, normal); - if (vap_wedge < (vpb_wedge + wedge_epsilon)) { - id_a1 = a_i; - id_b1 = p_i; - a1 = a; - b1 = p; - } else { - id_a1 = p_i; - id_b1 = b_i; - a1 = p; - b1 = b; - } - } -#if 0 -#if 1 - if (radius0 > 0.0) { - struct v2 scale = xform_get_scale(xf0); - struct v2 normal_radius = v2_mul_v2(v2_mul(normal, radius0), scale); - a0 = v2_add(a0, normal_radius); - b0 = v2_add(b0, normal_radius); - } - - if (radius1 > 0.0) { - struct v2 scale = xform_get_scale(xf1); - struct v2 normal_radius = v2_mul_v2(v2_mul(normal, radius1), scale); - a1 = v2_sub(a1, normal_radius); - b1 = v2_sub(b1, normal_radius); - } -#else - if (radius0 > 0.0) { - struct v2 scale = xform_get_scale(xf0); - struct v2 perp_radius = v2_mul_v2(v2_with_len(v2_neg(v2_perp(v2_sub(b0, a0))), radius0), scale); - a0 = v2_add(a0, perp_radius); - b0 = v2_add(b0, perp_radius); - } - - if (radius1 > 0.0) { - struct v2 scale = xform_get_scale(xf1); - struct v2 perp_radius = v2_mul_v2(v2_with_len(v2_neg(v2_perp(v2_sub(b1, a1))), radius1), scale); - a1 = v2_sub(a1, perp_radius); - b1 = v2_sub(b1, perp_radius); - } -#endif -#endif - - f32 a0t = 0; - f32 a1t = 0; - f32 b0t = 0; - f32 b1t = 0; - - struct v2 vab0 = v2_sub(b0, a0); - struct v2 vab1 = v2_sub(b1, a1); - - { - struct v2 va0a1 = v2_sub(a1, a0); - struct v2 vb0b1 = v2_sub(b1, b0); - - f32 vab0_wedge_normal = v2_wedge(vab0, normal); - f32 vab1_wedge_normal = v2_wedge(vab1, normal); - f32 va0a1_wedge_normal = v2_wedge(va0a1, normal); - f32 vb0b1_wedge_normal = v2_wedge(vb0b1, normal); - - if (math_fabs(vab0_wedge_normal) > 0.01f) { - f32 w = 1 / vab0_wedge_normal; - a0t = clamp_f32(va0a1_wedge_normal * w, 0, 1); - b0t = clamp_f32(vb0b1_wedge_normal * -w, 0, 1); - } - - if (math_fabs(vab1_wedge_normal) > 0.01f) { - f32 w = 1 / vab1_wedge_normal; - a1t = clamp_f32(-va0a1_wedge_normal * w, 0, 1); - b1t = clamp_f32(-vb0b1_wedge_normal * -w, 0, 1); - } - } - - struct v2 a0_clipped = v2_add(a0, v2_mul(vab0, a0t)); - struct v2 a1_clipped = v2_add(a1, v2_mul(vab1, a1t)); - struct v2 b0_clipped = v2_add(b0, v2_mul(vab0, -b0t)); - struct v2 b1_clipped = v2_add(b1, v2_mul(vab1, -b1t)); - - struct v2 va0a1_clipped = v2_sub(a1_clipped, a0_clipped); - struct v2 vb0b1_clipped = v2_sub(b1_clipped, b0_clipped); - - f32 a_sep = v2_dot(va0a1_clipped, normal); - f32 b_sep = v2_dot(vb0b1_clipped, normal); - - struct v2 contact_a = v2_add(a0_clipped, v2_mul(va0a1_clipped, 0.5f)); - struct v2 contact_b = v2_add(b0_clipped, v2_mul(vb0b1_clipped, 0.5f)); - - //b32 merge_contacts = v2_len_sq(v2_sub(contact_b, contact_a)) < 0.01f; - b32 merge_contacts = false; - - b32 force = false; - -#if 0 - if (a_sep > tolerance && b_sep > tolerance) { - res.path = 999999999; - DEBUGBREAKABLE; - } -#endif - - if (force || a_sep < tolerance) { - struct collider_collision_point *point = &points[num_points++]; - point->id = id_a0 | (id_a1 << 4); - point->separation = a_sep; - point->point = contact_a; - } - - if (force || (b_sep < tolerance && !merge_contacts)) { - struct collider_collision_point *point = &points[num_points++]; - point->id = id_b0 | (id_b1 << 4); - point->separation = b_sep; - point->point = contact_b; - } - - res.a0 = a0_clipped; - res.a1 = a1_clipped; - res.b0 = b0_clipped; - res.b1 = b1_clipped; - } - } - - res.solved = true; -abort: - if (proto_count > 0) { - u32 len = min_u32(proto_count, ARRAY_COUNT(res.prototype.points)); - for (u32 i = 0; i < len; ++i) { - res.prototype.points[i] = proto[i]; - } - res.prototype.len = len; - } else { - if (s.len >= 1) { - res.prototype.points[0] = s.a; - if (s.len >= 2) { - res.prototype.points[1] = s.b; - if (s.len >= 3) { - res.prototype.points[2] = s.c; - } - } - } - res.prototype.len = s.len; - } - res.normal = normal; - res.points[0] = points[0]; - res.points[1] = points[1]; - res.num_points = num_points; - res.simplex = s; - scratch_end(scratch); - return res; -} -#endif diff --git a/src/common.h b/src/common.h index 9fecbdf9..92cd0dcd 100644 --- a/src/common.h +++ b/src/common.h @@ -578,7 +578,10 @@ struct xform { }; struct mat4x4 { - f32 e[4][4]; + union { + struct { struct v4 bx, by, bz, bw; }; + f32 e[4][4]; + }; }; #define RECT(_x, _y, _width, _height) (struct rect) { .x = (_x), .y = (_y), .width = (_width), .height = (_height) } diff --git a/src/config.h b/src/config.h index d41de787..3eb9323f 100644 --- a/src/config.h +++ b/src/config.h @@ -33,12 +33,12 @@ #define GAME_TIMESCALE 1.0 #define GAME_PHYSICS_SUBSTEPS 4 -#define GAME_PHYSICS_ENABLE_WARM_STARTING 1 +#define GAME_PHYSICS_ENABLE_WARM_STARTING 0 #define GAME_PHYSICS_ENABLE_RELAXATION 1 #define USER_DRAW_MENKOWSKI 0 #define GAME_PHYSICS_ENABLE_GROUND_FRICTION 1 -#define GAME_PHYSICS_ENABLE_COLLISION 0 +#define GAME_PHYSICS_ENABLE_COLLISION 1 #define GAME_SPAWN_LOTS 0 #define GAME_SPAWN_TESTENT 0 #define GAME_SPAWN_BOX 1 @@ -54,7 +54,8 @@ #define USER_INTERP_OFFSET_TICK_RATIO 1.1 #define USER_INTERP_ENABLED 1 -#define COLLIDER_DEBUG RTC +//#define COLLIDER_DEBUG RTC +#define COLLIDER_DEBUG 1 /* ========================== * * Settings diff --git a/src/entity.h b/src/entity.h index fd079a80..a34b1b6a 100644 --- a/src/entity.h +++ b/src/entity.h @@ -13,6 +13,7 @@ enum entity_prop { ENTITY_PROP_PHYSICAL, ENTITY_PROP_CONTACT_CONSTRAINT, + ENTITY_PROP_MOTOR_JOINT, ENTITY_PROP_PLAYER_CONTROLLED, ENTITY_PROP_CAMERA, @@ -78,7 +79,6 @@ struct contact_point { struct contact_constraint { struct entity_handle e0; struct entity_handle e1; - f32 inv_m0; f32 inv_m1; f32 inv_i0; @@ -96,6 +96,58 @@ struct contact_constraint { }; + + + + +struct motor_joint_def { + struct entity_handle e0; + struct entity_handle e1; + f32 correction_factor; + f32 max_force; + f32 max_torque; +}; + +struct motor_joint { + struct entity_handle e0; + struct entity_handle e1; + f32 correction_factor; + 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_factor = clamp_f32(def.correction_factor, 0, 1); + res.max_force = def.max_force; + res.max_torque = def.max_torque; + return res; +} + + + + + + + + struct entity { /* ====================================================================== */ /* Metadata */ @@ -135,14 +187,17 @@ struct entity { struct collider_shape local_collider; - - - /* ====================================================================== */ /* Contact constraint */ /* ENTITY_PROP_CONSTRAINT_CONTACT */ - struct contact_constraint contact_constraint; + struct contact_constraint contact_constraint_data; + + /* ====================================================================== */ + /* Motor joint */ + + /* ENTITY_PROP_MOTOR_JOINT */ + struct motor_joint motor_joint_data; @@ -173,6 +228,8 @@ struct entity { struct v2 focus; } control; + struct entity_handle aim_joint; + /* ====================================================================== */ /* Physics */ diff --git a/src/game.c b/src/game.c index c0ae2dc0..86df0f2f 100644 --- a/src/game.c +++ b/src/game.c @@ -122,6 +122,8 @@ INTERNAL void activate_now(struct entity *ent) INTERNAL void spawn_test_entities(f32 offset) { struct entity *root = entity_from_handle(G.tick.entity_store, G.tick.entity_store->root); + root->mass_unscaled = F32_INFINITY; + root->inertia_unscaled = F32_INFINITY; //const f32 offset_all = -20000; //const f32 offset_all = -5000; @@ -148,8 +150,9 @@ INTERNAL void spawn_test_entities(f32 offset) pos = v2_add(pos, V2(0, offset_all)); //struct v2 size = V2(1, 1); + struct v2 size = V2(0.25, 0.25); //struct v2 size = V2(0.5, 0.5); - struct v2 size = V2(0.5, 0.25); + //struct v2 size = V2(0.5, 0.25); //struct v2 size = V2(1.0, 1.0); //struct v2 size = V2(1.5, 1.5); //f32 r = PI; @@ -343,10 +346,10 @@ INTERNAL void spawn_test_entities(f32 offset) /* ========================== * - * TESTING MANIFOLDS / CONTACTS + * TESTING CONTACT CONSTRAINT * ========================== */ -INTERNAL void generate_contacts(void) +INTERNAL void prepare_contacts(void) { /* TODO: Remove this */ /* FIXME: Dict has leaks: Entries never removed, even when constraint is no longer valid, or either entities are no longer valid. */ @@ -364,7 +367,6 @@ INTERNAL void generate_contacts(void) struct entity_store *store = G.tick.entity_store; struct entity *root = G.root; -#if 0 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; @@ -384,55 +386,56 @@ INTERNAL void generate_contacts(void) continue; } - /* Retrieve constraint */ u64 constraint_hash; struct string constraint_key; - { - struct entity_handle h0 = e0->handle; - struct entity_handle h1 = e1->handle; - constraint_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&h0)); - constraint_hash = hash_fnv64(constraint_hash, BUFFER_FROM_STRUCT(&h1)); - constraint_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&constraint_hash)); - } - + struct entity_handle *entry; struct entity *constraint_ent = NULL; - struct entity_handle *entry = fixed_dict_get(&dict, constraint_key); - if (entry) { - struct entity *t = entity_from_handle(store, *entry); - if (entity_is_valid_and_active(t)) { - constraint_ent = t; + { + { + struct entity_handle h0 = e0->handle; + struct entity_handle h1 = e1->handle; + constraint_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&h0)); + constraint_hash = hash_fnv64(constraint_hash, BUFFER_FROM_STRUCT(&h1)); + constraint_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&constraint_hash)); + } + + entry = fixed_dict_get(&dict, constraint_key); + if (entry) { + struct entity *t = entity_from_handle(store, *entry); + if (entity_is_valid_and_active(t)) { + if (t->contact_constraint_data.last_iteration == constraint_iteration) { + /* Constraint has already been computed this iteration */ + continue; + } else { + t->contact_constraint_data.last_iteration = constraint_iteration; + constraint_ent = t; + } + } else { + /* Constraint entity no longer valid */ + fixed_dict_set(&dict_arena, &dict, constraint_key, NULL); + continue; + } } } - /* Ensure constraint hasn't already been computed this iteration */ - if (constraint_ent) { - if (constraint_ent->contact_constraint.last_iteration == constraint_iteration) { - /* Already iterated this constraint from The other entity's perspective, skip */ - continue; - } - constraint_ent->contact_constraint.last_iteration = constraint_iteration; - } - - /* Calculate entity 1 shape */ + /* Calculate collision */ struct xform e1_xf = entity_get_xform(e1); - struct collider_shape e1_collider = e1->local_collider; - - struct collider_collision_points_result res = collider_collision_points(&e0_collider, &e1_collider, e0_xf, e1_xf); + struct collider_collision_points_result res = collider_collision_points(&e0_collider, &e1->local_collider, e0_xf, e1_xf); /* Parts of algorithm are hard-coded to support 2 contact points */ - CT_ASSERT(ARRAY_COUNT(constraint_ent->contact_constraint.points) == 2); + CT_ASSERT(ARRAY_COUNT(constraint_ent->contact_constraint_data.points) == 2); CT_ASSERT(ARRAY_COUNT(res.points) == 2); + /* TODO: Move this down */ if (res.num_points > 0 || COLLIDER_DEBUG) { if (!constraint_ent) { constraint_ent = entity_alloc(root); - constraint_ent->contact_constraint.e1 = e1->handle; - constraint_ent->contact_constraint.e0 = e0->handle; + constraint_ent->contact_constraint_data.e1 = e1->handle; + constraint_ent->contact_constraint_data.e0 = e0->handle; /* TODO: Should we recalculate normal as more contact points are added? */ entity_enable_prop(constraint_ent, ENTITY_PROP_CONTACT_CONSTRAINT); activate_now(constraint_ent); - if (entry) { *entry = constraint_ent->handle; } else { @@ -441,7 +444,8 @@ INTERNAL void generate_contacts(void) fixed_dict_set(&dict_arena, &dict, constraint_key, entry); } } - data->normal = res.normal; + struct contact_constraint *constraint = &constraint_ent->contact_constraint_data; + constraint->normal = res.normal; /* TODO: Remove this (debugging) */ #if COLLIDER_DEBUG @@ -465,11 +469,13 @@ INTERNAL void generate_contacts(void) } if (res.num_points > 0) { + struct contact_constraint *constraint = &constraint_ent->contact_constraint_data; + struct v2 normal = res.normal; struct v2 tangent = v2_perp(normal); /* TODO: Cache this */ - /* Prepare constraint masses */ + /* Calculate masses */ f32 inv_m0; f32 inv_m1; f32 inv_i0; @@ -481,15 +487,15 @@ INTERNAL void generate_contacts(void) inv_m1 = 1.f / (e1->mass_unscaled * scale1); inv_i0 = 1.f / (e0->inertia_unscaled * scale0); inv_i1 = 1.f / (e1->inertia_unscaled * scale1); - data->inv_m0 = inv_m0; - data->inv_m1 = inv_m1; - data->inv_i0 = inv_i0; - data->inv_i1 = inv_i1; } + constraint->inv_m0 = inv_m0; + constraint->inv_m1 = inv_m1; + constraint->inv_i0 = inv_i0; + constraint->inv_i1 = inv_i1; /* Delete old contacts that are no longer present */ for (u32 i = 0; i < constraint->num_points; ++i) { - struct contact *old = &constraint->contacts[i]; + struct contact_point *old = &constraint->points[i]; u32 id = old->id; b32 found = false; for (u32 j = 0; j < res.num_points; ++j) { @@ -500,223 +506,7 @@ INTERNAL void generate_contacts(void) } if (!found) { /* Delete contact by replacing with last in array */ - *old = constraint->contacts[--constraint->num_points]; - --i; - } - } - - /* Update / insert returned contacts */ - for (u32 i = 0; i < res.num_points; ++i) { - struct collider_collision_point *res_point = &res.points[i]; - struct v2 point = res_point->point; - f32 sep = res_point->separation; - u32 id = res_point->id; - struct contact *contact = NULL; - /* Match */ - for (u32 j = 0; j < constraint->num_points; ++j) { - struct contact *t = &constraint->contacts[j]; - if (t->id == id) { - contact = t; - break; - } - } - if (contact) { - /* Update existing */ -#if !GAME_PHYSICS_ENABLE_WARM_STARTING - contact->normal_impulse = 0; - contact->tangent_impulse = 0; -#endif - } else { - /* Insert new */ - contact = &constraint->contacts[constraint->num_points++]; - MEMZERO_STRUCT(contact); - contact->id = id; - } - contact->point_local_e0 = xform_invert_mul_v2(e0_xf, point); - contact->point_local_e1 = xform_invert_mul_v2(e1_xf, point); - contact->starting_separation = sep; - - /* TODO: Remove this (debugging) */ -#if COLLIDER_DEBUG - { - contact->dbg_pt = point; - } -#endif - - { - struct v2 vcp0 = v2_sub(point, e0_xf.og); - struct v2 vcp1 = v2_sub(point, e1_xf.og); - - /* Normal mass */ - { - f32 vcp0_wedge = v2_wedge(vcp0, normal); - f32 vcp1_wedge = v2_wedge(vcp1, normal); - f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge); - contact->inv_normal_mass = k > 0.0f ? 1.0f / k : 0.0f; - } - - /* Tangent mass */ - { - f32 vcp0_wedge = v2_wedge(vcp0, tangent); - f32 vcp1_wedge = v2_wedge(vcp1, tangent); - f32 k = (inv_m0 + inv_m1) + (inv_i0 * vcp0_wedge * vcp0_wedge) + (inv_i1 * vcp1_wedge * vcp1_wedge); - contact->inv_tangent_mass = k > 0.0f ? 1.0f / k : 0.0f; - } - } - } - - - } else if (constraint) { -#if COLLIDER_DEBUG - constraint->num_points = 0; -#else - /* No longer colliding, delete constraint */ - constraint->num_points = 0; - entity_enable_prop(constraint, ENTITY_PROP_RELEASE_AT_END_OF_FRAME); -#endif - } - } - } -#else - 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)) continue; - - struct xform e0_xf = entity_get_xform(e0); - struct collider_shape e0_collider = e0->local_collider; - - for (u64 e1_index = 0; 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)) continue; - - /* TODO: Remove this (temporary stop to prevent double-constraint creation) */ - if (e0_index >= e1_index) { - continue; - } - - u64 constraint_hash; - struct string constraint_key; - struct entity_handle *entry; - struct entity *constraint = NULL; - { - { - struct entity_handle h0 = e0->handle; - struct entity_handle h1 = e1->handle; - constraint_hash = hash_fnv64(HASH_FNV64_BASIS, BUFFER_FROM_STRUCT(&h0)); - constraint_hash = hash_fnv64(constraint_hash, BUFFER_FROM_STRUCT(&h1)); - constraint_key = STRING_FROM_BUFFER(BUFFER_FROM_STRUCT(&constraint_hash)); - } - - entry = fixed_dict_get(&dict, constraint_key); - if (entry) { - struct entity *t = entity_from_handle(store, *entry); - if (entity_is_valid_and_active(t)) { - if (t->contact_constraint.last_iteration == constraint_iteration) { - /* Constraint has already been computed this iteration */ - continue; - } else { - t->contact_constraint.last_iteration = constraint_iteration; - constraint = t; - } - } else { - /* Constraint entity no longer valid */ - continue; - } - } - } - - /* Calculate collision */ - struct xform e1_xf = entity_get_xform(e1); - struct collider_collision_points_result res = collider_collision_points(&e0_collider, &e1->local_collider, e0_xf, e1_xf); - - /* Parts of algorithm are hard-coded to support 2 contact points */ - CT_ASSERT(ARRAY_COUNT(constraint->contact_constraint.points) == 2); - CT_ASSERT(ARRAY_COUNT(res.points) == 2); - - - /* TODO: Move this down */ - if (res.num_points > 0 || COLLIDER_DEBUG) { - if (!constraint) { - constraint = entity_alloc(root); - constraint->contact_constraint.e1 = e1->handle; - constraint->contact_constraint.e0 = e0->handle; - /* TODO: Should we recalculate normal as more contact points are added? */ - entity_enable_prop(constraint, ENTITY_PROP_CONTACT_CONSTRAINT); - activate_now(constraint); - if (entry) { - *entry = constraint->handle; - } else { - entry = arena_push(&dict_arena, struct entity_handle); - *entry = constraint->handle; - fixed_dict_set(&dict_arena, &dict, constraint_key, entry); - } - } - struct contact_constraint *data = &constraint->contact_constraint; - data->normal = res.normal; - - /* TODO: Remove this (debugging) */ -#if COLLIDER_DEBUG - { - data->res = res; - data->dbg_xf0 = e0_xf; - data->dbg_xf1 = e1_xf; - if (data->num_points == 0) { - if (res.num_points > 0) { - ++e0->colliding; - ++e1->colliding; - } - } else { - if (res.num_points == 0) { - --e0->colliding; - --e1->colliding; - } - } - } -#endif - } - - if (res.num_points > 0) { - struct contact_constraint *data = &constraint->contact_constraint; - - struct v2 normal = res.normal; - struct v2 tangent = v2_perp(normal); - - /* TODO: Cache this */ - /* Prepare constraint 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); - data->inv_m0 = inv_m0; - data->inv_m1 = inv_m1; - data->inv_i0 = inv_i0; - data->inv_i1 = inv_i1; - } - - /* Delete old contacts that are no longer present */ - for (u32 i = 0; i < data->num_points; ++i) { - struct contact_point *old = &data->points[i]; - u32 id = old->id; - b32 found = false; - for (u32 j = 0; j < res.num_points; ++j) { - if (res.points[j].id == id) { - found = true; - break; - } - } - if (!found) { - /* Delete contact by replacing with last in array */ - *old = data->points[--data->num_points]; + *old = constraint->points[--constraint->num_points]; --i; } } @@ -729,8 +519,8 @@ INTERNAL void generate_contacts(void) u32 id = res_point->id; struct contact_point *contact = NULL; /* Match */ - for (u32 j = 0; j < data->num_points; ++j) { - struct contact_point *t = &data->points[j]; + for (u32 j = 0; j < constraint->num_points; ++j) { + struct contact_point *t = &constraint->points[j]; if (t->id == id) { contact = t; break; @@ -744,10 +534,11 @@ INTERNAL void generate_contacts(void) #endif } else { /* Insert new */ - contact = &data->points[data->num_points++]; + contact = &constraint->points[constraint->num_points++]; MEMZERO_STRUCT(contact); contact->id = id; } + 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; @@ -758,7 +549,6 @@ INTERNAL void generate_contacts(void) contact->dbg_pt = point; } #endif - { struct v2 vcp0 = v2_sub(point, e0_xf.og); struct v2 vcp1 = v2_sub(point, e1_xf.og); @@ -782,16 +572,16 @@ INTERNAL void generate_contacts(void) } - } else if (constraint) { - constraint->contact_constraint.num_points= 0; + } else if (constraint_ent) { + constraint_ent->contact_constraint_data.num_points = 0; #if !COLLIDER_DEBUG /* No longer colliding, delete constraint */ - entity_enable_prop(constraint, ENTITY_PROP_RELEASE_AT_END_OF_FRAME); + fixed_dict_set(&dict_arena, &dict, constraint_key, NULL); + entity_enable_prop(constraint_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME); #endif } } } -#endif } INTERNAL void warm_start_contacts(void) @@ -799,23 +589,23 @@ 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 = &store->entities[entity_index]; - if (!entity_is_valid_and_active(constraint)) continue; - if (!entity_has_prop(constraint, ENTITY_PROP_CONTACT_CONSTRAINT)) continue; + 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 *data = &constraint->contact_constraint; + struct contact_constraint *constraint = &constraint_ent->contact_constraint_data; - u32 num_points = data->num_points; - struct entity *e0 = entity_from_handle(store, data->e0); - struct entity *e1 = entity_from_handle(store, data->e1); + 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 xform e0_xf = entity_get_xform(e0); struct xform e1_xf = entity_get_xform(e1); - f32 inv_m0 = data->inv_m0; - f32 inv_m1 = data->inv_m1; - f32 inv_i0 = data->inv_i0; - f32 inv_i1 = data->inv_i1; + 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; @@ -823,11 +613,11 @@ INTERNAL void warm_start_contacts(void) f32 w1 = e1->angular_velocity; /* Warm start */ - struct v2 normal = data->normal; + 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 = &data->points[i]; + struct contact_point *point = &constraint->points[i]; struct v2 p0 = xform_mul_v2(e0_xf, point->point_local_e0); struct v2 p1 = xform_mul_v2(e1_xf, point->point_local_e1); struct v2 vcp0 = v2_sub(p0, e0_xf.og); @@ -872,35 +662,34 @@ 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 = &store->entities[entity_index]; - if (!entity_is_valid_and_active(constraint)) continue; - if (!entity_has_prop(constraint, ENTITY_PROP_CONTACT_CONSTRAINT)) continue; + 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 *data = &constraint->contact_constraint; + struct contact_constraint *constraint = &constraint_ent->contact_constraint_data; - struct entity *e0 = entity_from_handle(store, data->e0); - struct entity *e1 = entity_from_handle(store, data->e1); + 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 = data->num_points; + u32 num_points = constraint->num_points; if (num_points > 0 && 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); - f32 inv_m0 = data->inv_m0; - f32 inv_m1 = data->inv_m1; - f32 inv_i0 = data->inv_i0; - f32 inv_i1 = data->inv_i1; + 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 = data->normal; + struct v2 normal = constraint->normal; for (u32 point_index = 0; point_index < num_points; ++point_index) { - struct contact_point *point = &data->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); @@ -919,7 +708,7 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) /* Soft constraint */ f32 contact_damping_ratio = 10.0f; - f32 contact_hertz = (GAME_FPS * GAME_PHYSICS_SUBSTEPS) / 8.f; + f32 contact_hertz = (1.f / dt) / 8; struct soft_result softness = make_soft(contact_hertz, contact_damping_ratio, dt); @@ -955,7 +744,7 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) /* Tangent impulse */ struct v2 tangent = v2_perp(normal); for (u32 point_index = 0; point_index < num_points; ++point_index) { - struct contact_point *point = &data->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); @@ -997,11 +786,184 @@ INTERNAL void solve_contacts(f32 dt, b32 apply_bias) + + + + /* ========================== * * TESTING MOTOR JOINT * ========================== */ +#if 1 +INTERNAL void prepare_motor_joints(void) +{ + struct entity_store *store = G.tick.entity_store; + 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_invert_mul_v2(e0_xf, joint->point_local_e0), e0_xf.og); + struct v2 vcp1 = v2_sub(xform_invert_mul_v2(e1_xf, joint->point_local_e1), e1_xf.og); + + struct 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 { + entity_disable_prop(joint_ent, ENTITY_PROP_ACTIVE); + entity_enable_prop(joint_ent, ENTITY_PROP_RELEASE_AT_END_OF_FRAME); + } + } +} + +INTERNAL void warm_start_motor_joints(void) +{ +} + +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_factor / dt; + + /* Angular constraint */ + { + f32 max_impulse = joint->max_torque * dt; + +#if 0 + f32 angular_separation = math_unwind_angle(xform_get_rotation(e1_xf) - xform_get_rotation(e0_xf)); + f32 angular_bias = angular_separation * correction_rate; + + f32 vel_diff = math_unwind_angle(w1 - w0); + f32 impulse = -joint->angular_mass * (vel_diff + angular_bias); +#else + f32 angular_separation = math_unwind_angle(xform_get_rotation(e1_xf) - xform_get_rotation(e0_xf)); + f32 angular_bias = angular_separation * correction_rate; + + //f32 vel_diff = w1 - w0; + f32 vel_diff = 0; + f32 impulse = -joint->angular_mass * (vel_diff + angular_bias); +#endif + + f32 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 Cdot = 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(Cdot, 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(impulse, vcp0) * inv_i0; + w1 += v2_wedge(impulse, vcp1) * inv_i1; + } + + e0->linear_velocity = v0; + e0->angular_velocity = w0; + e1->linear_velocity = v1; + e1->angular_velocity = w1; + } + +} + +#else + +INTERNAL void prepare_motor_joints(void) +{ +} + +INTERNAL void warm_start_motor_joints(void) +{ +} + +INTERNAL void solve_motor_joints(f32 dt) +{ + (UNUSED)dt; +} + +#endif @@ -1030,13 +992,9 @@ INTERNAL void integrate_velocities_from_forces(f32 dt) struct v2 force_accel = v2_mul(v2_div(ent->force, mass), dt); f32 torque_accel = (ent->torque / inertia) * dt; - /* Integrate */ - ent->linear_velocity = v2_add(ent->linear_velocity, force_accel); - ent->angular_velocity += torque_accel; - - /* 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); + /* 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); @@ -1057,9 +1015,10 @@ INTERNAL void integrate_positions_from_velocities(f32 dt) 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 xform xf = entity_get_xform(ent); 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); @@ -1545,6 +1504,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * ========================== */ #if GAME_PLAYER_AIM +#if 0 for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { struct entity *ent = &store->entities[entity_index]; if (!entity_is_valid_and_active(ent)) continue; @@ -1599,7 +1559,7 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) if (!F32_IS_NAN(final_xf_angle)) { const f32 angle_error_allowed = 0.001; - f32 diff = math_diff_angle(final_xf_angle, old_angle); + f32 diff = math_unwind_angle(final_xf_angle - old_angle); if (math_fabs(diff) > angle_error_allowed) { xf = xform_basis_rotated_world(xf, diff); } @@ -1608,6 +1568,96 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) entity_set_xform(ent, xf); } } +#else + for (u64 entity_index = 0; entity_index < store->reserved; ++entity_index) { + struct entity *ent = &store->entities[entity_index]; + if (!entity_is_valid_and_active(ent)) continue; + + if (entity_has_prop(ent, ENTITY_PROP_PLAYER_CONTROLLED)) { + struct xform xf = entity_get_xform(ent); + struct xform sprite_xf = xform_mul(xf, ent->sprite_local_xform); + + /* Retrieve / create aim joint */ + struct entity *joint_ent = entity_from_handle(store, ent->aim_joint); + if (!entity_is_valid_and_active(joint_ent)) { + joint_ent = entity_alloc(root); + entity_enable_prop(joint_ent, ENTITY_PROP_MOTOR_JOINT); + entity_enable_prop(joint_ent, ENTITY_PROP_ACTIVE); + + struct motor_joint_def def = ZI; + def.e0 = joint_ent->handle; /* Re-using joint entity as e0 */ + def.e1 = ent->handle; + def.correction_factor = 0.5; + def.max_force = 0; + def.max_torque = 2500; + joint_ent->motor_joint_data = motor_joint_from_def(def); + + ent->aim_joint = joint_ent->handle; + } + + /* Solve for final angle using law of sines */ + f32 new_angle; + { + struct v2 ent_pos = xf.og; + struct v2 focus_pos = v2_add(ent_pos, ent->control.focus); + + struct v2 sprite_hold_pos; + struct v2 sprite_hold_dir; + { + struct sprite_sheet *sheet = sprite_sheet_from_tag_await(sprite_frame_scope, ent->sprite); + struct sprite_sheet_slice slice = sprite_sheet_get_slice(sheet, STR("attach.wep"), ent->animation_frame); + sprite_hold_pos = slice.center; + sprite_hold_dir = slice.dir; + } + + struct v2 hold_dir = xform_basis_mul_v2(sprite_xf, sprite_hold_dir); + struct v2 hold_pos = xform_mul_v2(sprite_xf, sprite_hold_pos); + if (v2_eq(hold_pos, ent_pos)) { + /* If hold pos is same as origin (E.G if pivot is being used as hold pos), then move hold pos forward a tad to avoid issue */ + sprite_hold_pos = v2_add(sprite_hold_pos, V2(0, -1)); + hold_pos = xform_mul_v2(sprite_xf, sprite_hold_pos); + } + + f32 forward_hold_angle_offset; + { + struct xform xf_unrotated = xform_basis_with_rotation_world(xf, 0); + struct v2 hold_pos_unrotated = xform_mul_v2(xf_unrotated, xform_mul_v2(ent->sprite_local_xform, sprite_hold_pos)); + forward_hold_angle_offset = v2_angle_from_dirs(V2(0, -1), v2_sub(hold_pos_unrotated, xf_unrotated.og)); + } + + struct v2 hold_ent_dir = v2_sub(ent_pos, hold_pos); + struct v2 focus_ent_dir = v2_sub(ent_pos, focus_pos); + + f32 hold_ent_len = v2_len(hold_ent_dir); + f32 focus_ent_len = v2_len(focus_ent_dir); + + f32 final_hold_angle_btw_ent_and_focus = v2_angle_from_dirs(hold_ent_dir, hold_dir); + f32 final_focus_angle_btw_ent_and_hold = math_asin((math_sin(final_hold_angle_btw_ent_and_focus) * hold_ent_len) / focus_ent_len); + f32 final_ent_angle_btw_focus_and_hold = PI - (final_focus_angle_btw_ent_and_hold + final_hold_angle_btw_ent_and_focus); + + new_angle = math_unwind_angle(v2_angle_from_dirs(V2(0, -1), v2_sub(focus_pos, ent_pos)) + final_ent_angle_btw_focus_and_hold - forward_hold_angle_offset); + } + +#if 1 + if (!F32_IS_NAN(new_angle)) { + const f32 angle_error_allowed = 0.001; + struct xform joint_xf = entity_get_xform(joint_ent); + f32 diff = math_unwind_angle(new_angle - xform_get_rotation(joint_xf)); + if (math_fabs(diff) > angle_error_allowed) { + struct xform joint_ent_xf = xform_basis_with_rotation_world(joint_xf, new_angle); + entity_set_xform(joint_ent, joint_ent_xf); + } + } +#endif + +#if 0 + struct xform joint_ent_xf = xform_basis_with_rotation_world(entity_get_xform(joint_ent), new_angle); + entity_set_xform(joint_ent, joint_ent_xf); +#endif + + } + } +#endif #endif /* ========================== * @@ -1688,25 +1738,37 @@ INTERNAL void game_update(struct game_cmd_array game_cmds) * ========================== */ - (UNUSED)generate_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)integrate_velocities_from_forces; (UNUSED)integrate_positions_from_velocities; - (UNUSED)warm_start_contacts; #if 1 { integrate_velocities_from_forces(dt); - generate_contacts(); + prepare_contacts(); + prepare_motor_joints(); f32 substep_dt = dt / GAME_PHYSICS_SUBSTEPS; for (u32 i = 0; i < GAME_PHYSICS_SUBSTEPS; ++i) { #if GAME_PHYSICS_ENABLE_WARM_STARTING warm_start_contacts(); + warm_start_motor_joints(); #endif + #if GAME_PHYSICS_ENABLE_COLLISION solve_contacts(substep_dt, true); #endif + solve_motor_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 diff --git a/src/math.h b/src/math.h index eecdaf2f..004393cf 100644 --- a/src/math.h +++ b/src/math.h @@ -579,12 +579,11 @@ INLINE f32 math_acos(f32 x) return (PI / 2.0f) - math_atan2(x, math_sqrt(1.0f - (x * x))); } -/* Returns wrapped difference between angles. - * E.G. diff(PI, -PI) = 0 */ -INLINE f32 math_diff_angle(f32 a, f32 b) +/* Returns angle in range [-PI, PI] */ +INLINE f32 math_unwind_angle(f32 a) { - f32 diff = math_fmod(a - b, TAU); - return math_fmod(2.0f * diff, TAU) - diff; + f32 d = math_fmod(a, TAU); + return math_fmod(2.0f * d, TAU) - d; } /* ========================== * @@ -602,7 +601,7 @@ INLINE f64 math_lerp64(f64 val0, f64 val1, f64 t) } INLINE f32 math_lerp_angle(f32 a, f32 b, f32 t) { - f32 diff = math_diff_angle(b, a); + f32 diff = math_unwind_angle(b - a); return a + diff * t; } @@ -708,7 +707,7 @@ INLINE struct v2 v2_with_len(struct v2 a, f32 len) INLINE struct v2 v2_clamp_len(struct v2 a, f32 max) { f32 l_sq = a.x * a.x + a.y * a.y; - if (l_sq > (max * max) && l_sq != 0) { + if (l_sq > max * max) { f32 denom = max / math_sqrt(l_sq); a.x *= denom; a.y *= denom; diff --git a/src/user.c b/src/user.c index 875fcbbe..ff27f412 100644 --- a/src/user.c +++ b/src/user.c @@ -1035,7 +1035,7 @@ INTERNAL void user_update(void) /* Draw collision */ #if 1 if (entity_has_prop(ent, ENTITY_PROP_CONTACT_CONSTRAINT)) { - struct contact_constraint *data = &ent->contact_constraint; + struct contact_constraint *data = &ent->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; @@ -1178,8 +1178,8 @@ INTERNAL void user_update(void) /* Draw contacts */ { f32 radius = 5; - for (u32 i = 0; i < ent->contact_constraint.num_points; ++i) { - struct contact_point point = ent->contact_constraint.points[i]; + for (u32 i = 0; i < ent->contact_constraint_data.num_points; ++i) { + struct contact_point point = ent->contact_constraint_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); @@ -1202,7 +1202,7 @@ INTERNAL void user_update(void) f32 arrow_thickness = 2; f32 arrow_height = 5; struct v2 start = xform_mul_v2(G.world_view, dbg_pt); - struct v2 end = xform_mul_v2(G.world_view, v2_add(dbg_pt, v2_mul(v2_norm(ent->contact_constraint.normal), len))); + struct v2 end = xform_mul_v2(G.world_view, v2_add(dbg_pt, v2_mul(v2_norm(ent->contact_constraint_data.normal), len))); draw_solid_arrow_line(G.viewport_canvas, start, end, arrow_thickness, arrow_height, color); } #if 0 @@ -1263,7 +1263,7 @@ INTERNAL void user_update(void) u32 color_line = RGBA_32_F(1, 0, 1, 0.5); u32 color_a = RGBA_32_F(1, 0, 0, 0.5); u32 color_b = RGBA_32_F(0, 1, 0, 0.5); - struct collider_collision_points_result res = ent->contact_constraint.res; + struct collider_collision_points_result res = ent->contact_constraint_data.res; { struct v2 a = xform_mul_v2(G.world_view, res.a0); struct v2 b = xform_mul_v2(G.world_view, res.b0); @@ -1397,7 +1397,7 @@ INTERNAL void user_update(void) }); } -#if RTC +#if COLLIDER_DEBUG /* Gjk steps */ { i64 new_steps = collider_debug_steps; @@ -1478,7 +1478,7 @@ INTERNAL void user_update(void) draw_text(G.viewport_canvas, font, pos, string_format(temp.arena, STR("player pos: (%F, %F)"), FMT_FLOAT_P((f64)plaeyr_pos.x, 12), FMT_FLOAT_P((f64)plaeyr_pos.y, 12))); pos.y += spacing; -#if RTC +#if COLLIDER_DEBUG draw_text(G.viewport_canvas, font, pos, string_format(temp.arena, STR("collider gjk steps: %F"), FMT_UINT(collider_debug_steps))); pos.y += spacing; #endif