public ContactSolver(ContactSolverDef def) {
m_step = def.step;
m_positionConstraints = new List<ContactPositionConstraint>();
m_velocityConstraints = new List<ContactVelocityConstraint>();
m_positions = def.positions;
m_velocities = def.velocities;
m_contacts = def.contacts;
// Initialize position independent portions of the constraints.
for (int i = 0; i < def.contacts.Count(); ++i) {
Contact contact = m_contacts[i];
Fixture fixtureA = contact.m_fixtureA;
Fixture fixtureB = contact.m_fixtureB;
Shape shapeA = fixtureA.GetShape();
Shape shapeB = fixtureB.GetShape();
float radiusA = shapeA.m_radius;
float radiusB = shapeB.m_radius;
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
Manifold manifold = contact.GetManifold();
int pointCount = manifold.points.Count();
Utilities.Assert(pointCount > 0);
ContactVelocityConstraint vc = new ContactVelocityConstraint();
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.points.Count() = pointCount;
vc.K.SetZero();
vc.normalMass.SetZero();
ContactPositionConstraint pc = new ContactPositionConstraint();
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) {
ManifoldPoint cp = manifold.points[j];
VelocityConstraintPoint vcp = new VelocityConstraintPoint();
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;
vc.points.Add(vcp);
pc.localPoints[j] = cp.localPoint;
}
m_velocityConstraints.Add(vc);
m_positionConstraints.Add(pc);
}
}
public ContactSolver(ContactSolverDef def)
{
m_step = def.step;
m_positionConstraints = new List <ContactPositionConstraint>();
m_velocityConstraints = new List <ContactVelocityConstraint>();
m_positions = def.positions;
m_velocities = def.velocities;
m_contacts = def.contacts;
// Initialize position independent portions of the constraints.
for (int i = 0; i < def.contacts.Count(); ++i)
{
Contact contact = m_contacts[i];
Fixture fixtureA = contact.m_fixtureA;
Fixture fixtureB = contact.m_fixtureB;
Shape shapeA = fixtureA.GetShape();
Shape shapeB = fixtureB.GetShape();
float radiusA = shapeA.m_radius;
float radiusB = shapeB.m_radius;
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
Manifold manifold = contact.GetManifold();
int pointCount = manifold.points.Count();
Utilities.Assert(pointCount > 0);
ContactVelocityConstraint vc = new ContactVelocityConstraint();
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.points.Count() = pointCount;
vc.K.SetZero();
vc.normalMass.SetZero();
ContactPositionConstraint pc = new ContactPositionConstraint();
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)
{
ManifoldPoint cp = manifold.points[j];
VelocityConstraintPoint vcp = new VelocityConstraintPoint();
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;
vc.points.Add(vcp);
pc.localPoints[j] = cp.localPoint;
}
m_velocityConstraints.Add(vc);
m_positionConstraints.Add(pc);
}
}
