public ContactSolver(ContactSolverDef def)
{
var step = def.Step;
_count = def.Count;
_positionConstraints = new ContactPositionConstraint[_count];
VelocityConstraints = new ContactVelocityConstraint[_count];
for (var i = 0; i < _count; i++)
{
_positionConstraints[i] = new ContactPositionConstraint();
VelocityConstraints[i] = new ContactVelocityConstraint();
}
_positions = def.Positions;
_velocities = def.Velocities;
_contacts = def.Contacts;
// Initialize position independent portions of the constraints.
for (var i = 0; i < _count; ++i)
{
var contact = _contacts[i];
var fixtureA = contact.FixtureA;
var fixtureB = contact.FixtureB;
var shapeA = fixtureA.GetShape();
var shapeB = fixtureB.GetShape();
var radiusA = shapeA.Radius;
var radiusB = shapeB.Radius;
var bodyA = fixtureA.GetBody();
var bodyB = fixtureB.GetBody();
var manifold = contact.GetManifold();
var pointCount = manifold.PointCount;
Debug.Assert(pointCount > 0);
var vc = VelocityConstraints[i];
vc.Friction = contact.Friction;
vc.Restitution = contact.Restitution;
vc.TangentSpeed = contact.TangentSpeed;
vc.IndexA = bodyA.IslandIndex;
vc.IndexB = bodyB.IslandIndex;
vc.InvMassA = bodyA.InvMass;
vc.InvMassB = bodyB.InvMass;
vc.InvIa = bodyA.InverseInertia;
vc.InvIb = bodyB.InverseInertia;
vc.ContactIndex = i;
vc.PointCount = pointCount;
vc.K.SetZero();
vc.NormalMass.SetZero();
var pc = _positionConstraints[i];
pc.IndexA = bodyA.IslandIndex;
pc.IndexB = bodyB.IslandIndex;
pc.InvMassA = bodyA.InvMass;
pc.InvMassB = bodyB.InvMass;
pc.LocalCenterA = bodyA.Sweep.LocalCenter;
pc.LocalCenterB = bodyB.Sweep.LocalCenter;
pc.InvIa = bodyA.InverseInertia;
pc.InvIb = bodyB.InverseInertia;
pc.LocalNormal = manifold.LocalNormal;
pc.LocalPoint = manifold.LocalPoint;
pc.PointCount = pointCount;
pc.RadiusA = radiusA;
pc.RadiusB = radiusB;
pc.Type = manifold.Type;
for (var j = 0; j < pointCount; ++j)
{
var cp = manifold.Points[j];
var vcp = vc.Points[j];
if (step.WarmStarting)
{
vcp.NormalImpulse = step.DtRatio * cp.NormalImpulse;
vcp.TangentImpulse = step.DtRatio * cp.TangentImpulse;
}
else
{
vcp.NormalImpulse = 0.0f;
vcp.TangentImpulse = 0.0f;
}
vcp.Ra.SetZero();
vcp.Rb.SetZero();
vcp.NormalMass = 0.0f;
vcp.TangentMass = 0.0f;
vcp.VelocityBias = 0.0f;
pc.LocalPoints[j] = cp.LocalPoint;
}
}
}