jacob/power_play
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

sim physics solver testing

Browse Source
This commit is contained in:
jacob 2026-01-10 14:04:28 -06:00
parent 3945008984
commit e461eabeb8
5 changed files with 663 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/collider/collider.c
View File

@ -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; CLD_DBGSTEP;

701
src/pp/pp_sim/pp_sim_core.c
View File

@ -231,6 +231,19 @@ S_Shape S_LocalShapeFromEnt(S_Ent *ent)
.count = 1, .count = 1,
.radius = 0.3, .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; 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); 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_p0 = closest_p0;
result.closest_p1 = closest_p1; result.closest_p1 = closest_p1;
EndScratch(scratch); EndScratch(scratch);
return result; return result;
} }
@ -1336,10 +1348,10 @@ void S_TickForever(WaveLaneCtx *lane)
////////////////////////////// //////////////////////////////
//- Update double-buffered entity data //- Update double-buffered entity data
// for (S_Ent *ent = S_FirstEnt(world); ent->valid; ent = S_NextEnt(ent)) for (S_Ent *ent = S_FirstEnt(world); ent->valid; ent = S_NextEnt(ent))
// { {
// ent->last_xf = ent->xf; ent->last_xf = ent->xf;
// } }
////////////////////////////// //////////////////////////////
//- Process save commands //- Process save commands
@ -1451,68 +1463,253 @@ void S_TickForever(WaveLaneCtx *lane)
for (S_Ent *ent = S_FirstEnt(world); ent->valid; ent = S_NextEnt(ent)) 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);
} }
f32 move_speed = TweakFloat("Player move speed", 6.5, 0, 20); else
desired_xf.og = AddVec2(xf.og, MulVec2(ent->move, move_speed * sim_dt)); {
ent->solved_v = VEC2(0, 0);
Vec2 pos_diff = SubVec2(desired_xf.og, xf.og);
f32 angle_diff = UnwindAngleF32(RotationFromXform(desired_xf) - RotationFromXform(xf));
ent->solved_dv = pos_diff;
ent->solved_dw = angle_diff;
} }
////////////////////////////// }
//- Prune constraints
{
f32 move_force = TweakFloat("Player move force", 400, 0, 400);
f32 max_speed = TweakFloat("Player max speed", 10, 0, 20);
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;
}
}
////////////////////////////// //////////////////////////////
//- Generate player wall constraints //- Generate player wall constraints
// TODO: Not like this // 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; PERSIST i64 max_constraints = 4096;
i64 constraints_count = 0; PERSIST i64 constraints_count = 0;
S_Constraint *constraints = PushStructsNoZero(frame_arena, S_Constraint, max_constraints); 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)) // for (S_Ent *ent = S_FirstEnt(world); ent->valid; ent = S_NextEnt(ent))
// { // {
// if (ent->is_player) // if (ent->is_player)
// { // {
// Xform last_xf = ent->last_xf; // Xform last_xf = ent->last_xf;
// S_Shape last_world_shape = S_MulXformShape(last_xf, ent->local_shape);
// Xform xf = ent->xf; // Xform xf = ent->xf;
// S_Shape world_shape = S_MulXformShape(xf, ent->local_shape);
// Rng2 bb0 = S_BoundingBoxFromShape(last_world_shape); // S_Shape local_shape = S_LocalShapeFromEnt(ent);
// Rng2 bb1 = S_BoundingBoxFromShape(world_shape); // 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; // Rng2 test_rect = Zi;
// test_rect.p0 = VEC2(-1, -1); // test_rect.p0 = VEC2(-1, -1);
// test_rect.p1 = VEC2(1, 1); // test_rect.p1 = VEC2(1, 1);
// constraint->shape1 = S_ShapeFromDesc( // S_Shape shape1 = S_ShapeFromDesc(
// .radius = 0.5, // // .radius = 0.5,
// .radius = 2,
// .count = 4, // .count = 4,
// .points[0] = VEC2(test_rect.p0.x, test_rect.p0.y), // .points[0] = VEC2(test_rect.p0.x, test_rect.p0.y),
// .points[1] = VEC2(test_rect.p1.x, test_rect.p0.y), // .points[1] = VEC2(test_rect.p1.x, test_rect.p0.y),
@ -1520,99 +1717,379 @@ void S_TickForever(WaveLaneCtx *lane)
// .points[3] = VEC2(test_rect.p0.x, test_rect.p1.y), // .points[3] = VEC2(test_rect.p0.x, test_rect.p1.y),
// ); // );
// S_DebugDrawShape(shape1, Color_Orange);
// // 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, ent->key))
// {
// match = 1;
// break;
// }
// }
// if (!match)
// {
// if (constraints_count < max_constraints)
// {
// constraint = &constraints[constraints_count];
// constraints_count += 1; // 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);
// }
// }
// }
// } // }
// } // }
// } // }
//////////////////////////////
//- Prune constraints
{
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 //- Solve constraints
// for (i64 constraint_idx = 0; constraint_idx < constraints_count; ++constraint_idx) for (i64 constraint_idx = 0; constraint_idx < constraints_count; ++constraint_idx)
// { {
// S_Constraint *constraint = &constraints[constraint_idx]; S_Constraint *constraint = &constraints[constraint_idx];
// S_Ent *ent0 = S_EntFromKey(world, constraint->ent0); S_Ent *ent0 = S_EntFromKey(world, constraint->ent0);
// S_Ent *ent1 = S_EntFromKey(world, constraint->ent1); S_Ent *ent1 = S_EntFromKey(world, constraint->ent1);
// Vec2 old_dv0 = ent0->solved_dv; f32 inv_m0 = constraint->inv_m0;
// Vec2 old_dv1 = ent1->solved_dv; 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 dv0 = VEC2(0, 0); Vec2 center0 = constraint->static_center0;
// Vec2 dv1 = VEC2(0, 0); Vec2 center1 = constraint->static_center1;
// f32 dw0 = 0; if (ent0->valid)
// f32 dw1 = 0; {
// { center0 = S_WorldShapeFromEnt(ent0).center_of_mass;
// // // Get shapes }
// // S_Shape shape0 = S_ShapeFromDesc(.count = 1); if (ent1->valid)
// // S_Shape shape1 = S_ShapeFromDesc(.count = 1); {
// // if (ent0->active) center1 = S_WorldShapeFromEnt(ent1).center_of_mass;
// // { }
// // shape0 = S_MulXformShape(ent0->xf, ent0->local_shape);
// // }
// // if (ent1->active)
// // {
// // shape1 = S_MulXformShape(ent1->xf, ent1->local_shape);
// // }
// S_Shape shape0 = constraint->shape0; // Normal impulse
// S_Shape shape1 = constraint->shape1; 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;
// S_DebugDrawShape(shape1, Color_Orange); f32 velocity_bias = 0.0;
f32 mass_scale = 1.0;
f32 impulse_scale = 0.0;
// // TODO: Real dir // TDOO: Do a relaxation pass without bias
// Vec2 shape_dir = NormVec2(SubVec2(shape1.centroid, shape0.centroid)); b32 apply_bias = 1;
// Vec2 neg_shape_dir = NegVec2(shape_dir); 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);
}
// // TODO: Real relative velocity Vec2 vel0 = AddVec2(v0, MulPerpVec2(vcp0, w0));
// Vec2 rel_vel = SubVec2(old_dv1, old_dv0); Vec2 vel1 = AddVec2(v1, MulPerpVec2(vcp1, w1));
Vec2 vrel = SubVec2(vel0, vel1);
// // Vec2 normal = NormVec2(rel_vel); f32 k = contact->inv_normal_mass;
// // Vec2 neg_normal = NegVec2(normal);
// Vec2 normal = NormVec2(shape_dir);
// Vec2 neg_normal = NegVec2(normal);
// // Vec2 shape0_pt = S_SupportPointFromShape(shape0, shape_dir); /* (to be applied along n) */
// // Vec2 shape1_pt = S_SupportPointFromShape(shape1, neg_shape_dir); f32 vn = DotVec2(vrel, normal);
f32 j = ((k * mass_scale) * (vn - velocity_bias)) - (contact->solved_normal_impulse * impulse_scale);
// S_CollisionResult collision_data = S_CollisionResultFromShapes(shape0, shape1); f32 old_impulse = contact->solved_normal_impulse;
// Vec2 shape0_pt = collision_data.closest_p0; f32 new_impulse = MaxF32(old_impulse + j, 0);
// Vec2 shape1_pt = collision_data.closest_p1; f32 delta = new_impulse - old_impulse;
contact->solved_normal_impulse = new_impulse;
// Vec2 sep = SubVec2(shape1_pt, shape0_pt); Vec2 impulse = MulVec2(normal, delta);
// f32 sep_along_normal = DotVec2(sep, normal); 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;
}
// f32 rel_vel_along_normal = DotVec2(rel_vel, normal); // 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;
// // f32 sep_normal = DotVec2(sep, normal); Vec2 vel0 = AddVec2(v0, MulPerpVec2(vcp0, w0));
Vec2 vel1 = AddVec2(v1, MulPerpVec2(vcp1, w1));
Vec2 vrel = SubVec2(vel0, vel1);
// S_DebugDrawPoint(shape0_pt, Color_Cyan); f32 k = contact->inv_tangent_mass;
// S_DebugDrawPoint(shape1_pt, Color_Cyan);
// S_DebugDrawPoint(collision_data.collision_points[0].p, Color_Red); /* (to be applied along t) */
// S_DebugDrawPoint(collision_data.collision_points[0].p, Color_Red); f32 vt = DotVec2(vrel, tangent);
f32 j = vt * k;
// if (sep_along_normal < 0) f32 max_friction = constraint->friction * contact->solved_normal_impulse;
// { f32 old_impulse = contact->solved_tangent_impulse;
// dv0 = AddVec2(dv0, MulVec2(normal, sep_along_normal)); f32 new_impulse = ClampF32(old_impulse + j, -max_friction, max_friction);
// } f32 delta = new_impulse - old_impulse;
// // dv0 = VEC2(sep_normal, sep_normal); contact->solved_tangent_impulse = new_impulse;
// // f32 separation = constraint->sep; 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;
}
// // Update solved velocity if (ent0->valid)
// if (ent0->valid) {
// { ent0->solved_v = v0;
// ent0->solved_dv = AddVec2(ent0->solved_dv, dv0); ent0->solved_w = w0;
// ent0->solved_dw += dw0; }
// } if (ent1->valid)
// if (ent1->valid) {
// { ent1->solved_v = v1;
// ent1->solved_dv = AddVec2(ent1->solved_dv, dv1); ent1->solved_w = w1;
// ent1->solved_dw += dw1; }
// } }
// }
////////////////////////////// //////////////////////////////
//- Integrate velocities //- Integrate velocities
@ -1620,10 +2097,12 @@ void S_TickForever(WaveLaneCtx *lane)
for (S_Ent *ent = S_FirstEnt(world); ent->valid; ent = S_NextEnt(ent)) for (S_Ent *ent = S_FirstEnt(world); ent->valid; ent = S_NextEnt(ent))
{ {
Xform xf = ent->xf; Xform xf = ent->xf;
xf.og = AddVec2(xf.og, ent->solved_dv); xf.og = AddVec2(xf.og, MulVec2(ent->solved_v, solver_dt));
xf = RotateXform(xf, ent->solved_dw); xf = RotateXform(xf, ent->solved_w * solver_dt);
ent->xf = xf; ent->xf = xf;
} }
}
@ -1938,10 +2417,18 @@ void S_TickForever(WaveLaneCtx *lane)
S_DebugDrawShape(world_shape, color); S_DebugDrawShape(world_shape, color);
} }
// Draw look // Draw rot
{ {
Vec4 color = VEC4(0.8, 0.8, 0.8, 1); Vec4 color = VEC4(0.8, 0.8, 0.8, 1);
Vec2 p0 = world_shape.centroid; 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); Vec2 p1 = S_EdgePointFromShape(world_shape, ent->look);
S_DebugDrawLine(p0, p1, color); S_DebugDrawLine(p0, p1, color);
} }

