1383 lines
50 KiB
C
1383 lines
50 KiB
C
////////////////////////////////////////////////////////////
|
|
//~ Contact
|
|
|
|
b32 CanEntitiesContact(Entity *e0, Entity *e1)
|
|
{
|
|
b32 result = 0;
|
|
result = e0 != e1 &&
|
|
!EqId(e0->top, e1->top) &&
|
|
!(HasProp(e0, Prop_Wall) && HasProp(e1, Prop_Wall));
|
|
return result;
|
|
}
|
|
|
|
void CreateAndUpdateContacts(PhysStepCtx *ctx, f32 elapsed_dt, u64 phys_iteration)
|
|
{
|
|
__prof;
|
|
SimStepCtx *sim_step_ctx = ctx->sim_step_ctx;
|
|
Snapshot *ss = sim_step_ctx->world;
|
|
Space *space = sim_step_ctx->accel->space;
|
|
EntityId local_player = ss->local_player;
|
|
CollisionCallbackFunc *collision_callback = ctx->collision_callback;
|
|
|
|
Entity *root = EntityFromId(ss, RootEntityId);
|
|
u64 tick = ss->tick;
|
|
|
|
for (u64 check0_index = 0; check0_index < ss->num_ents_reserved; ++check0_index)
|
|
{
|
|
Entity *check0 = &ss->ents[check0_index];
|
|
if (!IsValidAndActive(check0)) continue;
|
|
if (!(HasProp(check0, Prop_Solid) || HasProp(check0, Prop_Sensor))) continue;
|
|
if (check0->local_collider.count <= 0) continue;
|
|
|
|
Xform check0_xf = XformFromEntity(check0);
|
|
CLD_Shape check0_collider = check0->local_collider;
|
|
Aabb aabb = CLD_AabbFromShape(&check0_collider, check0_xf);
|
|
|
|
SpaceIter iter = BeginSpaceIterAabb(space, aabb);
|
|
SpaceEntry *space_entry;
|
|
while ((space_entry = NextSpaceIterAabb(&iter)) != 0)
|
|
{
|
|
Entity *check1 = EntityFromId(ss, space_entry->ent);
|
|
if (!IsValidAndActive(check1)) continue;
|
|
if (!(HasProp(check1, Prop_Solid) || HasProp(check1, Prop_Sensor))) continue;
|
|
if (check1->local_collider.count <= 0) continue;
|
|
if (!CanEntitiesContact(check0, check1)) continue;
|
|
|
|
/* Deterministic order based on entity id */
|
|
Entity *e0;
|
|
Entity *e1;
|
|
Xform e0_xf;
|
|
Xform e1_xf;
|
|
CLD_Shape e0_collider;
|
|
CLD_Shape e1_collider;
|
|
if (check0->id.uid.hi < check1->id.uid.hi)
|
|
{
|
|
e0 = check0;
|
|
e1 = check1;
|
|
e0_xf = check0_xf;
|
|
e1_xf = XformFromEntity(check1);
|
|
e0_collider = check0_collider;
|
|
e1_collider = check1->local_collider;
|
|
}
|
|
else
|
|
{
|
|
e0 = check1;
|
|
e1 = check0;
|
|
e0_xf = XformFromEntity(check1);
|
|
e1_xf = check0_xf;
|
|
e0_collider = check1->local_collider;
|
|
e1_collider = check0_collider;
|
|
}
|
|
|
|
EntityId constraint_id = ContactConstraintIdFromContactingIds(local_player, e0->id, e1->id);
|
|
Entity *constraint_ent = EntityFromId(ss, constraint_id);
|
|
|
|
if (constraint_ent->valid)
|
|
{
|
|
if (constraint_ent->contact_constraint_data.last_phys_iteration >= phys_iteration)
|
|
{
|
|
/* Already processed constraint this iteration */
|
|
continue;
|
|
}
|
|
else
|
|
{
|
|
constraint_ent->contact_constraint_data.last_phys_iteration = phys_iteration;
|
|
}
|
|
}
|
|
|
|
/* Calculate collision */
|
|
CLD_CollisionData collision_result = CLD_CollisionDataFromShapes(&e0_collider, &e1_collider, e0_xf, e1_xf);
|
|
|
|
/* Parts of algorithm are hard-coded to support 2 contact points */
|
|
StaticAssert(countof(constraint_ent->contact_constraint_data.points) == 2);
|
|
StaticAssert(countof(collision_result.points) == 2);
|
|
|
|
ContactConstraint *constraint = 0;
|
|
if (collision_result.num_points > 0)
|
|
{
|
|
b32 is_start = 0;
|
|
if (!constraint_ent->valid)
|
|
{
|
|
is_start = 1;
|
|
/* Create constraint */
|
|
constraint_ent = AcquireLocalWithId(root, constraint_id);
|
|
constraint_ent->contact_constraint_data.e0 = e0->id;
|
|
constraint_ent->contact_constraint_data.e1 = e1->id;
|
|
/* Both entities must be solid and one must be dynamic for a solve to be necessary. */
|
|
constraint_ent->contact_constraint_data.skip_solve = !HasProp(e0, Prop_Solid) || !HasProp(e1, Prop_Solid) ||
|
|
!(HasProp(e0, Prop_Dynamic) || HasProp(e1, Prop_Dynamic));
|
|
EnableProp(constraint_ent, Prop_Active);
|
|
|
|
/* TODO: Should we recalculate normal as more contact points are added? */
|
|
EnableProp(constraint_ent, Prop_ContactConstraint);
|
|
Activate(constraint_ent, tick);
|
|
}
|
|
constraint = &constraint_ent->contact_constraint_data;
|
|
constraint->normal = collision_result.normal;
|
|
constraint->friction = SqrtF32(e0->friction * e1->friction);
|
|
|
|
/* Delete old contacts that are no longer present */
|
|
for (u32 i = 0; i < constraint->num_points; ++i)
|
|
{
|
|
ContactPoint *old = &constraint->points[i];
|
|
u32 id = old->id;
|
|
b32 found = 0;
|
|
for (u32 j = 0; j < collision_result.num_points; ++j)
|
|
{
|
|
if (collision_result.points[j].id == id)
|
|
{
|
|
found = 1;
|
|
break;
|
|
}
|
|
}
|
|
if (!found)
|
|
{
|
|
/* Delete contact by replacing with last in array */
|
|
*old = constraint->points[--constraint->num_points];
|
|
--i;
|
|
}
|
|
}
|
|
|
|
/* Update / insert returned contacts */
|
|
for (u32 i = 0; i < collision_result.num_points; ++i)
|
|
{
|
|
CLD_CollisionPoint *res_point = &collision_result.points[i];
|
|
Vec2 point = res_point->point;
|
|
f32 sep = res_point->separation;
|
|
u32 id = res_point->id;
|
|
ContactPoint *contact = 0;
|
|
/* Match */
|
|
for (u32 j = 0; j < constraint->num_points; ++j)
|
|
{
|
|
ContactPoint *t = &constraint->points[j];
|
|
if (t->id == id)
|
|
{
|
|
contact = t;
|
|
break;
|
|
}
|
|
}
|
|
if (!contact)
|
|
{
|
|
/* Insert */
|
|
contact = &constraint->points[constraint->num_points++];
|
|
ZeroStruct(contact);
|
|
contact->id = id;
|
|
constraint->pushout_velocity = 3.0f;
|
|
}
|
|
|
|
/* Update points & separation */
|
|
contact->vcp0 = SubVec2(point, e0_xf.og);
|
|
contact->vcp1 = SubVec2(point, e1_xf.og);
|
|
contact->starting_separation = sep;
|
|
|
|
#if DeveloperIsEnabled
|
|
contact->dbg_pt = point;
|
|
#endif
|
|
}
|
|
|
|
/* Skip solve based on collision direction */
|
|
{
|
|
Vec2 normal = collision_result.normal;
|
|
Vec2 dir0 = e0->collision_dir;
|
|
Vec2 dir1 = e1->collision_dir;
|
|
f32 threshold = 0.5;
|
|
b32 is_wrong_dir = 0;
|
|
if (!IsVec2Zero(dir0))
|
|
{
|
|
is_wrong_dir = DotVec2(dir0, normal) <= threshold;
|
|
}
|
|
if (!IsVec2Zero(dir1) && !is_wrong_dir)
|
|
{
|
|
is_wrong_dir = DotVec2(dir1, NegVec2(normal)) <= threshold;
|
|
}
|
|
constraint->wrong_dir = is_wrong_dir;
|
|
}
|
|
|
|
/* Run collision callback */
|
|
if (collision_callback)
|
|
{
|
|
CollisionData data = ZI;
|
|
data.e0 = e0->id;
|
|
data.e1 = e1->id;
|
|
data.normal = collision_result.normal;
|
|
data.is_start = is_start;
|
|
data.dt = elapsed_dt;
|
|
|
|
/* Calculate point */
|
|
Vec2 midpoint = collision_result.points[0].point;
|
|
if (collision_result.num_points > 1)
|
|
{
|
|
midpoint = AddVec2(midpoint, MulVec2(SubVec2(collision_result.points[1].point, midpoint), 0.5f));
|
|
}
|
|
data.point = midpoint;
|
|
|
|
/* Calculate relative velocity */
|
|
Vec2 vrel;
|
|
{
|
|
Vec2 v0 = e0->linear_velocity;
|
|
Vec2 v1 = e1->linear_velocity;
|
|
f32 w0 = e0->angular_velocity;
|
|
f32 w1 = e1->angular_velocity;
|
|
Vec2 vcp1 = SubVec2(midpoint, e1_xf.og);
|
|
Vec2 vcp0 = SubVec2(midpoint, e0_xf.og);
|
|
Vec2 vel0 = AddVec2(v0, MulPerpVec2(vcp0, w0));
|
|
Vec2 vel1 = AddVec2(v1, MulPerpVec2(vcp1, w1));
|
|
vrel = SubVec2(vel0, vel1);
|
|
}
|
|
data.vrel = vrel;
|
|
|
|
/* Collision data from e1's perspective */
|
|
CollisionData data_inverted = data;
|
|
data_inverted.e0 = data.e1;
|
|
data_inverted.e1 = data.e0;
|
|
data_inverted.normal = NegVec2(data.normal);
|
|
data_inverted.vrel = NegVec2(data.vrel);
|
|
|
|
/* Run callback twice for both e0 & e1 */
|
|
b32 skip_solve0 = collision_callback(&data, sim_step_ctx);
|
|
b32 skip_solve1 = collision_callback(&data_inverted, sim_step_ctx);
|
|
if (skip_solve0 || skip_solve1)
|
|
{
|
|
constraint->skip_solve = 1;
|
|
}
|
|
}
|
|
}
|
|
else if (constraint_ent->valid)
|
|
{
|
|
constraint_ent->contact_constraint_data.num_points = 0;
|
|
}
|
|
|
|
/* TODO: Remove this (debugging) */
|
|
#if COLLIDER_DEBUG && COLLIDER_DEBUG_DETAILED
|
|
{
|
|
EntityId dbg_ent_id = CollisionDebugIdFromIds(local_player, e0->id, e1->id);
|
|
Entity *dbg_ent = EntityFromId(ss, dbg_ent_id);
|
|
if (!dbg_ent->valid)
|
|
{
|
|
/* FIXME: Entity never released */
|
|
dbg_ent = AcquireLocalWithId(root, dbg_ent_id);
|
|
EnableProp(dbg_ent, Prop_CollisionDebug);
|
|
}
|
|
|
|
ContactDebugData *dbg = &dbg_ent->collision_debug_data;
|
|
|
|
dbg->e0 = e0->id;
|
|
dbg->e1 = e1->id;
|
|
dbg->collision_result = collision_result;
|
|
|
|
if (constraint)
|
|
{
|
|
CopyBytes(dbg->points, constraint->points, sizeof(dbg->points));
|
|
dbg->num_points = constraint->num_points;
|
|
}
|
|
else
|
|
{
|
|
dbg->num_points = 0;
|
|
}
|
|
|
|
dbg->xf0 = e0_xf;
|
|
dbg->xf1 = e1_xf;
|
|
|
|
/* Update closest points */
|
|
{
|
|
CLD_ClosestPointData closest_points = CLD_ClosestPointDataFromShapes(&e0_collider, &e1_collider, e0_xf, e1_xf);
|
|
dbg->closest0 = closest_points.p0;
|
|
dbg->closest1 = closest_points.p1;
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
EndSpaceIter(&iter);
|
|
}
|
|
}
|
|
|
|
void PrepareContacts(PhysStepCtx *ctx, u64 phys_iteration)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *constraint_ent = &ss->ents[sim_ent_index];
|
|
if (!ShouldSimulate(constraint_ent)) continue;
|
|
if (!HasProp(constraint_ent, Prop_ContactConstraint)) continue;
|
|
|
|
ContactConstraint *constraint = &constraint_ent->contact_constraint_data;
|
|
|
|
u32 num_points = constraint->num_points;
|
|
Entity *e0 = EntityFromId(ss, constraint->e0);
|
|
Entity *e1 = EntityFromId(ss, constraint->e1);
|
|
if (constraint->last_phys_iteration >= phys_iteration && num_points > 0 && IsValidAndActive(e0) && IsValidAndActive(e1))
|
|
{
|
|
Vec2 normal = constraint->normal;
|
|
Vec2 tangent = PerpVec2(normal);
|
|
|
|
Xform e0_xf = XformFromEntity(e0);
|
|
Xform e1_xf = XformFromEntity(e1);
|
|
|
|
/* TODO: Cache this */
|
|
/* Calculate masses */
|
|
f32 inv_m0 = 0;
|
|
f32 inv_m1 = 0;
|
|
f32 inv_i0 = 0;
|
|
f32 inv_i1 = 0;
|
|
{
|
|
/* If not simulated locally or ent is not dynamic, pretend contact mass is infinite */
|
|
if (ShouldSimulate(e0) && HasProp(e0, Prop_Dynamic))
|
|
{
|
|
f32 scale = AbsF32(DeterminantFromXform(e0_xf));
|
|
f32 scaled_mass = e0->mass_unscaled * scale;
|
|
f32 scaled_inertia = e0->inertia_unscaled * scale;
|
|
inv_m0 = 1.f / scaled_mass;
|
|
inv_i0 = 1.f / scaled_inertia;
|
|
}
|
|
if (ShouldSimulate(e1) && HasProp(e1, Prop_Dynamic))
|
|
{
|
|
f32 scale = AbsF32(DeterminantFromXform(e1_xf));
|
|
f32 scaled_mass = e1->mass_unscaled * scale;
|
|
f32 scaled_inertia = e1->inertia_unscaled * scale;
|
|
inv_m1 = 1.f / scaled_mass;
|
|
inv_i1 = 1.f / scaled_inertia;
|
|
}
|
|
|
|
}
|
|
constraint->inv_m0 = inv_m0;
|
|
constraint->inv_m1 = inv_m1;
|
|
constraint->inv_i0 = inv_i0;
|
|
constraint->inv_i1 = inv_i1;
|
|
|
|
/* Update / insert returned contacts */
|
|
for (u32 i = 0; i < num_points; ++i)
|
|
{
|
|
ContactPoint *contact = &constraint->points[i];
|
|
Vec2 vcp0 = contact->vcp0;
|
|
Vec2 vcp1 = contact->vcp1;
|
|
|
|
/* 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;
|
|
}
|
|
|
|
/* 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;
|
|
}
|
|
|
|
#if !SIM_PHYSICS_ENABLE_WARM_STARTING
|
|
contact->normal_impulse = 0;
|
|
contact->tangent_impulse = 0;
|
|
#endif
|
|
}
|
|
}
|
|
else
|
|
{
|
|
/* Mark constraint for removal */
|
|
constraint_ent->contact_constraint_data.num_points = 0;
|
|
DisableProp(constraint_ent, Prop_Active);
|
|
EnableProp(constraint_ent, Prop_Release);
|
|
}
|
|
}
|
|
|
|
#if 0
|
|
#if COLLIDER_DEBUG
|
|
/* Remove collision debug ents */
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *dbg_ent = &ss->ents[sim_ent_index];
|
|
if (!ShouldSimulate(dbg_ent)) continue;
|
|
if (!HasProp(dbg_ent, Prop_CollisionDebug)) continue;
|
|
|
|
ContactDebugData *dbg = &dbg_ent->collision_debug_data;
|
|
Entity *e0 = EntityFromId(ss, dbg->e0);
|
|
Entity *e1 = EntityFromId(ss, dbg->e1);
|
|
|
|
if (!(ShouldSimulate(e0) && ShouldSimulate(e1)) ||
|
|
!(HasProp(e0, Prop_Solid) || HasProp(e0, Prop_Sensor)) ||
|
|
!(HasProp(e1, Prop_Solid) || HasProp(e1, Prop_Sensor)))
|
|
{
|
|
/* Mark dbg ent for removal */
|
|
DisableProp(dbg_ent, Prop_Active);
|
|
EnableProp(dbg_ent, Prop_Release);
|
|
}
|
|
}
|
|
#endif
|
|
#endif
|
|
}
|
|
|
|
void WarmStartContacts(PhysStepCtx *ctx)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *constraint_ent = &ss->ents[sim_ent_index];
|
|
if (!ShouldSimulate(constraint_ent)) continue;
|
|
if (!HasProp(constraint_ent, Prop_ContactConstraint)) continue;
|
|
|
|
ContactConstraint *constraint = &constraint_ent->contact_constraint_data;
|
|
|
|
u32 num_points = constraint->num_points;
|
|
Entity *e0 = EntityFromId(ss, constraint->e0);
|
|
Entity *e1 = EntityFromId(ss, constraint->e1);
|
|
|
|
if (num_points > 0 && IsValidAndActive(e0) && IsValidAndActive(e1) && !constraint->skip_solve && !constraint->wrong_dir)
|
|
{
|
|
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 = e0->linear_velocity;
|
|
Vec2 v1 = e1->linear_velocity;
|
|
f32 w0 = e0->angular_velocity;
|
|
f32 w1 = e1->angular_velocity;
|
|
|
|
/* Warm start */
|
|
Vec2 normal = constraint->normal;
|
|
Vec2 tangent = PerpVec2(normal);
|
|
f32 inv_num_points = 1.f / num_points;
|
|
for (u32 i = 0; i < num_points; ++i)
|
|
{
|
|
ContactPoint *point = &constraint->points[i];
|
|
Vec2 vcp0 = point->vcp0;
|
|
Vec2 vcp1 = point->vcp1;
|
|
|
|
Vec2 impulse = AddVec2(MulVec2(normal, point->normal_impulse), MulVec2(tangent, point->tangent_impulse));
|
|
impulse = MulVec2(impulse, inv_num_points);
|
|
|
|
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;
|
|
}
|
|
|
|
SetLinearVelocity(e0, v0);
|
|
SetAngularVelocity(e0, w0);
|
|
SetLinearVelocity(e1, v1);
|
|
SetAngularVelocity(e1, w1);
|
|
}
|
|
}
|
|
}
|
|
|
|
void SolveContacts(PhysStepCtx *ctx, f32 dt, b32 apply_bias)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *constraint_ent = &ss->ents[sim_ent_index];
|
|
if (!ShouldSimulate(constraint_ent)) continue;
|
|
if (!HasProp(constraint_ent, Prop_ContactConstraint)) continue;
|
|
|
|
ContactConstraint *constraint = &constraint_ent->contact_constraint_data;
|
|
|
|
Entity *e0 = EntityFromId(ss, constraint->e0);
|
|
Entity *e1 = EntityFromId(ss, constraint->e1);
|
|
|
|
Vec2 v0 = e0->linear_velocity;
|
|
Vec2 v1 = e1->linear_velocity;
|
|
f32 w0 = e0->angular_velocity;
|
|
f32 w1 = e1->angular_velocity;
|
|
|
|
u32 num_points = constraint->num_points;
|
|
if (num_points > 0 && IsValidAndActive(e0) && IsValidAndActive(e1) && !constraint->skip_solve && !constraint->wrong_dir)
|
|
{
|
|
Xform e0_xf = XformFromEntity(e0);
|
|
Xform e1_xf = XformFromEntity(e1);
|
|
|
|
f32 inv_m0 = constraint->inv_m0;
|
|
f32 inv_m1 = constraint->inv_m1;
|
|
f32 inv_i0 = constraint->inv_i0;
|
|
f32 inv_i1 = constraint->inv_i1;
|
|
|
|
/* Normal impulse */
|
|
Vec2 normal = constraint->normal;
|
|
for (u32 point_index = 0; point_index < num_points; ++point_index)
|
|
{
|
|
ContactPoint *point = &constraint->points[point_index];
|
|
Vec2 vcp0 = point->vcp0;
|
|
Vec2 vcp1 = point->vcp1;
|
|
Vec2 p0 = AddVec2(e0_xf.og, vcp0);
|
|
Vec2 p1 = AddVec2(e1_xf.og, vcp1);
|
|
|
|
/* FIXME: Should separation use the rotated contact points? */
|
|
f32 separation = DotVec2(SubVec2(p1, p0), normal) + point->starting_separation;
|
|
|
|
f32 velocity_bias = 0.0f;
|
|
f32 mass_scale = 1.0f;
|
|
f32 impulse_scale = 0.0f;
|
|
|
|
if (separation > 0.0f)
|
|
{
|
|
/* Speculative */
|
|
velocity_bias = separation / dt;
|
|
}
|
|
else if (apply_bias)
|
|
{
|
|
/* Soft constraint */
|
|
SoftSpring softness = MakeSpring(ContactSpringHz, ContactSpringDamp, dt);
|
|
f32 pushout_velocity = constraint->pushout_velocity;
|
|
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 = point->inv_normal_mass;
|
|
|
|
/* (to be applied along n) */
|
|
f32 vn = DotVec2(vrel, normal);
|
|
f32 j = ((k * mass_scale) * (vn - velocity_bias)) - (point->normal_impulse * impulse_scale);
|
|
|
|
f32 old_impulse = point->normal_impulse;
|
|
f32 new_impulse = MaxF32(old_impulse + j, 0);
|
|
f32 delta = new_impulse - old_impulse;
|
|
point->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 (u32 point_index = 0; point_index < num_points; ++point_index)
|
|
{
|
|
ContactPoint *point = &constraint->points[point_index];
|
|
Vec2 vcp0 = point->vcp0;
|
|
Vec2 vcp1 = point->vcp1;
|
|
|
|
Vec2 vel0 = AddVec2(v0, MulPerpVec2(vcp0, w0));
|
|
Vec2 vel1 = AddVec2(v1, MulPerpVec2(vcp1, w1));
|
|
Vec2 vrel = SubVec2(vel0, vel1);
|
|
|
|
f32 k = point->inv_tangent_mass;
|
|
|
|
/* (to be applied along t) */
|
|
f32 vt = DotVec2(vrel, tangent);
|
|
f32 j = vt * k;
|
|
|
|
f32 max_friction = constraint->friction * point->normal_impulse;
|
|
f32 old_impulse = point->tangent_impulse;
|
|
f32 new_impulse = ClampF32(old_impulse + j, -max_friction, max_friction);
|
|
f32 delta = new_impulse - old_impulse;
|
|
point->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;
|
|
}
|
|
|
|
SetLinearVelocity(e0, v0);
|
|
SetAngularVelocity(e0, w0);
|
|
SetLinearVelocity(e1, v1);
|
|
SetAngularVelocity(e1, w1);
|
|
}
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////
|
|
//~ Motor joint
|
|
|
|
MotorJointDesc CreateMotorJointDef(void)
|
|
{
|
|
MotorJointDesc def = ZI;
|
|
return def;
|
|
}
|
|
|
|
MotorJoint MotorJointFromDef(MotorJointDesc def)
|
|
{
|
|
MotorJoint result = ZI;
|
|
result.e0 = def.e0;
|
|
result.e1 = def.e1;
|
|
result.correction_rate = ClampF32(def.correction_rate, 0, 1);
|
|
result.max_force = def.max_force;
|
|
result.max_torque = def.max_torque;
|
|
return result;
|
|
}
|
|
|
|
void PrepareMotorJoints(PhysStepCtx *ctx)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!ShouldSimulate(joint_ent)) continue;
|
|
if (!HasProp(joint_ent, Prop_MotorJoint)) continue;
|
|
|
|
MotorJoint *joint = &joint_ent->motor_joint_data;
|
|
|
|
Entity *e0 = EntityFromId(ss, joint->e0);
|
|
Entity *e1 = EntityFromId(ss, joint->e1);
|
|
|
|
if (ShouldSimulate(e0) && ShouldSimulate(e1))
|
|
{
|
|
Xform e0_xf = XformFromEntity(e0);
|
|
Xform e1_xf = XformFromEntity(e1);
|
|
|
|
/* TODO: Cache this */
|
|
/* Calculate masses */
|
|
f32 inv_m0;
|
|
f32 inv_m1;
|
|
f32 inv_i0;
|
|
f32 inv_i1;
|
|
{
|
|
f32 scale0 = AbsF32(DeterminantFromXform(e0_xf));
|
|
f32 scale1 = AbsF32(DeterminantFromXform(e1_xf));
|
|
inv_m0 = 1.f / (e0->mass_unscaled * scale0);
|
|
inv_m1 = 1.f / (e1->mass_unscaled * scale1);
|
|
inv_i0 = 1.f / (e0->inertia_unscaled * scale0);
|
|
inv_i1 = 1.f / (e1->inertia_unscaled * scale1);
|
|
}
|
|
joint->inv_m0 = inv_m0;
|
|
joint->inv_m1 = inv_m1;
|
|
joint->inv_i0 = inv_i0;
|
|
joint->inv_i1 = inv_i1;
|
|
|
|
joint->point_local_e0 = VEC2(0, 0);
|
|
joint->point_local_e1 = VEC2(0, 0);
|
|
|
|
Vec2 vcp0 = SubVec2(MulXformV2(e0_xf, joint->point_local_e0), e0_xf.og);
|
|
Vec2 vcp1 = SubVec2(MulXformV2(e1_xf, joint->point_local_e1), e1_xf.og);
|
|
|
|
Xform linear_mass_xf = ZI;
|
|
linear_mass_xf.bx.x = inv_m0 + inv_m1 + vcp0.y * vcp0.y * inv_i0 + vcp1.y * vcp1.y * inv_i1;
|
|
linear_mass_xf.bx.y = -vcp0.y * vcp0.x * inv_i0 - vcp1.y * vcp1.x * inv_i1;
|
|
linear_mass_xf.by.x = linear_mass_xf.bx.y;
|
|
linear_mass_xf.by.y = inv_m0 + inv_m1 + vcp0.x * vcp0.x * inv_i0 + vcp1.x * vcp1.x * inv_i1;
|
|
joint->linear_mass_xf = InvertXform(linear_mass_xf);
|
|
|
|
joint->angular_mass = 1.f / (inv_i0 + inv_i1);
|
|
|
|
#if !SIM_PHYSICS_ENABLE_WARM_STARTING
|
|
joint->linear_impulse = VEC2(0, 0);
|
|
joint->angular_impulse = 0;
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
/* Mark joint for removal */
|
|
DisableProp(joint_ent, Prop_Active);
|
|
EnableProp(joint_ent, Prop_Release);
|
|
}
|
|
}
|
|
}
|
|
|
|
void WarmStartMotorJoints(PhysStepCtx *ctx)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!ShouldSimulate(joint_ent)) continue;
|
|
if (!HasProp(joint_ent, Prop_MotorJoint)) continue;
|
|
|
|
MotorJoint *joint = &joint_ent->motor_joint_data;
|
|
|
|
Entity *e0 = EntityFromId(ss, joint->e0);
|
|
Entity *e1 = EntityFromId(ss, joint->e1);
|
|
|
|
Xform e0_xf = XformFromEntity(e0);
|
|
Xform e1_xf = XformFromEntity(e1);
|
|
|
|
f32 inv_m0 = joint->inv_m0;
|
|
f32 inv_m1 = joint->inv_m1;
|
|
f32 inv_i0 = joint->inv_i0;
|
|
f32 inv_i1 = joint->inv_i1;
|
|
|
|
Vec2 vcp0 = SubVec2(MulXformV2(e0_xf, joint->point_local_e0), e0_xf.og);
|
|
Vec2 vcp1 = SubVec2(MulXformV2(e1_xf, joint->point_local_e1), e1_xf.og);
|
|
|
|
SetLinearVelocity(e0, SubVec2(e0->linear_velocity, MulVec2(joint->linear_impulse, inv_m0)));
|
|
SetLinearVelocity(e1, AddVec2(e1->linear_velocity, MulVec2(joint->linear_impulse, inv_m1)));
|
|
e0->angular_velocity -= (WedgeVec2(vcp0, joint->linear_impulse) + joint->angular_impulse) * inv_i0;
|
|
e1->angular_velocity += (WedgeVec2(vcp1, joint->linear_impulse) + joint->angular_impulse) * inv_i1;
|
|
}
|
|
}
|
|
|
|
void SolveMotorJoints(PhysStepCtx *ctx, f32 dt)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!ShouldSimulate(joint_ent)) continue;
|
|
if (!HasProp(joint_ent, Prop_MotorJoint)) continue;
|
|
|
|
MotorJoint *joint = &joint_ent->motor_joint_data;
|
|
|
|
Entity *e0 = EntityFromId(ss, joint->e0);
|
|
Entity *e1 = EntityFromId(ss, joint->e1);
|
|
|
|
Xform e0_xf = XformFromEntity(e0);
|
|
Xform e1_xf = XformFromEntity(e1);
|
|
|
|
f32 inv_m0 = joint->inv_m0;
|
|
f32 inv_m1 = joint->inv_m1;
|
|
f32 inv_i0 = joint->inv_i0;
|
|
f32 inv_i1 = joint->inv_i1;
|
|
|
|
Vec2 v0 = e0->linear_velocity;
|
|
Vec2 v1 = e1->linear_velocity;
|
|
f32 w0 = e0->angular_velocity;
|
|
f32 w1 = e1->angular_velocity;
|
|
|
|
f32 correction_rate = joint->correction_rate;
|
|
f32 inv_dt = 1.f / dt;
|
|
|
|
/* Angular constraint */
|
|
{
|
|
f32 max_impulse = joint->max_torque * dt;
|
|
f32 angular_separation = UnwindAngleF32(RotationFromXform(e1_xf) - RotationFromXform(e0_xf));
|
|
|
|
f32 angular_bias = angular_separation * correction_rate * inv_dt;
|
|
|
|
f32 old_impulse = joint->angular_impulse;
|
|
f32 new_impulse = ClampF32(old_impulse + (-joint->angular_mass * (w1 - w0 + angular_bias)), -max_impulse, max_impulse);
|
|
joint->angular_impulse = new_impulse;
|
|
|
|
f32 delta = new_impulse - old_impulse;
|
|
w0 -= delta * inv_i0;
|
|
w1 += delta * inv_i1;
|
|
}
|
|
|
|
/* Linear constraint */
|
|
{
|
|
Vec2 vcp0 = SubVec2(MulXformV2(e0_xf, joint->point_local_e0), e0_xf.og);
|
|
Vec2 vcp1 = SubVec2(MulXformV2(e1_xf, joint->point_local_e1), e1_xf.og);
|
|
|
|
f32 max_impulse = joint->max_force * dt;
|
|
|
|
Vec2 linear_separation = SubVec2(AddVec2(e1_xf.og, vcp1), AddVec2(e0_xf.og, vcp0));
|
|
Vec2 linear_bias = MulVec2(linear_separation, correction_rate * inv_dt);
|
|
|
|
Vec2 vrel = SubVec2(AddVec2(v1, MulPerpVec2(vcp1, w1)), AddVec2(v0, MulPerpVec2(vcp0, w0)));
|
|
Vec2 impulse = NegVec2(MulXformBasisV2(joint->linear_mass_xf, AddVec2(vrel, linear_bias)));
|
|
|
|
Vec2 old_impulse = joint->linear_impulse;
|
|
Vec2 new_impulse = ClampVec2Len(AddVec2(old_impulse, impulse), max_impulse);
|
|
joint->linear_impulse = new_impulse;
|
|
|
|
Vec2 delta = SubVec2(new_impulse, old_impulse);
|
|
v0 = SubVec2(v0, MulVec2(delta, inv_m0));
|
|
v1 = AddVec2(v1, MulVec2(delta, inv_m1));
|
|
w0 -= WedgeVec2(vcp0, delta) * inv_i0;
|
|
w1 += WedgeVec2(vcp1, delta) * inv_i1;
|
|
}
|
|
|
|
SetLinearVelocity(e0, v0);
|
|
SetAngularVelocity(e0, w0);
|
|
SetLinearVelocity(e1, v1);
|
|
SetAngularVelocity(e1, w1);
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////
|
|
//~ Mouse joint
|
|
|
|
MouseJointDesc CreateMouseJointDef(void)
|
|
{
|
|
MouseJointDesc def = ZI;
|
|
def.linear_spring_hz = 5.0f;
|
|
def.linear_spring_damp = 0.7f;
|
|
def.angular_spring_hz = 5.0f;
|
|
def.angular_spring_damp = 0.1f;
|
|
def.max_force = 1000.0f;
|
|
return def;
|
|
}
|
|
|
|
MouseJoint MouseJointFromDef(MouseJointDesc def)
|
|
{
|
|
MouseJoint result = ZI;
|
|
result.target = def.target;
|
|
result.point_local_start = def.point_local_start;
|
|
result.point_end = def.point_end;
|
|
result.linear_spring_hz = def.linear_spring_hz;
|
|
result.linear_spring_damp = def.linear_spring_damp;
|
|
result.angular_spring_hz = def.angular_spring_hz;
|
|
result.angular_spring_damp = def.angular_spring_damp;
|
|
result.max_force = def.max_force;
|
|
return result;
|
|
}
|
|
|
|
void PrepareMouseJoints(PhysStepCtx *ctx)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!ShouldSimulate(joint_ent)) continue;
|
|
if (!HasProp(joint_ent, Prop_MouseJoint)) continue;
|
|
|
|
MouseJoint *joint = &joint_ent->mouse_joint_data;
|
|
Entity *ent = EntityFromId(ss, joint->target);
|
|
if (ShouldSimulate(ent))
|
|
{
|
|
Xform xf = XformFromEntity(ent);
|
|
|
|
/* TODO: Cache this */
|
|
/* Calculate masses */
|
|
f32 inv_m;
|
|
f32 inv_i;
|
|
{
|
|
f32 scale = AbsF32(DeterminantFromXform(xf));
|
|
inv_m = 1.f / (ent->mass_unscaled * scale);
|
|
inv_i = 1.f / (ent->inertia_unscaled * scale);
|
|
}
|
|
joint->inv_m = inv_m;
|
|
joint->inv_i = inv_i;
|
|
|
|
Vec2 vcp = SubVec2(MulXformV2(xf, joint->point_local_start), xf.og);
|
|
|
|
Xform linear_mass_xf = ZI;
|
|
linear_mass_xf.bx.x = inv_m + inv_i * vcp.y * vcp.y;
|
|
linear_mass_xf.bx.y = -inv_i * vcp.x * vcp.y;
|
|
linear_mass_xf.by.x = linear_mass_xf.bx.y;
|
|
linear_mass_xf.by.y = inv_m + inv_i * vcp.x * vcp.x;
|
|
joint->linear_mass_xf = InvertXform(linear_mass_xf);
|
|
|
|
#if !SIM_PHYSICS_ENABLE_WARM_STARTING
|
|
joint->linear_impulse = VEC2(0, 0);
|
|
joint->angular_impulse = 0;
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
/* Mark joint for removal */
|
|
DisableProp(joint_ent, Prop_Active);
|
|
EnableProp(joint_ent, Prop_Release);
|
|
}
|
|
}
|
|
}
|
|
|
|
void WarmStartMouseJoints(PhysStepCtx *ctx)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!ShouldSimulate(joint_ent)) continue;
|
|
if (!HasProp(joint_ent, Prop_MouseJoint)) continue;
|
|
|
|
MouseJoint *joint = &joint_ent->mouse_joint_data;
|
|
Entity *ent = EntityFromId(ss, joint->target);
|
|
if (ShouldSimulate(ent))
|
|
{
|
|
f32 inv_m = joint->inv_m;
|
|
f32 inv_i = joint->inv_i;
|
|
Xform xf = XformFromEntity(ent);
|
|
Vec2 vcp = SubVec2(MulXformV2(xf, joint->point_local_start), xf.og);
|
|
SetLinearVelocity(ent, AddVec2(ent->linear_velocity, MulVec2(joint->linear_impulse, inv_m)));
|
|
SetAngularVelocity(ent, ent->angular_velocity + ((WedgeVec2(vcp, joint->linear_impulse) + joint->angular_impulse) * inv_i));
|
|
}
|
|
}
|
|
}
|
|
|
|
void SolveMouseJoints(PhysStepCtx *ctx, f32 dt)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!ShouldSimulate(joint_ent)) continue;
|
|
if (!HasProp(joint_ent, Prop_MouseJoint)) continue;
|
|
|
|
MouseJoint *joint = &joint_ent->mouse_joint_data;
|
|
Entity *ent = EntityFromId(ss, joint->target);
|
|
if (ShouldSimulate(ent))
|
|
{
|
|
Vec2 v = ent->linear_velocity;
|
|
f32 w = ent->angular_velocity;
|
|
|
|
f32 inv_m = joint->inv_m;
|
|
f32 inv_i = joint->inv_i;
|
|
|
|
/* Angular impulse */
|
|
{
|
|
SoftSpring softness = MakeSpring(joint->angular_spring_hz, joint->angular_spring_damp, dt);
|
|
f32 mass_scale = softness.mass_scale;
|
|
f32 impulse_scale = softness.impulse_scale;
|
|
f32 impulse = mass_scale * (-w / inv_i) - impulse_scale * joint->angular_impulse;
|
|
joint->angular_impulse += impulse;
|
|
w += impulse * inv_i;
|
|
}
|
|
|
|
/* Linear impulse */
|
|
{
|
|
f32 max_impulse = joint->max_force / dt;
|
|
|
|
Xform xf = XformFromEntity(ent);
|
|
|
|
Vec2 point_start = MulXformV2(xf, joint->point_local_start);
|
|
Vec2 point_end = joint->point_end;
|
|
|
|
Vec2 vcp = SubVec2(point_start, xf.og);
|
|
Vec2 separation = SubVec2(point_start, point_end);
|
|
|
|
SoftSpring softness = MakeSpring(joint->linear_spring_hz, joint->linear_spring_damp, dt);
|
|
f32 bias_rate = softness.bias_rate;
|
|
f32 mass_scale = softness.mass_scale;
|
|
f32 impulse_scale = softness.impulse_scale;
|
|
|
|
Vec2 bias = MulVec2(separation, bias_rate);
|
|
Vec2 vel = AddVec2(v, MulPerpVec2(vcp, w));
|
|
Vec2 b = MulXformBasisV2(joint->linear_mass_xf, AddVec2(vel, bias));
|
|
|
|
Vec2 impulse = MulVec2(b, -mass_scale);
|
|
impulse = SubVec2(impulse, MulVec2(joint->linear_impulse, impulse_scale));
|
|
|
|
Vec2 old_impulse = joint->linear_impulse;
|
|
joint->linear_impulse = AddVec2(joint->linear_impulse, impulse);
|
|
|
|
joint->linear_impulse = ClampVec2Len(joint->linear_impulse, max_impulse);
|
|
|
|
impulse = SubVec2(joint->linear_impulse, old_impulse);
|
|
|
|
v = AddVec2(v, MulVec2(impulse, inv_m));
|
|
w += WedgeVec2(vcp, impulse) * inv_i;
|
|
}
|
|
|
|
SetLinearVelocity(ent, v);
|
|
SetAngularVelocity(ent, w);
|
|
}
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////
|
|
//~ Weld joint
|
|
|
|
WeldJointDesc CreateWeldJointDef(void)
|
|
{
|
|
WeldJointDesc def = ZI;
|
|
def.linear_spring_hz = 50;
|
|
def.linear_spring_damp = 0;
|
|
def.angular_spring_hz = 50;
|
|
def.angular_spring_damp = 0;
|
|
return def;
|
|
}
|
|
|
|
WeldJoint WeldJointFromDef(WeldJointDesc def)
|
|
{
|
|
WeldJoint result = ZI;
|
|
result.e0 = def.e0;
|
|
result.e1 = def.e1;
|
|
result.linear_spring_hz = def.linear_spring_hz;
|
|
result.linear_spring_damp = def.linear_spring_damp;
|
|
result.angular_spring_hz = def.angular_spring_hz;
|
|
result.angular_spring_damp = def.angular_spring_damp;
|
|
result.xf0_to_xf1 = def.xf;
|
|
return result;
|
|
}
|
|
|
|
void PrepareWeldJoints(PhysStepCtx *ctx)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!ShouldSimulate(joint_ent)) continue;
|
|
if (!HasProp(joint_ent, Prop_WeldJoint)) continue;
|
|
|
|
/* TODO: Lookup and disable collision for any contacts between e0 & e1? */
|
|
|
|
WeldJoint *joint = &joint_ent->weld_joint_data;
|
|
Entity *e0 = EntityFromId(ss, joint->e0);
|
|
Entity *e1 = EntityFromId(ss, joint->e1);
|
|
if (ShouldSimulate(e0) && ShouldSimulate(e1))
|
|
{
|
|
Xform e0_xf = XformFromEntity(e0);
|
|
Xform e1_xf = XformFromEntity(e1);
|
|
|
|
f32 inv_m0;
|
|
f32 inv_m1;
|
|
f32 inv_i0;
|
|
f32 inv_i1;
|
|
{
|
|
f32 scale0 = AbsF32(DeterminantFromXform(e0_xf));
|
|
f32 scale1 = AbsF32(DeterminantFromXform(e1_xf));
|
|
inv_m0 = 1.f / (e0->mass_unscaled * scale0);
|
|
inv_m1 = 1.f / (e1->mass_unscaled * scale1);
|
|
inv_i0 = 1.f / (e0->inertia_unscaled * scale0);
|
|
inv_i1 = 1.f / (e1->inertia_unscaled * scale1);
|
|
}
|
|
joint->inv_m0 = inv_m0;
|
|
joint->inv_m1 = inv_m1;
|
|
joint->inv_i0 = inv_i0;
|
|
joint->inv_i1 = inv_i1;
|
|
|
|
#if !SIM_PHYSICS_ENABLE_WARM_STARTING
|
|
joint->linear_impulse0 = VEC2(0, 0);
|
|
joint->linear_impulse1 = VEC2(0, 0);
|
|
joint->angular_impulse0 = 0;
|
|
joint->angular_impulse1 = 0;
|
|
#endif
|
|
}
|
|
else
|
|
{
|
|
/* Mark joint for removal */
|
|
DisableProp(joint_ent, Prop_Active);
|
|
EnableProp(joint_ent, Prop_Release);
|
|
}
|
|
}
|
|
}
|
|
|
|
void WarmStartWeldJoints(PhysStepCtx *ctx)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!ShouldSimulate(joint_ent)) continue;
|
|
if (!HasProp(joint_ent, Prop_WeldJoint)) continue;
|
|
|
|
WeldJoint *joint = &joint_ent->weld_joint_data;
|
|
|
|
#if 0
|
|
Entity *e0 = EntityFromId(ss, joint->e0);
|
|
if (ShouldSimulate(e0))
|
|
{
|
|
f32 inv_m = joint->inv_m0;
|
|
f32 inv_i = joint->inv_i0;
|
|
Xform xf = XformFromEntity(e1);
|
|
Vec2 vcp = SubVec2(MulXformV2(xf, joint->point_local_start), xf.og);
|
|
SetLinearVelocity(ent, AddVec2(ent->linear_velocity, MulVec2(joint->linear_impulse, inv_m)));
|
|
ent->angular_velocity += (WedgeVec2(vcp, joint->linear_impulse) + joint->angular_impulse) * inv_i;
|
|
}
|
|
#endif
|
|
|
|
#if 1
|
|
Entity *e1 = EntityFromId(ss, joint->e1);
|
|
if (ShouldSimulate(e1))
|
|
{
|
|
f32 inv_m = joint->inv_m1;
|
|
f32 inv_i = joint->inv_i1;
|
|
SetLinearVelocity(e1, AddVec2(e1->linear_velocity, MulVec2(joint->linear_impulse1, inv_m)));
|
|
SetAngularVelocity(e1, e1->angular_velocity + joint->angular_impulse1 * inv_i);
|
|
}
|
|
#else
|
|
LAX joint;
|
|
#endif
|
|
}
|
|
}
|
|
|
|
void SolveWeldJoints(PhysStepCtx *ctx, f32 dt)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *joint_ent = &ss->ents[sim_ent_index];
|
|
if (!ShouldSimulate(joint_ent)) continue;
|
|
if (!HasProp(joint_ent, Prop_WeldJoint)) continue;
|
|
|
|
WeldJoint *joint = &joint_ent->weld_joint_data;
|
|
Entity *e0 = EntityFromId(ss, joint->e0);
|
|
Entity *e1 = EntityFromId(ss, joint->e1);
|
|
if (ShouldSimulate(e0) && ShouldSimulate(e1))
|
|
{
|
|
Xform xf0 = XformFromEntity(e0);
|
|
Xform xf1 = XformFromEntity(e1);
|
|
|
|
Xform target_xf1 = MulXform(xf0, joint->xf0_to_xf1);
|
|
|
|
Vec2 v1 = e1->linear_velocity;
|
|
f32 w1 = e1->angular_velocity;
|
|
|
|
/* Angular constraint */
|
|
{
|
|
SoftSpring softness = MakeSpring(joint->angular_spring_hz, joint->angular_spring_damp, dt);
|
|
f32 inv_i1 = joint->inv_i1;
|
|
f32 k = 1 / inv_i1;
|
|
f32 separation = UnwindAngleF32(RotationFromXform(target_xf1) - RotationFromXform(xf1));
|
|
f32 bias = -separation * softness.bias_rate;
|
|
f32 b = (w1 + bias) * k;
|
|
f32 impulse = -softness.mass_scale * b - joint->angular_impulse1 * softness.impulse_scale;
|
|
joint->angular_impulse1 += impulse;
|
|
w1 += impulse * inv_i1;
|
|
}
|
|
|
|
/* Linear constraint */
|
|
{
|
|
SoftSpring softness = MakeSpring(joint->linear_spring_hz, joint->linear_spring_damp, dt);
|
|
f32 inv_m1 = joint->inv_m1;
|
|
|
|
Vec2 separation = SubVec2(xf1.og, target_xf1.og);
|
|
|
|
f32 k = 1 / inv_m1;
|
|
|
|
Vec2 bias = MulVec2(separation, softness.bias_rate);
|
|
Vec2 b = MulVec2(AddVec2(v1, bias), k);
|
|
|
|
Vec2 impulse = MulVec2(b, -softness.mass_scale);
|
|
impulse = SubVec2(impulse, MulVec2(joint->linear_impulse1, softness.impulse_scale));
|
|
|
|
Vec2 old_impulse = joint->linear_impulse1;
|
|
joint->linear_impulse1 = AddVec2(joint->linear_impulse1, impulse);
|
|
|
|
impulse = SubVec2(joint->linear_impulse1, old_impulse);
|
|
|
|
v1 = AddVec2(v1, MulVec2(impulse, inv_m1));
|
|
}
|
|
|
|
|
|
SetLinearVelocity(e1, v1);
|
|
SetAngularVelocity(e1, w1);
|
|
}
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////
|
|
//~ Integration
|
|
|
|
Xform GetDerivedEntityXform(Entity *ent, f32 dt)
|
|
{
|
|
Xform xf = XformFromEntity(ent);
|
|
|
|
Vec2 step_linear_velocity = MulVec2(ent->linear_velocity, dt);
|
|
f32 step_angular_velocity = ent->angular_velocity * dt;
|
|
|
|
xf.og = AddVec2(xf.og, step_linear_velocity);
|
|
xf = WorldRotateXformBasis(xf, step_angular_velocity);
|
|
return xf;
|
|
}
|
|
|
|
void IntegrateForces(PhysStepCtx *ctx, f32 dt)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *ent = &ss->ents[sim_ent_index];
|
|
if (!ShouldSimulate(ent)) continue;
|
|
|
|
b32 is_dynamic = HasProp(ent, Prop_Dynamic);
|
|
b32 is_kinematic = HasProp(ent, Prop_Kinematic);
|
|
if (is_dynamic || is_kinematic)
|
|
{
|
|
Vec2 linear_velocity = ent->linear_velocity;
|
|
f32 angular_velocity = ent->angular_velocity;
|
|
f32 linear_damping_factor = MaxF32(1.0f - (ent->linear_damping * dt), 0);
|
|
f32 angular_damping_factor = MaxF32(1.0f - (ent->angular_damping * dt), 0);
|
|
|
|
/* Integrate forces */
|
|
if (is_dynamic)
|
|
{
|
|
Xform xf = XformFromEntity(ent);
|
|
f32 det_abs = AbsF32(DeterminantFromXform(xf));
|
|
f32 mass = ent->mass_unscaled * det_abs;
|
|
f32 inertia = ent->inertia_unscaled * det_abs;
|
|
Vec2 force_accel = MulVec2(DivVec2(ent->force, mass), dt);
|
|
f32 torque_accel = (ent->torque / inertia) * dt;
|
|
linear_velocity = AddVec2(linear_velocity, force_accel);
|
|
angular_velocity += torque_accel;
|
|
}
|
|
|
|
/* Apply damping */
|
|
linear_velocity = MulVec2(linear_velocity, linear_damping_factor);
|
|
angular_velocity *= angular_damping_factor;
|
|
|
|
/* Update entity */
|
|
SetLinearVelocity(ent, linear_velocity);
|
|
SetAngularVelocity(ent, angular_velocity);
|
|
ent->force = VEC2(0, 0);
|
|
ent->torque = 0;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
void IntegrateVelocities(PhysStepCtx *ctx, f32 dt)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *ent = &ss->ents[sim_ent_index];
|
|
if (!ShouldSimulate(ent)) continue;
|
|
if (!HasProp(ent, Prop_Dynamic) && !HasProp(ent, Prop_Kinematic)) continue;
|
|
|
|
Xform xf = GetDerivedEntityXform(ent, dt);
|
|
SetXform(ent, xf);
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////
|
|
//~ Earliest time of impact
|
|
|
|
f32 DetermineEarliestToi(PhysStepCtx *ctx, f32 step_dt, f32 tolerance, u32 max_iterations)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
Space *space = ctx->sim_step_ctx->accel->space;
|
|
f32 smallest_t = 1;
|
|
|
|
for (u64 e0_index = 0; e0_index < ss->num_ents_reserved; ++e0_index)
|
|
{
|
|
Entity *e0 = &ss->ents[e0_index];
|
|
if (!ShouldSimulate(e0)) continue;
|
|
if (!(HasProp(e0, Prop_Solid) || HasProp(e0, Prop_Sensor))) continue;
|
|
if (!HasProp(e0, Prop_Toi)) continue;
|
|
if (e0->local_collider.count <= 0) continue;
|
|
|
|
CLD_Shape e0_collider = e0->local_collider;
|
|
Xform e0_xf_t0 = XformFromEntity(e0);
|
|
Xform e0_xf_t1 = GetDerivedEntityXform(e0, step_dt);
|
|
|
|
/* TODO: Use swept aabb rather than combined aabb. This should prevent spikes from bullets returning 0 positive TOIs with irrelevant entities. */
|
|
Aabb aabb_t0 = CLD_AabbFromShape(&e0_collider, e0_xf_t0);
|
|
Aabb aabb_t1 = CLD_AabbFromShape(&e0_collider, e0_xf_t1);
|
|
Aabb combined_aabb = CLD_CombineAabb(aabb_t0, aabb_t1);
|
|
|
|
SpaceIter iter = BeginSpaceIterAabb(space, combined_aabb);
|
|
SpaceEntry *entry;
|
|
while ((entry = NextSpaceIterAabb(&iter)) != 0)
|
|
{
|
|
Entity *e1 = EntityFromId(ss, entry->ent);
|
|
if (!ShouldSimulate(e1)) continue;
|
|
if (!(HasProp(e1, Prop_Solid) || HasProp(e1, Prop_Sensor))) continue;
|
|
if (e1->local_collider.count <= 0) continue;
|
|
if (!CanEntitiesContact(e0, e1)) continue;
|
|
|
|
CLD_Shape e1_collider = e1->local_collider;
|
|
Xform e1_xf_t0 = XformFromEntity(e1);
|
|
Xform e1_xf_t1 = GetDerivedEntityXform(e1, step_dt);
|
|
|
|
f32 t = CLD_TimeOfImpact(&e0_collider, &e1_collider, e0_xf_t0, e1_xf_t0, e0_xf_t1, e1_xf_t1, tolerance, max_iterations);
|
|
if (t != 0 && t < smallest_t)
|
|
{
|
|
smallest_t = t;
|
|
}
|
|
}
|
|
EndSpaceIter(&iter);
|
|
}
|
|
|
|
return smallest_t;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////
|
|
//~ Spatial
|
|
|
|
void UpdateAabbs(PhysStepCtx *ctx)
|
|
{
|
|
__prof;
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
Space *space = ctx->sim_step_ctx->accel->space;
|
|
for (u64 sim_ent_index = 0; sim_ent_index < ss->num_ents_reserved; ++sim_ent_index)
|
|
{
|
|
Entity *ent = &ss->ents[sim_ent_index];
|
|
if (!IsValidAndActive(ent)) continue;
|
|
if (ent->local_collider.count > 0)
|
|
{
|
|
Xform xf = XformFromEntity(ent);
|
|
SpaceEntry *space_entry = SpaceEntryFromHandle(space, ent->space_handle);
|
|
if (!space_entry->valid)
|
|
{
|
|
space_entry = AcquireSpaceEntry(space, ent->id);
|
|
ent->space_handle = space_entry->handle;
|
|
}
|
|
Aabb aabb = CLD_AabbFromShape(&ent->local_collider, xf);
|
|
UpdateSpaceEntryAabb(space_entry, aabb);
|
|
}
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////
|
|
//~ Step
|
|
|
|
/* Returns phys iteration to be fed into next step. Supplied iteration must be > 0. */
|
|
void StepPhys(PhysStepCtx *ctx, f32 timestep)
|
|
{
|
|
__prof;
|
|
IntegrateForces(ctx, timestep);
|
|
Snapshot *ss = ctx->sim_step_ctx->world;
|
|
u64 phys_iteration = ss->phys_iteration;
|
|
|
|
UpdateAabbs(ctx);
|
|
|
|
f32 remaining_dt = timestep;
|
|
while (remaining_dt > 0)
|
|
{
|
|
__profn("Step part");
|
|
++phys_iteration;
|
|
TempArena scratch = BeginScratchNoConflict();
|
|
|
|
/* TOI */
|
|
f32 step_dt = remaining_dt;
|
|
{
|
|
#if SIM_PHYSICS_ENABLE_TOI
|
|
const f32 min_toi = 0.000001f;
|
|
const f32 tolerance = 0.0001f;
|
|
const u32 max_iterations = 16;
|
|
f32 earliest_toi = MaxF32(DetermineEarliestToi(ctx, step_dt, tolerance, max_iterations), min_toi);
|
|
step_dt = remaining_dt * earliest_toi;
|
|
#else
|
|
LAX DetermineEarliestToi;
|
|
#endif
|
|
}
|
|
remaining_dt -= step_dt;
|
|
|
|
CreateAndUpdateContacts(ctx, timestep - remaining_dt, phys_iteration);
|
|
|
|
PrepareContacts(ctx, phys_iteration);
|
|
PrepareMotorJoints(ctx);
|
|
PrepareMouseJoints(ctx);
|
|
PrepareWeldJoints(ctx);
|
|
|
|
f32 substep_dt = step_dt / SIM_PHYSICS_SUBSTEPS;
|
|
for (u32 i = 0; i < SIM_PHYSICS_SUBSTEPS; ++i)
|
|
{
|
|
__profn("Substep");
|
|
|
|
/* Warm start */
|
|
#if SIM_PHYSICS_ENABLE_WARM_STARTING
|
|
WarmStartContacts(ctx);
|
|
WarmStartMotorJoints(ctx);
|
|
WarmStartMouseJoints(ctx);
|
|
WarmStartWeldJoints(ctx);
|
|
#endif
|
|
|
|
/* Solve */
|
|
#if SIM_PHYSICS_ENABLE_COLLISION
|
|
SolveContacts(ctx, substep_dt, 1);
|
|
#endif
|
|
SolveMotorJoints(ctx, substep_dt);
|
|
SolveMouseJoints(ctx, substep_dt);
|
|
SolveWeldJoints(ctx, substep_dt);
|
|
|
|
/* Integrate */
|
|
IntegrateVelocities(ctx, substep_dt);
|
|
|
|
/* Relaxation solve */
|
|
#if SIM_PHYSICS_ENABLE_COLLISION && SIM_PHYSICS_ENABLE_RELAXATION
|
|
SolveContacts(ctx, substep_dt, 0);
|
|
#endif
|
|
}
|
|
|
|
UpdateAabbs(ctx);
|
|
EndScratch(scratch);
|
|
}
|
|
|
|
ss->phys_iteration = phys_iteration;
|
|
}
|