private void CorrectContact(Rigidbody body0, Rigidbody body1, ClosestPoints cp)
{
var n = cp.normal;
var r0 = cp.point0 - body0.position;
var r1 = cp.point1 - body1.position;
var invVirtualMass = (
body0.invMass
+ body1.invMass
+ body0.invAngularMass * Mathf.Sq(r0 % n)
+ body1.invAngularMass * Mathf.Sq(r1 % n)
);
if (invVirtualMass < Mathf.EPS)
{
// Probably two static bodies or something not correctable
return;
}
var penetration = -cp.distance;
var dti = penetration / invVirtualMass;
body0.position -= n * (dti * body0.invMass);
body1.position += n * (dti * body1.invMass);
body0.angle -= (r0 % n) * (dti * body0.invAngularMass);
body1.angle += (r1 % n) * (dti * body1.invAngularMass);
}
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;
}
}
private void StoreContact(Rigidbody body0, Rigidbody body1, ClosestPoints p)
{
if (p.spreaded)
{
Vec2 trans = p.normal.Rot90() * (p.spread * 0.5f);
contacts.Add(new ContactHalf()
{
body0 = body0,
body1 = body1,
normal = p.normal,
tag = 1,
penetration = -p.distance,
position = p.point0 + trans,
});
contacts.Add(new ContactHalf()
{
body0 = body1,
body1 = body0,
normal = -p.normal,
tag = 2,
penetration = -p.distance,
position = p.point1 + trans,
});
contacts.Add(new ContactHalf()
{
body0 = body0,
body1 = body1,
normal = p.normal,
tag = 1,
penetration = -p.distance,
position = p.point0 - trans,
});
contacts.Add(new ContactHalf()
{
body0 = body1,
body1 = body0,
normal = -p.normal,
tag = 2,
penetration = -p.distance,
position = p.point1 - trans,
});
}
contacts.Add(new ContactHalf()
{
body0 = body0,
body1 = body1,
normal = p.normal,
tag = 1,
penetration = -p.distance,
position = p.point0,
});
contacts.Add(new ContactHalf()
{
body0 = body1,
body1 = body0,
normal = -p.normal,
tag = 2,
penetration = -p.distance,
position = p.point1,
});
}
