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