public override void PreSolve(b2Contact contact, b2Manifold oldManifold)
{
b2Manifold manifold = contact.GetManifold();
if (manifold.pointCount == 0)
{
return;
}
b2Fixture fixtureA = contact.GetFixtureA();
b2Fixture fixtureB = contact.GetFixtureB();
b2Collision.b2GetPointStates(state1, state2, oldManifold, manifold);
contact.GetWorldManifold(ref worldManifold);
for (int i = 0; i < manifold.pointCount && m_pointCount < k_maxContactPoints; ++i)
{
ContactPoint cp = m_points[m_pointCount];
if (cp == null)
{
cp = new ContactPoint();
m_points[m_pointCount] = cp;
}
cp.fixtureA = fixtureA;
cp.fixtureB = fixtureB;
cp.position = worldManifold.points[i];
cp.normal = worldManifold.normal;
cp.state = state2[i];
++m_pointCount;
}
}
public override void PostSolve(b2Contact contact, ref b2ContactImpulse impulse)
{
if (m_broke)
{
// The body already broke.
return;
}
// Should the body break?
int count = contact.GetManifold().pointCount;
float maxImpulse = 0.0f;
for (int i = 0; i < count; ++i)
{
maxImpulse = Math.Max(maxImpulse, impulse.normalImpulses[i]);
}
if (maxImpulse > 40.0f)
{
// Flag the body for breaking.
m_break = true;
}
}
public b2ContactSolver(b2ContactSolverDef def)
{
m_step = def.step;
m_count = def.count;
m_positionConstraints = Arrays.InitializeWithDefaultInstances <b2ContactPositionConstraint>(m_count);
m_velocityConstraints = Arrays.InitializeWithDefaultInstances <b2ContactVelocityConstraint>(m_count);
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.m_fixtureA;
b2Fixture fixtureB = contact.m_fixtureB;
b2Shape shapeA = fixtureA.GetShape();
b2Shape shapeB = fixtureB.GetShape();
float radiusA = shapeA.m_radius;
float radiusB = shapeB.m_radius;
b2Body bodyA = fixtureA.GetBody();
b2Body bodyB = fixtureB.GetBody();
b2Manifold manifold = contact.GetManifold();
int pointCount = manifold.pointCount;
Debug.Assert(pointCount > 0);
b2ContactVelocityConstraint vc = m_velocityConstraints[i];
vc.friction = contact.m_friction;
vc.restitution = contact.m_restitution;
vc.tangentSpeed = contact.m_tangentSpeed;
vc.indexA = bodyA.m_islandIndex;
vc.indexB = bodyB.m_islandIndex;
vc.invMassA = bodyA.m_invMass;
vc.invMassB = bodyB.m_invMass;
vc.invIA = bodyA.m_invI;
vc.invIB = bodyB.m_invI;
vc.contactIndex = i;
vc.pointCount = pointCount;
vc.K.SetZero();
vc.normalMass.SetZero();
b2ContactPositionConstraint pc = m_positionConstraints[i];
pc.indexA = bodyA.m_islandIndex;
pc.indexB = bodyB.m_islandIndex;
pc.invMassA = bodyA.m_invMass;
pc.invMassB = bodyB.m_invMass;
pc.localCenterA = bodyA.m_sweep.localCenter;
pc.localCenterB = bodyB.m_sweep.localCenter;
pc.invIA = bodyA.m_invI;
pc.invIB = bodyB.m_invI;
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;
}
}
}
