P_Ctx P = Zi; ThreadLocal P_ThreadLocalCtx P_tl = Zi; Readonly P_Ent P_NilEnt = { .xf = CompXformIdentity, .control.look = { 0, -1 }, }; Readonly P_Frame P_NilFrame = { .first_ent = &P_NilEnt, .last_ent = &P_NilEnt, }; //////////////////////////////////////////////////////////// //~ Bootstrap void P_Bootstrap(void) { P.s2v.arena = AcquireArena(Gibi(64)); } //////////////////////////////////////////////////////////// //~ Nil helpers b32 P_IsKeyNil(P_Key key) { return key.v == 0; } b32 P_IsEntNil(P_Ent *ent) { return ent == 0 || ent == &P_NilEnt; } b32 P_IsFrameNil(P_Frame *frame) { return frame == 0 || frame == &P_NilFrame; } //////////////////////////////////////////////////////////// //~ Key helpers b32 P_MatchKey(P_Key a, P_Key b) { return a.v == b.v; } P_Key P_RandKey(void) { // TODO: Don't use true randomness for entity keys. It's overkill & non-deterministic. P_Key result = Zi; TrueRand(StringFromStruct(&result)); return result; } u64 P_RandU64FromEnt(P_Ent *ent) { u64 result = MixU64s(ent->key.v, ent->rand_seq); if (!P_IsEntNil(ent)) { ent->rand_seq += 1; } return result; } //////////////////////////////////////////////////////////// //~ String helpers String P_StringFromEnt(P_Ent *ent) { String result = Zi; result.len = ent->string_len; result.text = ent->string_text; return result; } void P_SetEntString(P_Ent *ent, String str) { i64 len = MinI64(countof(ent->string_text), str.len); CopyBytes(ent->string_text, str.text, len); ent->string_len = len; } //////////////////////////////////////////////////////////// //~ Tile helpers String P_NameFromTileKind(P_TileKind kind) { // Tile names array #define X(name, ...) [P_TileKind_##name] = CompLit(#name), PERSIST Readonly String tile_names[] = { P_TilesXMacro(X) }; #undef X String result = Lit("Unknown"); if (kind >= 0 && kind < countof(tile_names)) { result = tile_names[kind]; } return result; } //////////////////////////////////////////////////////////// //~ Shape helpers P_Shape P_ShapeFromDescEx(P_ShapeDesc desc) { desc.count = MaxI32(desc.count, 1); P_Shape result = Zi; { result.points_count = desc.count; CopyStructs(result.points, desc.points, result.points_count); Vec2 accum = Zi; for (i32 p_idx = 0; p_idx < result.points_count; ++p_idx) { accum = AddVec2(accum, result.points[p_idx]); } result.centroid = DivVec2(accum, result.points_count); result.center_of_mass = result.centroid; result.radius = desc.radius; result.mass = desc.mass; } return result; } P_Shape P_MulXformShape(Xform xf, P_Shape shape) { P_Shape result = shape; for (i32 i = 0; i < shape.points_count; ++i) { result.points[i] = MulXformV2(xf, shape.points[i]); } Vec2 scale = ScaleFromXform(xf); result.radius *= MaxF32(scale.x, scale.y); result.centroid = MulXformV2(xf, shape.centroid); result.center_of_mass = MulXformV2(xf, shape.center_of_mass); return result; } Rng2 P_BoundingBoxFromShape(P_Shape shape) { Vec2 left = P_SupportPointFromShape(shape, VEC2(-1, 0)).p; Vec2 top = P_SupportPointFromShape(shape, VEC2(0, -1)).p; Vec2 right = P_SupportPointFromShape(shape, VEC2(1, 0)).p; Vec2 bottom = P_SupportPointFromShape(shape, VEC2(0, 1)).p; Rng2 result = Zi; result.p0 = VEC2(left.x, top.y); result.p1 = VEC2(right.x, bottom.y); return result; } P_Shape P_LocalShapeFromEnt(P_Ent *ent) { P_Shape result = Zi; // TODO: This is a temporary hack. We should eventually switch to using a prefab lookup table. if (ent->is_guy) { result = P_ShapeFromDesc( .mass = 10, .count = 1, .radius = 0.3, ); // f32 guy_width = 0.6; // f32 guy_height = 0.3; // result = P_ShapeFromDesc( // .mass = 10, // .count = 2, // .points = { VEC2(-guy_width / 2 + (guy_height / 2), 0), VEC2(guy_width / 2 - (guy_height / 2), 0) }, // .radius = guy_height / 2, // ); // Rng2 test_rect = Zi; // test_rect.p0 = VEC2(-1, -1); // test_rect.p1 = VEC2(1, 1); // result = P_ShapeFromDesc( // // .radius = 0.5, // .radius = 0, // .count = 4, // .points[0] = VEC2(test_rect.p0.x, test_rect.p0.y), // .points[1] = VEC2(test_rect.p1.x, test_rect.p0.y), // .points[2] = VEC2(test_rect.p1.x, test_rect.p1.y), // .points[3] = VEC2(test_rect.p0.x, test_rect.p1.y), // ); } return result; } P_Shape P_WorldShapeFromEnt(P_Ent *ent) { P_Shape local = P_LocalShapeFromEnt(ent); P_Shape world = P_MulXformShape(ent->xf, local); return world; } //////////////////////////////////////////////////////////// //~ Collision // NOTE: Everything here is pretty much copied directly from the old prototype. // The techniques are slow and do more than what we need. For example we should // probably just switch from GJK to SAT for shape collision testing. P_SupportPoint P_SupportPointFromShapeEx(P_Shape shape, Vec2 dir, i32 ignore_idx) { P_SupportPoint result = Zi; Vec2 dir_norm = NormVec2(dir); f32 max_dot = -Inf; if (shape.points_count == 1) { // Don't ignore for single-point colliders ignore_idx = -1; } for (i32 point_idx = 0; point_idx < shape.points_count; ++point_idx) { if (point_idx != ignore_idx) { Vec2 p = shape.points[point_idx]; f32 dot = DotVec2(p, dir_norm); if (dot > max_dot) { max_dot = dot; result.p = p; result.id = point_idx; } } } result.p = AddVec2(result.p, MulVec2(dir_norm, shape.radius)); return result; } P_SupportPoint P_SupportPointFromShape(P_Shape shape, Vec2 dir) { return P_SupportPointFromShapeEx(shape, dir, -1); } P_MenkowskiPoint P_MenkowskiPointFromShapes(P_Shape shape0, P_Shape shape1, Vec2 dir) { P_MenkowskiPoint result = Zi; result.s0 = P_SupportPointFromShape(shape0, dir); result.s1 = P_SupportPointFromShape(shape1, NegVec2(dir)); result.p = SubVec2(result.s0.p, result.s1.p); return result; } P_ClippedLine P_ClipLineToLine(Vec2 a0, Vec2 b0, Vec2 a1, Vec2 b1, Vec2 normal) { Vec2 vab0 = SubVec2(b0, a0); Vec2 vab1 = SubVec2(b1, a1); Vec2 va0a1 = SubVec2(a1, a0); Vec2 vb0b1 = SubVec2(b1, b0); f32 vab0_w = WedgeVec2(vab0, normal); f32 vab1_w = WedgeVec2(vab1, normal); f32 va0a1_w = WedgeVec2(va0a1, normal); f32 vb0b1_w = WedgeVec2(vb0b1, normal); // FIXME: Handle 0 denominator f32 a0t; f32 b0t; { f32 w = 1 / vab0_w; a0t = ClampF32(va0a1_w * w, 0, 1); b0t = ClampF32(vb0b1_w * -w, 0, 1); } f32 a1t; f32 b1t; { f32 w = 1 / vab1_w; a1t = ClampF32(-va0a1_w * w, 0, 1); b1t = ClampF32(-vb0b1_w * -w, 0, 1); } P_ClippedLine result = Zi; result.a0_clipped = AddVec2(a0, MulVec2(vab0, a0t)); result.a1_clipped = AddVec2(a1, MulVec2(vab1, a1t)); result.b0_clipped = AddVec2(b0, MulVec2(vab0, -b0t)); result.b1_clipped = AddVec2(b1, MulVec2(vab1, -b1t)); return result; } Vec2 P_ClipPointToLine(Vec2 a, Vec2 b, Vec2 p, Vec2 normal) { Vec2 vab = SubVec2(b, a); Vec2 vap = SubVec2(p, a); f32 vab_w = WedgeVec2(vab, normal); f32 vap_w = WedgeVec2(vap, normal); f32 w = 1 / vab_w; f32 t = ClampF32(vap_w * w, 0, 1); Vec2 result = AddVec2(a, MulVec2(vab, t)); return result; } P_CollisionResult P_CollisionResultFromShapes(P_Shape shape0, P_Shape shape1) { P_CollisionResult result = Zi; TempArena scratch = BeginScratchNoConflict(); f32 tolerance = 0.00005f; // How close can non-overlapping shapes be before collision is considered f32 min_unique_pt_dist_sq = (0.001f * 0.001f); // NOTE: Should always be less than tolerance, since colliding = 1 if origin is within this distance. u32 max_iterations = 64; // To prevent extremely large prototypes when origin is in exact center of rounded feature ////////////////////////////// //- GJK P_MenkowskiSimplex simplex = Zi; Vec2 non_overlapping_dir = Zi; b32 is_overlapping = 0; { P_MenkowskiPoint m = Zi; // First point is support point in shape's general directions to eachother Vec2 dir = SubVec2(shape1.centroid, shape0.centroid); if (IsVec2Zero(dir)) dir = VEC2(1, 0); simplex.a = P_MenkowskiPointFromShapes(shape0, shape1, dir); simplex.count = 1; Vec2 removed_a = Zi; Vec2 removed_b = Zi; u32 num_removed = 0; for (;;) { ////////////////////////////// //- Find initial points in simplex if (simplex.count == 1) { // Second point is support point towards origin dir = NegVec2(simplex.a.p); m = P_MenkowskiPointFromShapes(shape0, shape1, dir); // Check that new point is far enough away from existing point if (Vec2LenSq(SubVec2(m.p, simplex.a.p)) < min_unique_pt_dist_sq) { is_overlapping = 0; break; } simplex.b = simplex.a; simplex.a = m; simplex.count = 2; // Third point is support point in direction of line normal towards origin dir = PerpVec2TowardsDir(SubVec2(simplex.b.p, simplex.a.p), NegVec2(simplex.a.p)); } ////////////////////////////// //- Find third point in simplex { m = P_MenkowskiPointFromShapes(shape0, shape1, dir); // Check that new point is far enough away from existing points if ( Vec2LenSq(SubVec2(m.p, simplex.a.p)) < min_unique_pt_dist_sq || Vec2LenSq(SubVec2(m.p, simplex.b.p)) < min_unique_pt_dist_sq || ( (num_removed >= 1) && ( (Vec2LenSq(SubVec2(m.p, removed_a)) < min_unique_pt_dist_sq) || (num_removed >= 2 && Vec2LenSq(SubVec2(m.p, removed_b)) < min_unique_pt_dist_sq) ) ) || AbsF32(WedgeVec2(SubVec2(simplex.b.p, simplex.a.p), SubVec2(m.p, simplex.a.p))) < min_unique_pt_dist_sq ) { is_overlapping = 0; break; } simplex.c = simplex.b; simplex.b = simplex.a; simplex.a = m; simplex.count = 3; if ( (AbsF32(WedgeVec2(SubVec2(simplex.b.p, simplex.a.p), NegVec2(simplex.a.p))) <= min_unique_pt_dist_sq) || (AbsF32(WedgeVec2(SubVec2(simplex.c.p, simplex.b.p), NegVec2(simplex.b.p))) <= min_unique_pt_dist_sq) || (AbsF32(WedgeVec2(SubVec2(simplex.c.p, simplex.a.p), NegVec2(simplex.a.p))) <= min_unique_pt_dist_sq) ) { // Simplex lies on origin is_overlapping = 1; break; } } ////////////////////////////// //- Determine origin region Vec2 vab = SubVec2(simplex.b.p, simplex.a.p); Vec2 vac = SubVec2(simplex.c.p, simplex.a.p); Vec2 vbc = SubVec2(simplex.c.p, simplex.b.p); Vec2 rab_dir = PerpVec2TowardsDir(vab, NegVec2(vac)); Vec2 rac_dir = PerpVec2TowardsDir(vac, NegVec2(vab)); Vec2 rbc_dir = PerpVec2TowardsDir(vbc, vab); f32 rab_dot = DotVec2(rab_dir, NegVec2(simplex.a.p)); f32 rac_dot = DotVec2(rac_dir, NegVec2(simplex.a.p)); f32 rbc_dot = DotVec2(rbc_dir, NegVec2(simplex.b.p)); f32 vab_dot = DotVec2(vab, NegVec2(simplex.a.p)) / Vec2LenSq(vab); f32 vac_dot = DotVec2(vac, NegVec2(simplex.a.p)) / Vec2LenSq(vac); f32 vbc_dot = DotVec2(vbc, NegVec2(simplex.b.p)) / Vec2LenSq(vbc); if (rab_dot >= 0 && vab_dot >= 0 && vab_dot <= 1) { // Region ab, remove c num_removed = 1; removed_a = simplex.c.p; simplex.count = 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 = simplex.b.p; simplex.count = 2; simplex.b = simplex.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 = simplex.a.p; simplex.count = 2; simplex.a = simplex.b; simplex.b = simplex.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 = simplex.b.p; removed_b = simplex.c.p; simplex.count = 1; } else if (vab_dot >= 1 && vbc_dot <= 0) { // Region b, remove ac num_removed = 2; removed_a = simplex.a.p; removed_b = simplex.c.p; simplex.count = 1; simplex.a = simplex.b; } else if (vac_dot >= 1 && vbc_dot >= 1) { // Region c, remove ab num_removed = 2; removed_a = simplex.a.p; removed_b = simplex.b.p; simplex.count = 1; simplex.a = simplex.c; } else { // No region, must be in simplex is_overlapping = 1; break; } } if (!is_overlapping) { non_overlapping_dir = dir; } } ////////////////////////////// //- EPA // Find dir from origin to closest edge Vec2 normal = Zi; P_MenkowskiSimplex closest_feature = Zi; { P_MenkowskiPoint *proto = 0; if (is_overlapping) { u32 proto_count = 0; proto = ArenaNext(scratch.arena, P_MenkowskiPoint); { Assert(simplex.count == 3); P_MenkowskiPoint *tmp = PushStructsNoZero(scratch.arena, P_MenkowskiPoint, 3); tmp[0] = simplex.a; tmp[1] = simplex.b; tmp[2] = simplex.c; proto_count = 3; } i32 winding = WindingFromVec2(SubVec2(simplex.c.p, simplex.a.p), SubVec2(simplex.b.p, simplex.a.p)); u32 epa_iterations = 0; for (;;) { ++epa_iterations; // FIXME: Winding order of ps & pe index P_MenkowskiPoint closest_a = Zi; P_MenkowskiPoint closest_b = Zi; u32 closest_b_index = 0; { // Find edge segment on prototype closest to the origin f32 closest_len_sq = Inf; for (u32 i = 0; i < proto_count; ++i) { u32 a_index = i; u32 b_index = (i < proto_count - 1) ? (i + 1) : 0; P_MenkowskiPoint a = proto[a_index]; P_MenkowskiPoint b = proto[b_index]; Vec2 vab = SubVec2(b.p, a.p); Vec2 vao = NegVec2(a.p); f32 proj_ratio = ClampF32(DotVec2(vao, vab) / Vec2LenSq(vab), 0, 1); Vec2 proj = AddVec2(a.p, MulVec2(vab, proj_ratio)); f32 proj_len_sq = Vec2LenSq(proj); if (proj_len_sq < closest_len_sq - min_unique_pt_dist_sq) { closest_a = a; closest_b = b; closest_b_index = b_index; closest_len_sq = proj_len_sq; } } } Vec2 vab = SubVec2(closest_b.p, closest_a.p); // Find new point in dir Vec2 dir = MulVec2(PerpVec2(vab), winding); P_MenkowskiPoint m = P_MenkowskiPointFromShapes(shape0, shape1, dir); // Check validity of new point { b32 valid = 1; { // NOTE: Changing this value affects how stable normals are for rounded colliders //const f32 validity_epsilon = min_unique_pt_dist_sq; // Arbitrary //const f32 validity_epsilon = 0.00000000001f; // Arbitrary const f32 validity_epsilon = min_unique_pt_dist_sq; // Arbitrary Vec2 vam = SubVec2(m.p, closest_a.p); Vec2 vbm = SubVec2(closest_b.p, closest_a.p); f32 dot = DotVec2(vab, vam) / Vec2LenSq(vab); if (dot >= -validity_epsilon && dot <= 1 - validity_epsilon && (WedgeVec2(vab, vam) * -winding) >= -validity_epsilon) { // New point is not between edge valid = 0; } else if (Vec2LenSq(vam) < min_unique_pt_dist_sq || Vec2LenSq(vbm) < min_unique_pt_dist_sq) { // New point is too close to existing valid = 0; } } if (!valid || epa_iterations >= max_iterations) { normal = NormVec2(dir); closest_feature.a = closest_a; closest_feature.b = closest_b; closest_feature.count = 2; break; } } // Expand prototype PushStructNoZero(scratch.arena, P_MenkowskiPoint); ++proto_count; // Shift points in prototype to make room for (u32 i = proto_count - 1; i > closest_b_index; --i) { u32 shift_from = (i > 0) ? i - 1 : proto_count - 1; u32 shift_to = i; proto[shift_to] = proto[shift_from]; } // Insert new point into prototype proto[closest_b_index] = m; } // Debug draw // { // P_DebugDrawPoint(simplex.a.p, VEC4(1, 0, 0, 0.5)); // P_DebugDrawPoint(simplex.b.p, VEC4(0, 1, 0, 0.5)); // P_DebugDrawPoint(simplex.c.p, VEC4(0, 0, 1, 0.5)); // P_DebugDrawLine(simplex.a.p, simplex.b.p, Color_Yellow); // P_DebugDrawLine(simplex.b.p, simplex.c.p, Color_Yellow); // P_DebugDrawLine(simplex.c.p, simplex.a.p, Color_Yellow); // if (proto_count > 0) // { // for (i64 i = 0; i < proto_count; ++i) // { // i64 p1_idx = i + 1; // if (p1_idx == proto_count) // { // p1_idx = 0; // } // Vec2 p0 = proto[i].p; // Vec2 p1 = proto[p1_idx].p; // P_DebugDrawLine(p0, p1, VEC4(0, 1, 0, 0.5)); // } // } // } } else { normal = NormVec2(non_overlapping_dir); closest_feature.count = simplex.count; closest_feature.a = simplex.a; closest_feature.b = simplex.b; } } ////////////////////////////// //- Determine collision b32 is_colliding = 0; { if (is_overlapping) { is_colliding = 1; } else { // Shapes not overlapping, determine if distance between shapes within tolerance if (closest_feature.count == 1) { Vec2 p = NegVec2(closest_feature.a.p); if (Vec2LenSq(p) <= (tolerance * tolerance)) { is_colliding = 1; } } else { // Project origin to determine if distance is within tolerance. Assert(closest_feature.count == 2); Vec2 vab = SubVec2(closest_feature.b.p, closest_feature.a.p); Vec2 vao = NegVec2(closest_feature.a.p); f32 ratio = ClampF32(DotVec2(vab, vao) / DotVec2(vab, vab), 0, 1); Vec2 p = AddVec2(closest_feature.a.p, MulVec2(vab, ratio)); if (Vec2LenSq(p) <= (tolerance * tolerance)) { is_colliding = 1; } } } } ////////////////////////////// //- Compute collision points // Clip to determine final points i32 collision_points_count = 0; P_CollisionPoint collision_points[2] = Zi; if (is_colliding) { // Max vertices must be < 16 to fit in 4 bit ids StaticAssert(countof(shape0.points) <= 16); { b32 collapse0 = 0; b32 collapse1 = 0; P_SupportPoint a0 = closest_feature.a.s0; P_SupportPoint a1 = closest_feature.a.s1; P_SupportPoint b0 = closest_feature.b.s0; P_SupportPoint b1 = closest_feature.b.s1; // FIXME: Manually account for shapes w/ 1 & 2 points if (closest_feature.count == 2) { if (a0.id == b0.id) { if (shape0.points_count > 1) { b0 = P_SupportPointFromShapeEx(shape0, normal, b0.id); } else { collapse0 = 1; b0 = a0; } } if (a1.id == b1.id) { if (shape1.points_count > 1) { b1 = P_SupportPointFromShapeEx(shape1, NegVec2(normal), b1.id); } else { collapse1 = 1; b1 = a1; } } } else { collapse0 = 1; collapse1 = 1; b0 = a0; b1 = a1; } Vec2 vab0 = SubVec2(b0.p, a0.p); Vec2 vab1 = SubVec2(b1.p, a1.p); Vec2 vab0_norm = NormVec2(vab0); Vec2 vab1_norm = NormVec2(vab1); // Swap points based on normal direction for consistent clipping if (WedgeVec2(normal, vab0) < 0) { P_SupportPoint tmp = a0; a0 = b0; b0 = tmp; vab0 = NegVec2(vab0); } if (WedgeVec2(normal, vab1) < 0) { P_SupportPoint tmp = a1; a1 = b1; b1 = tmp; vab1 = NegVec2(vab1); } // Collapse lines that are too far in the direction of the normal to be accurately clipped f32 collapse_epsilon = 0.05f; collapse0 = collapse0 || AbsF32(WedgeVec2(normal, vab0_norm)) < collapse_epsilon; collapse1 = collapse1 || AbsF32(WedgeVec2(normal, vab1_norm)) < collapse_epsilon; // Collapse lines into deepest point if (collapse0) { if (DotVec2(normal, vab0) > 0) { a0 = b0; } else { // TODO: Remove this (debugging) b0 = a0; } } if (collapse1) { if (DotVec2(normal, vab1) < 0) { a1 = b1; } else { // TODO: Remove this (debugging) b1 = a1; } } f32 a_sep = Inf; f32 b_sep = Inf; Vec2 a_midpoint = Zi; Vec2 b_midpoint = Zi; b32 ignore_a = 1; b32 ignore_b = 1; if (!collapse0 && !collapse1) { // Clip line to line P_ClippedLine clip_result = P_ClipLineToLine(a0.p, b0.p, a1.p, b1.p, normal); Vec2 a0_clipped = clip_result.a0_clipped; Vec2 a1_clipped = clip_result.a1_clipped; Vec2 b0_clipped = clip_result.b0_clipped; Vec2 b1_clipped = clip_result.b1_clipped; // Calc midpoint between clipped a & b Vec2 va0a1_clipped = SubVec2(a1_clipped, a0_clipped); Vec2 vb0b1_clipped = SubVec2(b1_clipped, b0_clipped); a_sep = DotVec2(va0a1_clipped, normal); b_sep = DotVec2(vb0b1_clipped, normal); a_midpoint = AddVec2(a0_clipped, MulVec2(va0a1_clipped, 0.5f)); b_midpoint = AddVec2(b0_clipped, MulVec2(vb0b1_clipped, 0.5f)); ignore_a = 0; ignore_b = 0; Vec2 vfin = SubVec2(b_midpoint, a_midpoint); if (Vec2LenSq(vfin) < (0.005 * 0.005)) { if (a_sep > b_sep) { ignore_a = 1; } else { ignore_b = 1; } } } else { Vec2 p0 = a0.p; Vec2 p1 = a1.p; // TODO: Choose ID based on closest clipped point if (collapse1 && !collapse0) { // Project a1 onto vab0 p0 = P_ClipPointToLine(a0.p, b0.p, a1.p, normal); } if (collapse0 && !collapse1) { // Project a0 onto vab1 p1 = P_ClipPointToLine(a1.p, b1.p, a0.p, normal); } // Calc midpoint Vec2 vsep = SubVec2(p1, p0); a_midpoint = AddVec2(p0, MulVec2(vsep, 0.5f)); a_sep = DotVec2(normal, p1) - DotVec2(normal, p0); ignore_a = 0; } // Insert points if (!ignore_a && a_sep < tolerance) { P_CollisionPoint *point = &collision_points[collision_points_count++]; point->id = a0.id | (a1.id << 4); point->separation = a_sep; point->p = a_midpoint; } if (!ignore_b && b_sep < tolerance) { P_CollisionPoint *point = &collision_points[collision_points_count++]; point->id = b0.id | (b1.id << 4); point->separation = b_sep; point->p = b_midpoint; } } } ////////////////////////////// //- Compute closest points Vec2 closest_p0 = Zi; Vec2 closest_p1 = Zi; if (closest_feature.count == 1) { closest_p0 = closest_feature.a.s0.p; closest_p1 = closest_feature.a.s1.p; } else { Assert(closest_feature.count == 2); // FIXME: Winding order dependent? f32 ratio = 0; { // Determine ratio between edge a & b that projected origin lies Vec2 vab = SubVec2(closest_feature.b.p, closest_feature.a.p); Vec2 vao = NegVec2(closest_feature.a.p); ratio = ClampF32(DotVec2(vab, vao) / DotVec2(vab, vab), 0, 1); } // Shape 0 closest_p0 = SubVec2(closest_feature.b.s0.p, closest_feature.a.s0.p); closest_p0 = MulVec2(closest_p0, ratio); closest_p0 = AddVec2(closest_p0, closest_feature.a.s0.p); // Shape 1 closest_p1 = SubVec2(closest_feature.b.s1.p, closest_feature.a.s1.p); closest_p1 = MulVec2(closest_p1, ratio); closest_p1 = AddVec2(closest_p1, closest_feature.a.s1.p); } CopyStructs(result.collision_points, collision_points, countof(collision_points)); result.collision_points_count = collision_points_count; result.collision_normal = normal; result.closest_p0 = closest_p0; result.closest_p1 = closest_p1; EndScratch(scratch); return result; } P_RaycastResult P_RaycastShape(P_Shape shape, Vec2 ray_start, Vec2 ray_dir) { f32 radius = shape.radius; f32 ray_len = Vec2Len(ray_dir); Vec2 ray_dir_norm = DivVec2(ray_dir, ray_len); Vec2 s = ray_start; Vec2 e = AddVec2(ray_start, ray_dir_norm); Vec2 vse = ray_dir_norm; Vec2 isect = Zi; Vec2 isect_normal = Zi; b32 isect_is_round = 0; b32 isect_found = 0; if (shape.points_count > 1) { // Find expanded line intersections with ray for (i32 p_idx = 0; p_idx < shape.points_count && !isect_found; ++p_idx) { Vec2 a = Zi; Vec2 b = Zi; Vec2 vab = Zi; Vec2 normal = Zi; { i32 a_idx = p_idx; i32 b_idx = a_idx + 1; if (b_idx >= shape.points_count) { b_idx = 0; } Vec2 a_orig = shape.points[a_idx]; Vec2 b_orig = shape.points[b_idx]; vab = SubVec2(b_orig, a_orig); normal = NegVec2(PerpVec2(NormVec2(vab))); Vec2 radius_add = MulVec2(normal, radius); a = AddVec2(a_orig, radius_add); b = AddVec2(b_orig, radius_add); } Vec2 vsa = SubVec2(a, s); Vec2 vsb = SubVec2(b, s); f32 wa = WedgeVec2(vse, vsa); f32 wb = WedgeVec2(vse, vsb); if (wa > 0 && wb < 0) { f32 t = -wa / (wb - wa); isect = AddVec2(a, MulVec2(vab, t)); isect_normal = normal; isect_found = 1; } } // Find closest rounded corner if (!isect_found && radius != 0) { isect_is_round = 1; for (i32 f_idx = 0; f_idx < shape.points_count && !isect_found; ++f_idx) { Vec2 f_orig = shape.points[f_idx]; Vec2 a = Zi; Vec2 b = Zi; Vec2 vab = Zi; { i32 prev_idx = f_idx - 1; i32 next_idx = f_idx + 1; if (prev_idx < 0) { prev_idx = shape.points_count - 1; } if (next_idx >= shape.points_count) { next_idx = 0; } Vec2 prev_orig = shape.points[prev_idx]; Vec2 next_orig = shape.points[next_idx]; Vec2 vpf = SubVec2(f_orig, prev_orig); Vec2 vfn = SubVec2(next_orig, f_orig); Vec2 vpf_norm = NormVec2(vpf); Vec2 vfn_norm = NormVec2(vfn); Vec2 radius_add_a = MulVec2(PerpVec2(vpf_norm), -radius); Vec2 radius_add_b = MulVec2(PerpVec2(vfn_norm), -radius); a = AddVec2(f_orig, radius_add_a); b = AddVec2(f_orig, radius_add_b); } Vec2 vsa = SubVec2(a, s); Vec2 vsb = SubVec2(b, s); f32 wa = WedgeVec2(vse, vsa); f32 wb = WedgeVec2(vse, vsb); if (wa > 0 && wb < 0) { isect = f_orig; isect_found = 1; } } } // Find closest corner if (!isect_found) { f32 min_dist = Inf; for (i32 p_idx = 0; p_idx < shape.points_count && !isect_found; ++p_idx) { Vec2 p = shape.points[p_idx]; f32 dist = AbsF32(WedgeVec2(vse, SubVec2(p, s))); if (dist < min_dist) { isect = p; min_dist = dist; } } } } else if (shape.points_count == 1 && radius != 0) { isect = shape.points[0]; isect_is_round = 1; } // Find round intersection b32 is_intersecting = 0; if (isect_is_round || !isect_found) { Vec2 vsi = SubVec2(isect, s); f32 dot = DotVec2(vse, vsi); f32 wedge = WedgeVec2(vse, vsi); is_intersecting = AbsF32(wedge) < radius; if (is_intersecting) { f32 diff = SqrtF32(radius * radius - wedge * wedge); f32 entrance_t = dot - diff; f32 exit_t = dot + diff; { Vec2 old_isect = isect; isect = AddVec2(s, MulVec2(vse, entrance_t)); isect_normal = NormVec2(SubVec2(isect, old_isect)); } } } else { is_intersecting = isect_found; } P_RaycastResult result = Zi; result.is_intersecting = is_intersecting; if (is_intersecting) { result.p = isect; result.normal = isect_normal; } return result; } Vec2 P_EdgePointFromShape(P_Shape shape, Vec2 dir) { Vec2 result = shape.centroid; P_RaycastResult raycast = P_RaycastShape(shape, shape.centroid, NegVec2(dir)); if (raycast.is_intersecting) { result = raycast.p; } return result; } //////////////////////////////////////////////////////////// //~ Lookup helpers P_Ent *P_EntFromKey(P_Frame *frame, P_Key key) { P_Ent *result = &P_NilEnt; P_World *world = frame->world; if (!P_IsKeyNil(key) && frame->tick > 0 && frame->ents_count > 0 && frame->ent_bins_count > 0) { i64 tick = frame->tick; P_EntBin *bin = &frame->ent_bins[key.v % frame->ent_bins_count]; for (P_Ent *e = bin->first; e; e = e->next_in_bin) { if (e->key.v == key.v) { result = e; break; } } } return result; } //////////////////////////////////////////////////////////// //~ Iteration helpers P_Ent *P_FirstEnt(P_Frame *frame) { P_Ent *result = &P_NilEnt; if (!P_IsEntNil(frame->first_ent)) { result = frame->first_ent; } return result; } P_Ent *P_NextEnt(P_Ent *e) { P_Ent *result = &P_NilEnt; if (!P_IsEntNil(e) && !P_IsEntNil(e->next)) { result = e->next; } return result; } P_Ent *P_PushTempEnt(Arena *arena, P_EntList *list) { P_EntListNode *n = PushStruct(arena, P_EntListNode); SllQueuePush(list->first, list->last, n); ++list->count; P_Ent *ent = &n->ent; *ent = P_NilEnt; ent->exists = 1; return ent; } //////////////////////////////////////////////////////////// //~ Debug draw void P_DebugDrawPoint(Vec2 p, Vec4 srgb) { if (P_tl.debug_draw_enabled) { P_DebugDrawNode *n = PushStruct(P_tl.debug_arena, P_DebugDrawNode); { n->kind = P_DebugDrawKind_Point; n->srgb32 = U32FromVec4(srgb); n->point.p = p; } SllQueuePush(P_tl.first_debug_draw_node, P_tl.last_debug_draw_node, n); P_tl.debug_draw_nodes_count += 1; } } void P_DebugDrawLine(Vec2 p0, Vec2 p1, Vec4 srgb) { if (P_tl.debug_draw_enabled) { P_DebugDrawNode *n = PushStruct(P_tl.debug_arena, P_DebugDrawNode); { n->kind = P_DebugDrawKind_Line; n->srgb32 = U32FromVec4(srgb); n->line.p0 = p0; n->line.p1 = p1; } SllQueuePush(P_tl.first_debug_draw_node, P_tl.last_debug_draw_node, n); P_tl.debug_draw_nodes_count += 1; } } void P_DebugDrawRect(Rng2 rect, Vec4 srgb) { if (P_tl.debug_draw_enabled) { P_DebugDrawNode *n = PushStruct(P_tl.debug_arena, P_DebugDrawNode); { n->kind = P_DebugDrawKind_Rect; n->srgb32 = U32FromVec4(srgb); n->rect = rect; } SllQueuePush(P_tl.first_debug_draw_node, P_tl.last_debug_draw_node, n); P_tl.debug_draw_nodes_count += 1; } } void P_DebugDrawShape(P_Shape shape, Vec4 srgb) { if (P_tl.debug_draw_enabled) { P_DebugDrawNode *n = PushStruct(P_tl.debug_arena, P_DebugDrawNode); { n->kind = P_DebugDrawKind_Shape; n->srgb32 = U32FromVec4(srgb); n->shape = shape; } SllQueuePush(P_tl.first_debug_draw_node, P_tl.last_debug_draw_node, n); P_tl.debug_draw_nodes_count += 1; } } void P_DebugDrawFrame(P_Frame *frame, Vec4 tint) { if (P_tl.debug_draw_enabled) { for (P_Ent *ent = P_FirstEnt(frame); !P_IsEntNil(ent); ent = P_NextEnt(ent)) { P_Shape world_shape = P_WorldShapeFromEnt(ent); // Draw aabb { Vec4 color = VEC4(0.4, 0.2, 0.2, 1); Rng2 bb = P_BoundingBoxFromShape(world_shape); P_DebugDrawRect(bb, color); } // Draw shape { // Vec4 color = Color_Cyan; // Vec4 color = VEC4(0.2, 0.4, 0.2, 1); Vec4 color = MulVec4Vec4(VEC4(1, 1, 1, 1), tint); P_DebugDrawShape(world_shape, color); } // Draw rot { Vec4 color = VEC4(0.8, 0.8, 0.8, 1); Vec2 p0 = world_shape.centroid; Vec2 p1 = P_EdgePointFromShape(world_shape, UpFromXform(ent->xf)); P_DebugDrawLine(p0, p1, color); } // Draw look { Vec4 color = VEC4(0.4, 0.8, 0.4, 1); Vec2 p0 = world_shape.centroid; Vec2 p1 = P_EdgePointFromShape(world_shape, ent->control.look); P_DebugDrawLine(p0, p1, color); } } } } //////////////////////////////////////////////////////////// //~ Msg P_Msg *P_PushMsg(P_MsgKind kind, String data) { P_MsgNode *msg_node = PushStruct(P_tl.out_msgs_arena, P_MsgNode); P_Msg *msg = &msg_node->msg; msg->kind = kind; msg->data = PushString(P_tl.out_msgs_arena, data); DllQueuePush(P_tl.out_msgs.first, P_tl.out_msgs.last, msg_node); ++P_tl.out_msgs.count; return msg; } //////////////////////////////////////////////////////////// //~ World P_World *P_AcquireWorld(void) { P_World *world = 0; { Arena *arena = AcquireArena(Gibi(64)); world = PushStruct(arena, P_World); world->arena = arena; } world->frames_arena = AcquireArena(Gibi(64)); world->statics_arena = AcquireArena(Gibi(64)); world->first_frame = &P_NilFrame; world->last_frame = &P_NilFrame; world->frame_bins_count = Kibi(16); world->frame_bins = PushStructs(world->arena, P_FrameBin, world->frame_bins_count); // TODO world->tiles = PushStructs(world->arena, u8, P_TilesCount); TrueRand(StringFromStruct(&world->seed)); return world; } void P_SpawnEntsFromList(P_Frame *frame, P_EntList ents) { P_World *world = frame->world; for (P_EntListNode *n = ents.first; n; n = n->next) { P_Ent *src = &n->ent; P_Key key = src->key; if (!P_IsKeyNil(src->key)) { P_EntBin *bin = &frame->ent_bins[key.v % frame->ent_bins_count]; P_Ent *dst = bin->first; for (; dst; dst = dst->next_in_bin) { if (dst->key.v == key.v) { break; } } if (!dst) { dst = world->first_free_ent; if (dst) { SllStackPop(world->first_free_ent); } else { dst = PushStructNoZero(world->frames_arena, P_Ent); } DllQueuePushNPZ(&P_NilEnt, frame->first_ent, frame->last_ent, dst, next, prev); DllQueuePushNP(bin->first, bin->last, dst, next_in_bin, prev_in_bin); } P_Ent *old_next = dst->next; P_Ent *old_prev = dst->prev; P_Ent *old_next_in_bin = dst->next_in_bin; P_Ent *old_prev_in_bin = dst->prev_in_bin; { *dst = *src; } dst->next = old_next; dst->prev = old_prev; dst->next_in_bin = old_next_in_bin; dst->prev_in_bin = old_prev_in_bin; ++frame->ents_count; } } } P_Frame *P_FrameFromTick(P_World *world, i64 tick) { P_Frame *result = &P_NilFrame; if (world->frame_bins_count > 0) { u64 hash = MixU64(tick); P_FrameBin *bin = &world->frame_bins[hash % world->frame_bins_count]; for (P_Frame *frame = bin->first; frame; frame = frame->next_in_bin) { if (frame->tick == tick) { result = frame; break; } } } return result; } void P_ClearFrames(P_World *world, i64 tick_min, i64 tick_max) { // TODO: Fast path for when range encompasses all frames in the world // TODO: Don't need linear search P_Frame *frame = world->first_frame; while (!P_IsFrameNil(frame)) { P_Frame *next_frame = frame->next; if (frame->tick >= tick_min && frame->tick <= tick_max) { if (!P_IsEntNil(frame->first_ent)) { frame->last_ent->next = world->first_free_ent; world->first_free_ent = frame->first_ent; } u64 hash = MixU64(frame->tick); P_FrameBin *bin = &world->frame_bins[hash % world->frame_bins_count]; DllQueueRemoveNPZ(&P_NilFrame, world->first_frame, world->last_frame, frame, next, prev); DllQueueRemoveNPZ(0, bin->first, bin->last, frame, next_in_bin, prev_in_bin); SllStackPush(world->first_free_frame, frame); } else { break; } frame = next_frame; } } P_Frame *P_PushFrame(P_World *world, P_Frame *src_frame, i64 tick) { Assert(!(src_frame->world == world && tick <= src_frame->tick)); // Can't read from tick that is being overwritten by new tick P_ClearFrames(world, tick, I64Max); P_Frame *frame = world->first_free_frame; if (frame) { SllStackPop(world->first_free_frame); i64 old_ent_bins_count = frame->ent_bins_count; P_EntBin *old_ent_bins = frame->ent_bins; i64 old_max_constraints = frame->max_constraints; P_Constraint *old_constraints = frame->constraints; { ZeroStruct(frame); } frame->ent_bins_count = old_ent_bins_count; frame->ent_bins = old_ent_bins; frame->max_constraints = old_max_constraints; frame->constraints = old_constraints; ZeroStructs(frame->ent_bins, frame->ent_bins_count); } else { frame = PushStruct(world->frames_arena, P_Frame); } { frame->world = world; frame->tick = tick; frame->time_ns = src_frame->time_ns; frame->first_ent = &P_NilEnt; frame->last_ent = &P_NilEnt; if (!frame->ent_bins) { frame->ent_bins_count = Kibi(16); frame->ent_bins = PushStructs(world->frames_arena, P_EntBin, frame->ent_bins_count); } if (!frame->constraints) { frame->max_constraints = Kibi(4); frame->constraints_count = 0; frame->constraints = PushStructsNoZero(world->frames_arena, P_Constraint, frame->max_constraints); } u64 hash = MixU64(tick); P_FrameBin *bin = &world->frame_bins[hash % world->frame_bins_count]; DllQueuePushNPZ(&P_NilFrame, world->first_frame, world->last_frame, frame, next, prev); DllQueuePushNPZ(0, bin->first, bin->last, frame, next_in_bin, prev_in_bin); } // Copy ents for (P_Ent *src = P_FirstEnt(src_frame); !P_IsEntNil(src); src = P_NextEnt(src)) { P_Ent *dst = world->first_free_ent; if (dst) { SllStackPop(world->first_free_ent); } else { dst = PushStructNoZero(world->frames_arena, P_Ent); } *dst = *src; P_EntBin *bin = &frame->ent_bins[src->key.v % frame->ent_bins_count]; DllQueuePushNPZ(&P_NilEnt, frame->first_ent, frame->last_ent, dst, next, prev); DllQueuePushNP(bin->first, bin->last, dst, next_in_bin, prev_in_bin); ++frame->ents_count; } return frame; } //////////////////////////////////////////////////////////// //~ Step void P_StepFrame(P_Frame *frame) { TempArena scratch = BeginScratchNoConflict(); P_World *world = frame->world; P_Frame *prev_frame = frame->prev; i64 sim_dt_ns = NsFromSeconds(1) / SIM_TICKS_PER_SECOND; f64 sim_dt = SecondsFromNs(sim_dt_ns); ////////////////////////////// //- Update double-buffered entity data for (P_Ent *ent = P_FirstEnt(frame); !P_IsEntNil(ent); ent = P_NextEnt(ent)) { ent->prev_xf = ent->xf; } ////////////////////////////// //- Spawn entities // { // ////////////////////////////// // //- Push bullets // for (P_Ent *ent = P_FirstEnt(frame); !P_IsEntNil(ent); ent = P_NextEnt(ent)) // { // if (ent->control.fire_held) // { // if (ent->has_weapon) // { // } // } // } // } ////////////////////////////// //- Update guy controls from player controls for (P_Ent *guy = P_FirstEnt(frame); !P_IsEntNil(guy); guy = P_NextEnt(guy)) { if (guy->is_guy) { ZeroStruct(&guy->control); } } for (P_Ent *player = P_FirstEnt(frame); !P_IsEntNil(player); player = P_NextEnt(player)) { if (player->is_player) { P_Ent *guy = P_EntFromKey(frame, player->guy); if (!P_IsEntNil(guy)) { guy->control = player->control; } } } ////////////////////////////// //- Integrate guy control forces for (P_Ent *guy = P_FirstEnt(frame); !P_IsEntNil(guy); guy = P_NextEnt(guy)) { if (guy->is_guy) { P_Control control = guy->control; // Xform xf = guy->xf; // Xform desired_xf = xf; // if (!IsVec2Zero(control.look)) // { // desired_xf = XformWithWorldRotation(xf, AngleFromVec2(control.look)); // } // f32 move_speed = TweakFloat("Guy move speed", 6.5, 0, 20); // desired_xf.og = AddVec2(xf.og, MulVec2(control.move, move_speed * sim_dt)); // Vec2 pos_diff = SubVec2(desired_xf.og, xf.og); // f32 angle_diff = UnwindAngleF32(RotationFromXform(desired_xf) - RotationFromXform(xf)); // guy->solved_v = pos_diff; // guy->solved_w = angle_diff; { // f32 damp_vel = damp_force * sim_dt; if (Vec2Len(guy->solved_v) > 0.001) { f32 damp_force = TweakFloat("Guy damp force", 50, 0, 100); Vec2 damp = MulVec2(NegVec2(guy->solved_v), damp_force * sim_dt); guy->solved_v = AddVec2(guy->solved_v, damp); } else { guy->solved_v = VEC2(0, 0); } } { f32 move_force = TweakFloat("Guy move force", 400, 0, 400); f32 max_speed = TweakFloat("Guy max speed", 10, 0, 20); Vec2 new_velocity = guy->solved_v; new_velocity = AddVec2(new_velocity, MulVec2(control.move, move_force * sim_dt)); // if (Vec2Len(new_velocity) > max_speed) // { // new_velocity = Vec2WithLen(new_velocity, max_speed); // } guy->solved_v = new_velocity; } } } ////////////////////////////// //- Generate guy wall constraints // TODO: Not like this frame->constraints_count = MinI64(prev_frame->constraints_count, frame->max_constraints); CopyStructs(frame->constraints, prev_frame->constraints, frame->constraints_count); for (P_Ent *ent0 = P_FirstEnt(frame); !P_IsEntNil(ent0); ent0 = P_NextEnt(ent0)) { if (ent0->is_guy) { P_Shape shape0 = P_WorldShapeFromEnt(ent0); for (P_Ent *ent1 = P_FirstEnt(frame); !P_IsEntNil(ent1); ent1 = P_NextEnt(ent1)) { if (ent1->is_guy && ent1 > ent0) { P_Shape shape1 = P_WorldShapeFromEnt(ent1); // TODO: World query P_CollisionResult collision = P_CollisionResultFromShapes(shape0, shape1); if (collision.collision_points_count > 0) { // FIXME: Key lookup P_Constraint *constraint = 0; { b32 match = 0; for (i64 constraint_idx = 0; constraint_idx < frame->constraints_count; ++constraint_idx) { constraint = &frame->constraints[constraint_idx]; if (P_MatchKey(constraint->ent0, ent0->key) && P_MatchKey(constraint->ent1, ent1->key)) { match = 1; break; } } if (!match) { if (frame->constraints_count < frame->max_constraints) { constraint = &frame->constraints[frame->constraints_count]; frame->constraints_count += 1; ZeroStruct(constraint); } } } if (constraint) { constraint->last_touched_tick = frame->tick; constraint->normal = collision.collision_normal; // constraint->friction = SqrtF32(ent0->friction * ent1->friction); constraint->friction = 0; // TODO: Real masses f32 inv_m0 = 10; f32 inv_m1 = 10; f32 inv_i0 = 0; f32 inv_i1 = 0; constraint->ent0 = ent0->key; constraint->ent1 = ent1->key; // constraint->static_center1 = shape1.center_of_mass; 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 (i32 contact_point_idx = 0; contact_point_idx < constraint->points_count; ++contact_point_idx) { P_ContactPoint *contact = &constraint->points[contact_point_idx]; u32 id = contact->id; b32 match = 0; for (i32 collision_point_idx = 0; collision_point_idx < collision.collision_points_count; ++collision_point_idx) { if (collision.collision_points[collision_point_idx].id == id) { match = 1; break; } } if (!match) { // Delete contact by replacing with last in array *contact = constraint->points[constraint->points_count - 1]; constraint->points_count -= 1; contact_point_idx -= 1; } } // Create / update contacts from collision for (i32 collision_point_idx = 0; collision_point_idx < collision.collision_points_count; ++collision_point_idx) { P_CollisionPoint collision_point = collision.collision_points[collision_point_idx]; u32 id = collision_point.id; P_ContactPoint *contact = 0; { for (i32 contact_point_idx = 0; contact_point_idx < constraint->points_count; ++contact_point_idx) { P_ContactPoint *tmp = &constraint->points[contact_point_idx]; if (tmp->id == id) { contact = tmp; break; } } if (!contact) { contact = &constraint->points[constraint->points_count]; constraint->points_count += 1; ZeroStruct(contact); } } contact->id = id; Vec2 vcp0 = SubVec2(collision_point.p, shape0.center_of_mass); Vec2 vcp1 = SubVec2(collision_point.p, shape1.center_of_mass); contact->vcp0 = vcp0; contact->vcp1 = vcp1; contact->starting_separation = collision_point.separation; // // Debug draw // { // // P_Ent *ent0 = P_EntFromKey(frame, constraint->ent0); // // P_Ent *ent1 = P_EntFromKey(frame, constraint->ent1); // Vec2 normal = constraint->normal; // Vec2 center0 = Zi; // Vec2 center1 = Zi; // if (!P_IsEntNil(ent0)) center0 = P_WorldShapeFromEnt(ent0).center_of_mass; // if (!P_IsEntNil(ent1)) center1 = P_WorldShapeFromEnt(ent1).center_of_mass; // Vec2 p0 = AddVec2(center0, vcp0); // Vec2 p1 = AddVec2(center1, vcp1); // P_DebugDrawPoint(p0, Color_Cyan); // P_DebugDrawLine(p0, AddVec2(p0, normal), Color_White); // } } } } } } } } ////////////////////////////// //- Prune constraints { i64 constraint_idx = 0; while (constraint_idx < frame->constraints_count) { P_Constraint *constraint = &frame->constraints[constraint_idx]; b32 prune = 1; if (constraint->last_touched_tick == frame->tick) { P_Ent *ent0 = P_EntFromKey(frame, constraint->ent0); P_Ent *ent1 = P_EntFromKey(frame, constraint->ent1); if (!P_IsEntNil(ent0) && !P_IsEntNil(ent1)) { prune = 0; } } if (prune) { // Prune by replacing with last constraint // TODO: Investigate whether the reordering here can degrade stability P_Constraint *last_constraint = &frame->constraints[frame->constraints_count - 1]; *constraint = *last_constraint; frame->constraints_count -= 1; } else { constraint_idx += 1; } } } ////////////////////////////// //- Run solver steps i32 solver_steps_count = 4; f32 solver_dt = sim_dt / solver_steps_count; f32 contact_spring_hz = TweakFloat("Contact spring hz", 25, 5, 100); f32 contact_spring_damp = TweakFloat("Contact spring damp", 10, 5, 100); for (i32 solver_step_idx = 0; solver_step_idx < solver_steps_count; ++solver_step_idx) { ////////////////////////////// //- Prepare constraints for (i64 constraint_idx = 0; constraint_idx < frame->constraints_count; ++constraint_idx) { P_Constraint *constraint = &frame->constraints[constraint_idx]; Vec2 normal = constraint->normal; Vec2 tangent = PerpVec2(normal); f32 inv_m0 = constraint->inv_m0; f32 inv_m1 = constraint->inv_m1; f32 inv_i0 = constraint->inv_i0; f32 inv_i1 = constraint->inv_i1; for (i32 contact_idx = 0; contact_idx < constraint->points_count; ++contact_idx) { P_ContactPoint *contact = &constraint->points[contact_idx]; Vec2 vcp0 = contact->vcp0; Vec2 vcp1 = contact->vcp1; // Compute normal mass { f32 vcp0_wedge = WedgeVec2(vcp0, normal); f32 vcp1_wedge = WedgeVec2(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; } // Compute tangent mass { f32 vcp0_wedge = WedgeVec2(vcp0, tangent); f32 vcp1_wedge = WedgeVec2(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; } } } ////////////////////////////// //- Warm start constraints for (i64 constraint_idx = 0; constraint_idx < frame->constraints_count; ++constraint_idx) { P_Constraint *constraint = &frame->constraints[constraint_idx]; P_Ent *ent0 = P_EntFromKey(frame, constraint->ent0); P_Ent *ent1 = P_EntFromKey(frame, constraint->ent1); Vec2 v0 = ent0->solved_v; Vec2 v1 = ent1->solved_v; f32 w0 = ent0->solved_w; f32 w1 = ent1->solved_w; Vec2 normal = constraint->normal; Vec2 tangent = PerpVec2(normal); for (i32 contact_idx = 0; contact_idx < constraint->points_count; ++contact_idx) { P_ContactPoint *contact = &constraint->points[contact_idx]; Vec2 vcp0 = contact->vcp0; Vec2 vcp1 = contact->vcp1; Vec2 impulse = AddVec2(MulVec2(normal, contact->solved_normal_impulse), MulVec2(tangent, contact->solved_tangent_impulse)); // impulse = MulVec2(impulse, inv_num_points); v0 = SubVec2(v0, MulVec2(impulse, constraint->inv_m0)); v1 = AddVec2(v1, MulVec2(impulse, constraint->inv_m1)); w0 -= WedgeVec2(vcp0, impulse) * constraint->inv_i0; w1 += WedgeVec2(vcp1, impulse) * constraint->inv_i1; } if (!P_IsEntNil(ent0)) { ent0->solved_v = v0; ent0->solved_w = w0; } if (!P_IsEntNil(ent1)) { ent1->solved_v = v1; ent1->solved_w = w1; } } ////////////////////////////// //- Solve constraints for (i64 constraint_idx = 0; constraint_idx < frame->constraints_count; ++constraint_idx) { P_Constraint *constraint = &frame->constraints[constraint_idx]; P_Ent *ent0 = P_EntFromKey(frame, constraint->ent0); P_Ent *ent1 = P_EntFromKey(frame, constraint->ent1); f32 inv_m0 = constraint->inv_m0; f32 inv_m1 = constraint->inv_m1; f32 inv_i0 = constraint->inv_i0; f32 inv_i1 = constraint->inv_i1; Vec2 v0 = ent0->solved_v; Vec2 v1 = ent1->solved_v; f32 w0 = ent0->solved_w; f32 w1 = ent1->solved_w; Vec2 center0 = constraint->static_center0; Vec2 center1 = constraint->static_center1; if (!P_IsEntNil(ent0)) { center0 = P_WorldShapeFromEnt(ent0).center_of_mass; } if (!P_IsEntNil(ent1)) { center1 = P_WorldShapeFromEnt(ent1).center_of_mass; } // Normal impulse Vec2 normal = constraint->normal; for (i32 contact_idx = 0; contact_idx < constraint->points_count; ++contact_idx) { P_ContactPoint *contact = &constraint->points[contact_idx]; Vec2 vcp0 = contact->vcp0; Vec2 vcp1 = contact->vcp1; Vec2 p0 = AddVec2(center0, vcp0); Vec2 p1 = AddVec2(center1, vcp1); f32 separation = DotVec2(SubVec2(p1, p0), normal) + contact->starting_separation; f32 velocity_bias = 0.0; f32 mass_scale = 1.0; f32 impulse_scale = 0.0; // TODO: Do a relaxation pass without bias b32 apply_bias = 1; if (separation > 0.0) { // Speculative velocity_bias = separation / solver_dt; } else if (apply_bias) { // Soft constraint SoftSpring softness = MakeSpring(contact_spring_hz, contact_spring_damp, solver_dt); // f32 pushout_velocity = constraint->pushout_velocity; f32 pushout_velocity = 3.0; mass_scale = softness.mass_scale; impulse_scale = softness.impulse_scale; velocity_bias = MaxF32(softness.bias_rate * separation, -pushout_velocity); } Vec2 vel0 = AddVec2(v0, MulPerpVec2(vcp0, w0)); Vec2 vel1 = AddVec2(v1, MulPerpVec2(vcp1, w1)); Vec2 vrel = SubVec2(vel0, vel1); f32 k = contact->inv_normal_mass; // (to be applied along n) f32 vn = DotVec2(vrel, normal); f32 j = ((k * mass_scale) * (vn - velocity_bias)) - (contact->solved_normal_impulse * impulse_scale); f32 old_impulse = contact->solved_normal_impulse; f32 new_impulse = MaxF32(old_impulse + j, 0); f32 delta = new_impulse - old_impulse; contact->solved_normal_impulse = new_impulse; Vec2 impulse = MulVec2(normal, delta); v0 = SubVec2(v0, MulVec2(impulse, inv_m0)); v1 = AddVec2(v1, MulVec2(impulse, inv_m1)); w0 -= WedgeVec2(vcp0, impulse) * inv_i0; w1 += WedgeVec2(vcp1, impulse) * inv_i1; } // Tangent impulse Vec2 tangent = PerpVec2(normal); for (i32 contact_idx = 0; contact_idx < constraint->points_count; ++contact_idx) { P_ContactPoint *contact = &constraint->points[contact_idx]; Vec2 vcp0 = contact->vcp0; Vec2 vcp1 = contact->vcp1; Vec2 vel0 = AddVec2(v0, MulPerpVec2(vcp0, w0)); Vec2 vel1 = AddVec2(v1, MulPerpVec2(vcp1, w1)); Vec2 vrel = SubVec2(vel0, vel1); f32 k = contact->inv_tangent_mass; // (to be applied along t) f32 vt = DotVec2(vrel, tangent); f32 j = vt * k; f32 max_friction = constraint->friction * contact->solved_normal_impulse; f32 old_impulse = contact->solved_tangent_impulse; f32 new_impulse = ClampF32(old_impulse + j, -max_friction, max_friction); f32 delta = new_impulse - old_impulse; contact->solved_tangent_impulse = new_impulse; Vec2 impulse = MulVec2(tangent, delta); v0 = SubVec2(v0, MulVec2(impulse, inv_m0)); v1 = AddVec2(v1, MulVec2(impulse, inv_m1)); w0 -= WedgeVec2(vcp0, impulse) * inv_i0; w1 += WedgeVec2(vcp1, impulse) * inv_i1; } if (!P_IsEntNil(ent0)) { ent0->solved_v = v0; ent0->solved_w = w0; } if (!P_IsEntNil(ent1)) { ent1->solved_v = v1; ent1->solved_w = w1; } } ////////////////////////////// //- Integrate velocities for (P_Ent *ent = P_FirstEnt(frame); !P_IsEntNil(ent); ent = P_NextEnt(ent)) { Xform xf = ent->xf; xf.og = AddVec2(xf.og, MulVec2(ent->solved_v, solver_dt)); xf = RotateXform(xf, ent->solved_w * solver_dt); ent->xf = xf; } } ////////////////////////////// //- Move bullets for (P_Ent *bullet = P_FirstEnt(frame); !P_IsEntNil(bullet); bullet = P_NextEnt(bullet)) { if (bullet->is_bullet) { Vec2 start = bullet->bullet_start; Vec2 end = bullet->bullet_end; Vec2 vel = SubVec2(end, start); bullet->bullet_start = end; bullet->bullet_end = AddVec2(end, vel); } } ////////////////////////////// //- Fire bullets // TODO: Remove this { P_EntList bullets_to_spawn = Zi; for (P_Ent *firer = P_FirstEnt(frame); !P_IsEntNil(firer); firer = P_NextEnt(firer)) { if (firer->is_guy && firer->control.fire_held) // if (firer->fire_presses) { // i64 fire_delta_ns = frame->time_ns - firer->last_fire_ns; // i64 single_bullet_delta_ns = NsFromSeconds(1) / firer->fire_rate; // i64 tick_bullets_count = sim_dt * firer->fire_rate; f32 fire_rate = 50; f32 bullets_per_fire = 1; // f32 spread = Tau * 0.05; f32 spread = Tau * 0.01; f32 tweak_speed = TweakFloat("Bullet speed", 100, 1, 100); b32 can_fire = (firer->last_fire_ns + NsFromSeconds(1.0 / fire_rate)) <= frame->time_ns; if (can_fire) { i64 tick_bullets_count = bullets_per_fire; if (tick_bullets_count > 0) { P_Shape firer_world_shape = P_WorldShapeFromEnt(firer); Vec2 pos = P_EdgePointFromShape(firer_world_shape, firer->control.look); for (i64 bullet_idx = 0; bullet_idx < tick_bullets_count; ++bullet_idx) { P_Ent *bullet = P_PushTempEnt(scratch.arena, &bullets_to_spawn); bullet->is_bullet = 1; bullet->key = P_RandKey(); f32 rand_speed = ((f32)P_RandU64FromEnt(firer) / (f32)0xFFFFFFFFFFFFFFFFull) - 0.5; f32 rand_angle = ((f32)P_RandU64FromEnt(firer) / (f32)0xFFFFFFFFFFFFFFFFull) - 0.5; f32 speed = tweak_speed * sim_dt; f32 angle = AngleFromVec2(firer->control.look); speed += (speed * 0.5) * rand_speed; angle += rand_angle * spread; Vec2 dir = Vec2FromAngle(angle); bullet->bullet_start = pos; bullet->bullet_end = AddVec2(bullet->bullet_start, MulVec2(dir, speed)); bullet->bullet_firer = firer->key; } } firer->last_fire_ns = frame->time_ns; } } } P_SpawnEntsFromList(frame, bullets_to_spawn); } ////////////////////////////// //- Update bullet hits // TODO: Not like this // TODO: Separate 'hits' from bullets, so that bullets can have multiple hits for (P_Ent *bullet = P_FirstEnt(frame); !P_IsEntNil(bullet); bullet = P_NextEnt(bullet)) { if (bullet->is_bullet) { bullet->has_hit = 0; Vec2 ray_start = bullet->bullet_start; Vec2 ray_end = bullet->bullet_end; Vec2 ray_dir = SubVec2(ray_end, ray_start); // TODO: Real raycast query P_Ent *closest_victim = &P_NilEnt; P_RaycastResult victim_raycast = Zi; { f32 closest_len_sq = Inf; for (P_Ent *victim = P_FirstEnt(frame); !P_IsEntNil(victim); victim = P_NextEnt(victim)) { if (victim->is_guy && !P_MatchKey(victim->key, bullet->bullet_firer)) { P_Shape victim_world_shape = P_WorldShapeFromEnt(victim); P_RaycastResult entrance_raycast = P_RaycastShape(victim_world_shape, ray_start, ray_dir); Vec2 entrance = entrance_raycast.p; if (entrance_raycast.is_intersecting) { P_RaycastResult exit_raycast = P_RaycastShape(victim_world_shape, ray_start, NegVec2(ray_dir)); Vec2 exit = exit_raycast.p; f32 da = DotVec2(ray_dir, SubVec2(entrance, ray_start)); f32 db = DotVec2(ray_dir, SubVec2(exit, ray_start)); if (db > 0 && (da <= Vec2LenSq(ray_dir) || da <= 0)) { f32 len_sq = Vec2LenSq(SubVec2(entrance_raycast.p, ray_start)); if (len_sq < closest_len_sq) { closest_len_sq = len_sq; closest_victim = victim; victim_raycast = entrance_raycast; } } } } } } if (!P_IsEntNil(closest_victim)) { bullet->has_hit = 1; bullet->hit_entry = victim_raycast.p; bullet->hit_entry_normal = victim_raycast.normal; // bullet->bullet_end = bullet->hit_entry; bullet->exists = 0; } Rng2 bounds = Zi; bounds.p0 = VEC2(-P_WorldPitch / 2, -P_WorldPitch / 2); bounds.p1 = VEC2(P_WorldPitch / 2, P_WorldPitch / 2); if ( bullet->bullet_start.x < bounds.p0.x || bullet->bullet_start.y < bounds.p0.y || bullet->bullet_start.x > bounds.p1.x || bullet->bullet_start.y > bounds.p1.y ) { bullet->exists = 0; } } } ////////////////////////////// //- End frame frame->time_ns += sim_dt_ns; EndScratch(scratch); }