38
src/pp/pp_sim/pp_sim_core.h
View File

@ -62,6 +62,7 @@ Struct(S_Ent)
b32 is_player; b32 is_player;
f32 health; f32 health;
Xform last_xf;
Xform xf; Xform xf;
Vec2 move; Vec2 move;
@ -85,8 +86,8 @@ Struct(S_Ent)
////////////////////////////// //////////////////////////////
//- Solver data //- Solver data
Vec2 solved_dv; Vec2 solved_v;
f32 solved_dw; f32 solved_w;
}; };
Struct(S_EntListNode) Struct(S_EntListNode)
@ -114,7 +115,7 @@ Struct(S_EntBin)
Struct(S_SupportPoint) Struct(S_SupportPoint)
{ {
Vec2 p; 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) Struct(S_CollisionPoint)
@ -145,7 +146,7 @@ Struct(S_ClippedLine)
Struct(S_CollisionResult) Struct(S_CollisionResult)
{ {
// Contact manifold // Collision manifold
i32 collision_points_count; i32 collision_points_count;
S_CollisionPoint collision_points[2]; S_CollisionPoint collision_points[2];
Vec2 collision_normal; Vec2 collision_normal;
@ -165,13 +166,38 @@ Struct(S_RaycastResult)
//////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////
//~ Constraint types //~ 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) Struct(S_Constraint)
{ {
i64 last_touched_tick;
S_Key ent0; S_Key ent0;
S_Key ent1; S_Key ent1;
S_Shape shape0; Vec2 static_center0;
S_Shape shape1; 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];
}; };
//////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////

6
src/pp/pp_vis/pp_vis_core.c
View File

@ -3114,8 +3114,8 @@ void V_TickForever(WaveLaneCtx *lane)
case S_DebugDrawKind_Shape: case S_DebugDrawKind_Shape:
{ {
S_Shape ui_shape = S_MulXformShape(frame->xf.world_to_ui, desc->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_Line);
V_DrawShape(ui_shape, color, detail, V_DrawFlag_None); // V_DrawShape(ui_shape, color, detail, V_DrawFlag_None);
} break; } break;
} }
} }
@ -3210,7 +3210,7 @@ void V_TickForever(WaveLaneCtx *lane)
for (S_TileKind tile_kind = 0; tile_kind < S_TileKind_COUNT; ++tile_kind) for (S_TileKind tile_kind = 0; tile_kind < S_TileKind_COUNT; ++tile_kind)
{ {
String tile_name = S_TileNameFromKind(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); ResourceKey sheet_resource = ResourceKeyFromStore(&P_Resources, sheet_name);
SPR_SheetKey sheet = SPR_SheetKeyFromResource(sheet_resource); SPR_SheetKey sheet = SPR_SheetKeyFromResource(sheet_resource);
SPR_Slice tile_slice = SPR_SliceFromSheet(sheet, Lit("")); SPR_Slice tile_slice = SPR_SliceFromSheet(sheet, Lit(""));

1
src/sprite/sprite.c
View File

@ -145,6 +145,7 @@ void SPR_TickAsync(WaveLaneCtx *lane, AsyncFrameLaneCtx *base_async_lane_frame)
cmds_count = SPR.submit.count; cmds_count = SPR.submit.count;
SPR.submit.first = 0; SPR.submit.first = 0;
SPR.submit.last = 0; SPR.submit.last = 0;
SPR.submit.count = 0;
} }
Unlock(&lock); Unlock(&lock);
} }