public void Step(float full_dt)
{
var dt = full_dt / substeps;
var gravity_dp = gravity * Mathf.Sq(dt);
var positionDampCoeff = Mathf.Pow(1f - positionDamping, dt);
var angularDampCoeff = Mathf.Pow(1f - angularDamping, dt);
foreach (var body in tree.GetAll())
{
body.positionVelocity = body.positionVelocity.Clamp(maxPositionVelocity);
body.angularVelocity = Mathf.ClampSymmetric(body.angularVelocity, maxAngularVelocity);
body.lastPosition = body.position - body.positionVelocity * dt;
body.lastAngle = body.angle - body.angularVelocity * dt;
}
for (var substep = 0; substep < substeps; substep++)
{
foreach (var body in tree.GetAll())
{
if (body.type == BodyType.Dynamic)
{
body.position += gravity_dp;
var positionDelta = body.position - body.lastPosition;
body.lastPosition = body.position;
body.position += positionDelta * positionDampCoeff;
var angleDelta = body.angle - body.lastAngle;
body.lastAngle = body.angle;
body.angle += angleDelta * angularDampCoeff;
var angleWrapper = DiffToMod(body.angle, Mathf.TWO_PI);
body.angle += angleWrapper;
body.lastAngle += angleWrapper;
}
UpdateBodyCache(body);
}
foreach (var body in tree.GetAll())
{
tree.Add(body);
}
contacts.Clear();
notables.Clear();
foreach (var body0 in tree.GetAll())
{
tempList.Clear();
tree.TraverseOverlapping(body0, _addToTempList);
foreach (var body1 in tempList)
{
if (body0 == body1)
{
continue;
}
ClosestPoints p = CapsuleCache.Collide(
body0.fixtureCache,
body1.fixtureCache,
null
);
if (!p.degenerate)
{
StoreContact(body0, body1, p);
if (p.distance < 0)
{
CorrectContact(body0, body1, p);
UpdateBodyCache(body0);
UpdateBodyCache(body1);
}
}
}
}
foreach (var body in tree.GetAll())
{
tree.Add(body);
}
}
foreach (var body in tree.GetAll())
{
var invDt = 1f / dt;
body.positionVelocity = (body.position - body.lastPosition) * invDt;
body.angularVelocity = (body.angle - body.lastAngle) * invDt;
}
}