From e461eabeb88dd81d582dc3179f87cb8daf3ca847 Mon Sep 17 00:00:00 2001 From: jacob Date: Sat, 10 Jan 2026 14:04:28 -0600 Subject: [PATCH] sim physics solver testing --- src/collider/collider.c | 2 +- src/pp/pp_sim/pp_sim_core.c | 765 +++++++++++++++++++++++++++++------- src/pp/pp_sim/pp_sim_core.h | 38 +- src/pp/pp_vis/pp_vis_core.c | 6 +- src/sprite/sprite.c | 1 + 5 files changed, 663 insertions(+), 149 deletions(-) diff --git a/src/collider/collider.c b/src/collider/collider.c index 9afc9dbb..174a7f4a 100644 --- a/src/collider/collider.c +++ b/src/collider/collider.c @@ -184,7 +184,7 @@ CLD_GjkData CLD_GjkDataFromShapes(CLD_Shape *shape0, CLD_Shape *shape1, Xform xf } ////////////////////////////// - //- Find third piont in simplex + //- Find third point in simplex { CLD_DBGSTEP; diff --git a/src/pp/pp_sim/pp_sim_core.c b/src/pp/pp_sim/pp_sim_core.c index 493b8a8a..5e20b2c6 100644 --- a/src/pp/pp_sim/pp_sim_core.c +++ b/src/pp/pp_sim/pp_sim_core.c @@ -231,6 +231,19 @@ S_Shape S_LocalShapeFromEnt(S_Ent *ent) .count = 1, .radius = 0.3, ); + + // Rng2 test_rect = Zi; + // test_rect.p0 = VEC2(-1, -1); + // test_rect.p1 = VEC2(1, 1); + // result = S_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; @@ -395,7 +408,7 @@ S_CollisionResult S_CollisionResultFromShapes(S_Shape shape0, S_Shape shape1) } ////////////////////////////// - //- Find third piont in simplex + //- Find third point in simplex { m = S_MenkowskiPointFromShapes(shape0, shape1, dir); @@ -917,7 +930,6 @@ S_CollisionResult S_CollisionResultFromShapes(S_Shape shape0, S_Shape shape1) result.closest_p0 = closest_p0; result.closest_p1 = closest_p1; - EndScratch(scratch); return result; } @@ -1336,10 +1348,10 @@ void S_TickForever(WaveLaneCtx *lane) ////////////////////////////// //- Update double-buffered entity data - // for (S_Ent *ent = S_FirstEnt(world); ent->valid; ent = S_NextEnt(ent)) - // { - // ent->last_xf = ent->xf; - // } + for (S_Ent *ent = S_FirstEnt(world); ent->valid; ent = S_NextEnt(ent)) + { + ent->last_xf = ent->xf; + } ////////////////////////////// //- Process save commands @@ -1451,181 +1463,648 @@ void S_TickForever(WaveLaneCtx *lane) for (S_Ent *ent = S_FirstEnt(world); ent->valid; ent = S_NextEnt(ent)) { - Xform xf = ent->xf; + // Xform xf = ent->xf; + // Xform desired_xf = xf; + // if (!IsVec2Zero(ent->look)) + // { + // desired_xf = XformWithWorldRotation(xf, AngleFromVec2(ent->look)); + // } + // f32 move_speed = TweakFloat("Player move speed", 6.5, 0, 20); + // desired_xf.og = AddVec2(xf.og, MulVec2(ent->move, move_speed * sim_dt)); + + // Vec2 pos_diff = SubVec2(desired_xf.og, xf.og); + // f32 angle_diff = UnwindAngleF32(RotationFromXform(desired_xf) - RotationFromXform(xf)); + + // ent->solved_v = pos_diff; + // ent->solved_w = angle_diff; - Xform desired_xf = xf; - if (!IsVec2Zero(ent->look)) { - desired_xf = XformWithWorldRotation(xf, AngleFromVec2(ent->look)); + // f32 damp_vel = damp_force * sim_dt; + + if (Vec2Len(ent->solved_v) > 0.001) + { + f32 damp_force = TweakFloat("Player damp force", 50, 0, 100); + Vec2 damp = MulVec2(NegVec2(ent->solved_v), damp_force * sim_dt); + ent->solved_v = AddVec2(ent->solved_v, damp); + } + else + { + ent->solved_v = VEC2(0, 0); + } + } - f32 move_speed = TweakFloat("Player move speed", 6.5, 0, 20); - desired_xf.og = AddVec2(xf.og, MulVec2(ent->move, move_speed * sim_dt)); - Vec2 pos_diff = SubVec2(desired_xf.og, xf.og); - f32 angle_diff = UnwindAngleF32(RotationFromXform(desired_xf) - RotationFromXform(xf)); + { + f32 move_force = TweakFloat("Player move force", 400, 0, 400); + f32 max_speed = TweakFloat("Player max speed", 10, 0, 20); - ent->solved_dv = pos_diff; - ent->solved_dw = angle_diff; + Vec2 new_velocity = ent->solved_v; + new_velocity = AddVec2(new_velocity, MulVec2(ent->move, move_force * sim_dt)); + + // if (Vec2Len(new_velocity) > max_speed) + // { + // new_velocity = Vec2WithLen(new_velocity, max_speed); + // } + + ent->solved_v = new_velocity; + } } - ////////////////////////////// - //- Prune constraints + ////////////////////////////// //- Generate player wall constraints + + + // TODO: Not like this + // i64 max_constraints = 4096; + // i64 constraints_count = 0; + // S_Constraint *constraints = PushStructs(frame_arena, S_Constraint, max_constraints); - i64 max_constraints = 4096; - i64 constraints_count = 0; - S_Constraint *constraints = PushStructsNoZero(frame_arena, S_Constraint, max_constraints); + PERSIST i64 max_constraints = 4096; + PERSIST i64 constraints_count = 0; + PERSIST S_Constraint *constraints = 0; + if (!constraints) + { + constraints = PushStructs(frame_arena, S_Constraint, max_constraints); + } + + for (S_Ent *ent0 = S_FirstEnt(world); ent0->valid; ent0 = S_NextEnt(ent0)) + { + S_Shape shape0 = S_WorldShapeFromEnt(ent0); + for (S_Ent *ent1 = S_FirstEnt(world); ent1->valid; ent1 = S_NextEnt(ent1)) + { + if (ent1 > ent0) + { + S_Shape shape1 = S_WorldShapeFromEnt(ent1); + + // TODO: World query + S_CollisionResult collision = S_CollisionResultFromShapes(shape0, shape1); + if (collision.collision_points_count > 0) + { + // FIXME: Key lookup + S_Constraint *constraint = 0; + { + b32 match = 0; + for (i64 constraint_idx = 0; constraint_idx < constraints_count; ++constraint_idx) + { + constraint = &constraints[constraint_idx]; + if (S_MatchKey(constraint->ent0, ent0->key) && S_MatchKey(constraint->ent1, ent1->key)) + { + match = 1; + break; + } + } + if (!match) + { + if (constraints_count < max_constraints) + { + constraint = &constraints[constraints_count]; + constraints_count += 1; + ZeroStruct(constraint); + } + } + } + if (constraint) + { + constraint->last_touched_tick = world->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 = 10; + f32 inv_i1 = 10; + + 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) + { + S_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) + { + S_CollisionPoint collision_point = collision.collision_points[collision_point_idx]; + + u32 id = collision_point.id; + S_ContactPoint *contact = 0; + { + for (i32 contact_point_idx = 0; contact_point_idx < constraint->points_count; ++contact_point_idx) + { + S_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 + // { + // // S_Ent *ent0 = S_EntFromKey(world, constraint->ent0); + // // S_Ent *ent1 = S_EntFromKey(world, constraint->ent1); + // Vec2 normal = constraint->normal; + // Vec2 center0 = Zi; + // Vec2 center1 = Zi; + // if (ent0->valid) center0 = S_WorldShapeFromEnt(ent0).center_of_mass; + // if (ent1->valid) center1 = S_WorldShapeFromEnt(ent1).center_of_mass; + // Vec2 p0 = AddVec2(center0, vcp0); + // Vec2 p1 = AddVec2(center1, vcp1); + // S_DebugDrawPoint(p0, Color_Cyan); + // S_DebugDrawLine(p0, AddVec2(p0, normal), Color_White); + // } + } + } + } + } + } + } + + + + + + // // TODO: Not like this + + // // i64 max_constraints = 4096; + // // i64 constraints_count = 0; + // // S_Constraint *constraints = PushStructs(frame_arena, S_Constraint, max_constraints); + + // PERSIST i64 max_constraints = 4096; + // PERSIST i64 constraints_count = 0; + // PERSIST S_Constraint *constraints = 0; + // if (!constraints) + // { + // constraints = PushStructs(frame_arena, S_Constraint, max_constraints); + // } + // for (S_Ent *ent = S_FirstEnt(world); ent->valid; ent = S_NextEnt(ent)) // { // if (ent->is_player) // { // Xform last_xf = ent->last_xf; - // S_Shape last_world_shape = S_MulXformShape(last_xf, ent->local_shape); - // Xform xf = ent->xf; - // S_Shape world_shape = S_MulXformShape(xf, ent->local_shape); - // Rng2 bb0 = S_BoundingBoxFromShape(last_world_shape); - // Rng2 bb1 = S_BoundingBoxFromShape(world_shape); + // S_Shape local_shape = S_LocalShapeFromEnt(ent); + // S_Shape last_world_shape = S_MulXformShape(last_xf, local_shape); + // S_Shape shape0 = S_WorldShapeFromEnt(ent); - // if (constraints_count < max_constraints) - // { - // S_Constraint *constraint = &constraints[constraints_count]; + // // TODO: Real constraint data - // // TODO: Real constraint data - - // constraint->ent0 = ent->key; - // constraint->shape0 = world_shape; - - // Rng2 test_rect = Zi; - // test_rect.p0 = VEC2(-1, -1); - // test_rect.p1 = VEC2(1, 1); - // constraint->shape1 = S_ShapeFromDesc( - // .radius = 0.5, - // .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), - // ); - - // constraints_count += 1; - // } - // } - // } - - ////////////////////////////// - //- Solve constraints - - // for (i64 constraint_idx = 0; constraint_idx < constraints_count; ++constraint_idx) - // { - // S_Constraint *constraint = &constraints[constraint_idx]; - - // S_Ent *ent0 = S_EntFromKey(world, constraint->ent0); - // S_Ent *ent1 = S_EntFromKey(world, constraint->ent1); - - // Vec2 old_dv0 = ent0->solved_dv; - // Vec2 old_dv1 = ent1->solved_dv; - - // Vec2 dv0 = VEC2(0, 0); - // Vec2 dv1 = VEC2(0, 0); - // f32 dw0 = 0; - // f32 dw1 = 0; - // { - // // // Get shapes - // // S_Shape shape0 = S_ShapeFromDesc(.count = 1); - // // S_Shape shape1 = S_ShapeFromDesc(.count = 1); - // // if (ent0->active) - // // { - // // shape0 = S_MulXformShape(ent0->xf, ent0->local_shape); - // // } - // // if (ent1->active) - // // { - // // shape1 = S_MulXformShape(ent1->xf, ent1->local_shape); - // // } - - // S_Shape shape0 = constraint->shape0; - // S_Shape shape1 = constraint->shape1; + // Rng2 test_rect = Zi; + // test_rect.p0 = VEC2(-1, -1); + // test_rect.p1 = VEC2(1, 1); + // S_Shape shape1 = S_ShapeFromDesc( + // // .radius = 0.5, + // .radius = 2, + // .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), + // ); // S_DebugDrawShape(shape1, Color_Orange); - // // TODO: Real dir - // Vec2 shape_dir = NormVec2(SubVec2(shape1.centroid, shape0.centroid)); - // Vec2 neg_shape_dir = NegVec2(shape_dir); - - // // TODO: Real relative velocity - // Vec2 rel_vel = SubVec2(old_dv1, old_dv0); - - // // Vec2 normal = NormVec2(rel_vel); - // // Vec2 neg_normal = NegVec2(normal); - // Vec2 normal = NormVec2(shape_dir); - // Vec2 neg_normal = NegVec2(normal); - - // // Vec2 shape0_pt = S_SupportPointFromShape(shape0, shape_dir); - // // Vec2 shape1_pt = S_SupportPointFromShape(shape1, neg_shape_dir); - - // S_CollisionResult collision_data = S_CollisionResultFromShapes(shape0, shape1); - // Vec2 shape0_pt = collision_data.closest_p0; - // Vec2 shape1_pt = collision_data.closest_p1; - - // Vec2 sep = SubVec2(shape1_pt, shape0_pt); - // f32 sep_along_normal = DotVec2(sep, normal); - - // f32 rel_vel_along_normal = DotVec2(rel_vel, normal); - - // // f32 sep_normal = DotVec2(sep, normal); - - // S_DebugDrawPoint(shape0_pt, Color_Cyan); - // S_DebugDrawPoint(shape1_pt, Color_Cyan); - - // S_DebugDrawPoint(collision_data.collision_points[0].p, Color_Red); - // S_DebugDrawPoint(collision_data.collision_points[0].p, Color_Red); - - // if (sep_along_normal < 0) + // // TODO: World query + // S_CollisionResult collision = S_CollisionResultFromShapes(shape0, shape1); + // if (collision.collision_points_count > 0) // { - // dv0 = AddVec2(dv0, MulVec2(normal, sep_along_normal)); + // // FIXME: Key lookup + // S_Constraint *constraint = 0; + // { + // b32 match = 0; + // for (i64 constraint_idx = 0; constraint_idx < constraints_count; ++constraint_idx) + // { + // constraint = &constraints[constraint_idx]; + // if (S_MatchKey(constraint->ent0, ent->key)) + // { + // match = 1; + // break; + // } + // } + // if (!match) + // { + // if (constraints_count < max_constraints) + // { + // constraint = &constraints[constraints_count]; + // constraints_count += 1; + // ZeroStruct(constraint); + // } + // } + // } + // if (constraint) + // { + // constraint->last_touched_tick = world->tick; + // constraint->normal = collision.collision_normal; + + // // TODO: Real masses + // f32 inv_m0 = 0.5; + // f32 inv_m1 = 0; + // f32 inv_i0 = 0.5; + // f32 inv_i1 = 0; + + // constraint->ent0 = ent->key; + // constraint->static_center1 = shape1.center_of_mass; + + // constraint->inv_m0 = inv_m0; + // constraint->inv_m1 = 0; + // constraint->inv_i0 = 0; + // constraint->inv_i1 = 0; + + // // Delete old contacts that are no longer present + // for (i32 contact_point_idx = 0; contact_point_idx < constraint->points_count; ++contact_point_idx) + // { + // S_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) + // { + // S_CollisionPoint collision_point = collision.collision_points[collision_point_idx]; + + // u32 id = collision_point.id; + // S_ContactPoint *contact = 0; + // { + // for (i32 contact_point_idx = 0; contact_point_idx < constraint->points_count; ++contact_point_idx) + // { + // S_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 + // { + // S_Ent *ent0 = S_EntFromKey(world, constraint->ent0); + // S_Ent *ent1 = S_EntFromKey(world, constraint->ent1); + // Vec2 normal = constraint->normal; + // Vec2 center0 = Zi; + // Vec2 center1 = Zi; + // if (ent0->valid) center0 = S_WorldShapeFromEnt(ent0).center_of_mass; + // if (ent1->valid) center1 = S_WorldShapeFromEnt(ent1).center_of_mass; + // Vec2 p0 = AddVec2(center0, vcp0); + // Vec2 p1 = AddVec2(center1, vcp1); + // S_DebugDrawPoint(p0, Color_Cyan); + // S_DebugDrawLine(p0, AddVec2(p0, normal), Color_White); + // } + // } + // } // } - // // dv0 = VEC2(sep_normal, sep_normal); - - // // f32 separation = constraint->sep; - // } - - // // Update solved velocity - // if (ent0->valid) - // { - // ent0->solved_dv = AddVec2(ent0->solved_dv, dv0); - // ent0->solved_dw += dw0; - // } - // if (ent1->valid) - // { - // ent1->solved_dv = AddVec2(ent1->solved_dv, dv1); - // ent1->solved_dw += dw1; // } // } ////////////////////////////// - //- Integrate velocities + //- Prune constraints - for (S_Ent *ent = S_FirstEnt(world); ent->valid; ent = S_NextEnt(ent)) { - Xform xf = ent->xf; - xf.og = AddVec2(xf.og, ent->solved_dv); - xf = RotateXform(xf, ent->solved_dw); - ent->xf = xf; + i64 constraint_idx = 0; + while (constraint_idx < constraints_count) + { + S_Constraint *constraint = &constraints[constraint_idx]; + b32 prune = 1; + if (constraint->last_touched_tick == world->tick) + { + if (S_EntFromKey(world, constraint->ent0)->valid || S_EntFromKey(world, constraint->ent1)->valid) + { + prune = 0; + } + } + if (prune) + { + // Prune by replacing with last constraint + // TODO: Investigate whether the reordering here can degrade stability + S_Constraint *last_constraint = &constraints[constraints_count - 1]; + *constraint = *last_constraint; + 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 < constraints_count; ++constraint_idx) + { + S_Constraint *constraint = &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) + { + S_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 < constraints_count; ++constraint_idx) + { + S_Constraint *constraint = &constraints[constraint_idx]; + + S_Ent *ent0 = S_EntFromKey(world, constraint->ent0); + S_Ent *ent1 = S_EntFromKey(world, 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) + { + S_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 (ent0->valid) + { + ent0->solved_v = v0; + ent0->solved_w = w0; + } + if (ent1->valid) + { + ent1->solved_v = v1; + ent1->solved_w = w1; + } + } + + ////////////////////////////// + //- Solve constraints + + for (i64 constraint_idx = 0; constraint_idx < constraints_count; ++constraint_idx) + { + S_Constraint *constraint = &constraints[constraint_idx]; + + S_Ent *ent0 = S_EntFromKey(world, constraint->ent0); + S_Ent *ent1 = S_EntFromKey(world, 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 (ent0->valid) + { + center0 = S_WorldShapeFromEnt(ent0).center_of_mass; + } + if (ent1->valid) + { + center1 = S_WorldShapeFromEnt(ent1).center_of_mass; + } + + // Normal impulse + Vec2 normal = constraint->normal; + for (i32 contact_idx = 0; contact_idx < constraint->points_count; ++contact_idx) + { + S_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; + + // TDOO: 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) + { + S_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 (ent0->valid) + { + ent0->solved_v = v0; + ent0->solved_w = w0; + } + if (ent1->valid) + { + ent1->solved_v = v1; + ent1->solved_w = w1; + } + } + + ////////////////////////////// + //- Integrate velocities + + for (S_Ent *ent = S_FirstEnt(world); ent->valid; ent = S_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; + } + } + + + @@ -1938,10 +2417,18 @@ void S_TickForever(WaveLaneCtx *lane) S_DebugDrawShape(world_shape, color); } - // Draw look + // Draw rot { Vec4 color = VEC4(0.8, 0.8, 0.8, 1); Vec2 p0 = world_shape.centroid; + Vec2 p1 = S_EdgePointFromShape(world_shape, UpFromXform(ent->xf)); + S_DebugDrawLine(p0, p1, color); + } + + // Draw look + { + Vec4 color = VEC4(0.4, 0.8, 0.4, 1); + Vec2 p0 = world_shape.centroid; Vec2 p1 = S_EdgePointFromShape(world_shape, ent->look); S_DebugDrawLine(p0, p1, color); } diff --git a/src/pp/pp_sim/pp_sim_core.h b/src/pp/pp_sim/pp_sim_core.h index ac1ff941..264ef013 100644 --- a/src/pp/pp_sim/pp_sim_core.h +++ b/src/pp/pp_sim/pp_sim_core.h @@ -62,6 +62,7 @@ Struct(S_Ent) b32 is_player; f32 health; + Xform last_xf; Xform xf; Vec2 move; @@ -85,8 +86,8 @@ Struct(S_Ent) ////////////////////////////// //- Solver data - Vec2 solved_dv; - f32 solved_dw; + Vec2 solved_v; + f32 solved_w; }; Struct(S_EntListNode) @@ -114,7 +115,7 @@ Struct(S_EntBin) Struct(S_SupportPoint) { Vec2 p; - i32 id; // Index of the originating piont in the shape + u32 id; // Index of the originating piont in the shape }; Struct(S_CollisionPoint) @@ -145,7 +146,7 @@ Struct(S_ClippedLine) Struct(S_CollisionResult) { - // Contact manifold + // Collision manifold i32 collision_points_count; S_CollisionPoint collision_points[2]; Vec2 collision_normal; @@ -165,13 +166,38 @@ Struct(S_RaycastResult) //////////////////////////////////////////////////////////// //~ Constraint types +Struct(S_ContactPoint) +{ + Vec2 vcp0; + Vec2 vcp1; + f32 starting_separation; + f32 inv_normal_mass; + f32 inv_tangent_mass; + u32 id; + + f32 solved_normal_impulse; + f32 solved_tangent_impulse; +}; + Struct(S_Constraint) { + i64 last_touched_tick; S_Key ent0; S_Key ent1; - S_Shape shape0; - S_Shape shape1; + Vec2 static_center0; + Vec2 static_center1; + + f32 inv_m0; + f32 inv_m1; + f32 inv_i0; + f32 inv_i1; + + Vec2 normal; + f32 friction; + + i32 points_count; + S_ContactPoint points[2]; }; //////////////////////////////////////////////////////////// diff --git a/src/pp/pp_vis/pp_vis_core.c b/src/pp/pp_vis/pp_vis_core.c index 0d8703b6..70d87220 100644 --- a/src/pp/pp_vis/pp_vis_core.c +++ b/src/pp/pp_vis/pp_vis_core.c @@ -3114,8 +3114,8 @@ void V_TickForever(WaveLaneCtx *lane) case S_DebugDrawKind_Shape: { S_Shape ui_shape = S_MulXformShape(frame->xf.world_to_ui, desc->shape); - // V_DrawShape(ui_shape, color, detail, V_DrawFlag_Line); - V_DrawShape(ui_shape, color, detail, V_DrawFlag_None); + V_DrawShape(ui_shape, color, detail, V_DrawFlag_Line); + // V_DrawShape(ui_shape, color, detail, V_DrawFlag_None); } break; } } @@ -3210,7 +3210,7 @@ void V_TickForever(WaveLaneCtx *lane) for (S_TileKind tile_kind = 0; tile_kind < S_TileKind_COUNT; ++tile_kind) { String tile_name = S_TileNameFromKind(tile_kind); - String sheet_name = StringF(frame->arena, "sprite/%F.ase", FmtString(tile_name)); + String sheet_name = StringF(frame->arena, "tile/%F.ase", FmtString(tile_name)); ResourceKey sheet_resource = ResourceKeyFromStore(&P_Resources, sheet_name); SPR_SheetKey sheet = SPR_SheetKeyFromResource(sheet_resource); SPR_Slice tile_slice = SPR_SliceFromSheet(sheet, Lit("")); diff --git a/src/sprite/sprite.c b/src/sprite/sprite.c index 54fd06c1..4f3ad62d 100644 --- a/src/sprite/sprite.c +++ b/src/sprite/sprite.c @@ -145,6 +145,7 @@ void SPR_TickAsync(WaveLaneCtx *lane, AsyncFrameLaneCtx *base_async_lane_frame) cmds_count = SPR.submit.count; SPR.submit.first = 0; SPR.submit.last = 0; + SPR.submit.count = 0; } Unlock(&lock); }