//////////////////////////////////////////////////////////// //~ 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; }