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; } }