public b2ContactSolver(b2ContactSolverDef def)
{
m_step = def.step;
m_count = def.count;
m_positionConstraints = new b2ContactPositionConstraint[m_count];
for (int pc = 0; pc < m_count; pc++)
m_positionConstraints[pc] = b2ContactPositionConstraint.Create();
m_velocityConstraints = new b2ContactVelocityConstraint[m_count];
for (int vc = 0; vc < m_count; vc++)
m_velocityConstraints[vc] = b2ContactVelocityConstraint.Create();
m_positions = def.positions;
m_velocities = def.velocities;
m_contacts = def.contacts;
// Initialize position independent portions of the constraints.
for (int i = 0; i < m_count; ++i)
{
b2Contact contact = m_contacts[i];
b2Fixture fixtureA = contact.FixtureA;
b2Fixture fixtureB = contact.FixtureB;
b2Shape shapeA = fixtureA.Shape;
b2Shape shapeB = fixtureB.Shape;
float radiusA = shapeA.Radius;
float radiusB = shapeB.Radius;
b2Body bodyA = fixtureA.Body;
b2Body bodyB = fixtureB.Body;
b2Manifold manifold = contact.GetManifold();
int pointCount = manifold.pointCount;
Debug.Assert(pointCount > 0);
b2ContactVelocityConstraint vc = m_velocityConstraints[i];
vc.friction = contact.Friction;
vc.restitution = contact.Restitution;
vc.indexA = bodyA.IslandIndex;
vc.indexB = bodyB.IslandIndex;
vc.invMassA = bodyA.InvertedMass;
vc.invMassB = bodyB.InvertedMass;
vc.invIA = bodyA.InvertedI;
vc.invIB = bodyB.InvertedI;
vc.contactIndex = i;
vc.pointCount = pointCount;
vc.K.SetZero();
vc.normalMass.SetZero();
b2ContactPositionConstraint pc = m_positionConstraints[i];
pc.indexA = bodyA.IslandIndex;
pc.indexB = bodyB.IslandIndex;
pc.invMassA = bodyA.InvertedMass;
pc.invMassB = bodyB.InvertedMass;
pc.localCenterA = bodyA.Sweep.localCenter;
pc.localCenterB = bodyB.Sweep.localCenter;
pc.invIA = bodyA.InvertedI;
pc.invIB = bodyB.InvertedI;
pc.localNormal = manifold.localNormal;
pc.localPoint = manifold.localPoint;
pc.pointCount = pointCount;
pc.radiusA = radiusA;
pc.radiusB = radiusB;
pc.type = manifold.type;
for (int j = 0; j < pointCount; ++j)
{
b2ManifoldPoint cp = manifold.points[j];
b2VelocityConstraintPoint vcp = vc.points[j];
if (m_step.warmStarting)
{
vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse;
vcp.tangentImpulse = m_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;
vc.points[j] = vcp;
}
//Put back the struct data since struct data is copied by value
m_positionConstraints[i] = pc;
m_velocityConstraints[i] = vc;
}
}
public b2ContactSolver(b2ContactSolverDef def)
{
m_step = def.step;
m_count = def.count;
m_positionConstraints = new b2ContactPositionConstraint[m_count];
for (int pc = 0; pc < m_count; pc++)
{
m_positionConstraints[pc] = b2ContactPositionConstraint.Create();
}
m_velocityConstraints = new b2ContactVelocityConstraint[m_count];
for (int vc = 0; vc < m_count; vc++)
{
m_velocityConstraints[vc] = b2ContactVelocityConstraint.Create();
}
m_positions = def.positions;
m_velocities = def.velocities;
m_contacts = def.contacts;
// Initialize position independent portions of the constraints.
for (int i = 0; i < m_count; ++i)
{
b2Contact contact = m_contacts[i];
b2Fixture fixtureA = contact.FixtureA;
b2Fixture fixtureB = contact.FixtureB;
b2Shape shapeA = fixtureA.Shape;
b2Shape shapeB = fixtureB.Shape;
float radiusA = shapeA.Radius;
float radiusB = shapeB.Radius;
b2Body bodyA = fixtureA.Body;
b2Body bodyB = fixtureB.Body;
b2Manifold manifold = contact.GetManifold();
int pointCount = manifold.pointCount;
Debug.Assert(pointCount > 0);
b2ContactVelocityConstraint vc = m_velocityConstraints[i];
vc.friction = contact.Friction;
vc.restitution = contact.Restitution;
vc.indexA = bodyA.IslandIndex;
vc.indexB = bodyB.IslandIndex;
vc.invMassA = bodyA.InvertedMass;
vc.invMassB = bodyB.InvertedMass;
vc.invIA = bodyA.InvertedI;
vc.invIB = bodyB.InvertedI;
vc.contactIndex = i;
vc.pointCount = pointCount;
vc.K.SetZero();
vc.normalMass.SetZero();
b2ContactPositionConstraint pc = m_positionConstraints[i];
pc.indexA = bodyA.IslandIndex;
pc.indexB = bodyB.IslandIndex;
pc.invMassA = bodyA.InvertedMass;
pc.invMassB = bodyB.InvertedMass;
pc.localCenterA = bodyA.Sweep.localCenter;
pc.localCenterB = bodyB.Sweep.localCenter;
pc.invIA = bodyA.InvertedI;
pc.invIB = bodyB.InvertedI;
pc.localNormal = manifold.localNormal;
pc.localPoint = manifold.localPoint;
pc.pointCount = pointCount;
pc.radiusA = radiusA;
pc.radiusB = radiusB;
pc.type = manifold.type;
for (int j = 0; j < pointCount; ++j)
{
b2ManifoldPoint cp = manifold.points[j];
b2VelocityConstraintPoint vcp = vc.points[j];
if (m_step.warmStarting)
{
vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse;
vcp.tangentImpulse = m_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;
//vc.points[j] = vcp;
}
//Put back the struct data since struct data is copied by value
//m_positionConstraints[i] = pc;
//m_velocityConstraints[i] = vc;
}
}
