public ContactSolver(ContactSolverDef def) { var step = def.Step; _count = def.Count; _positionConstraints = new ContactPositionConstraint[_count]; VelocityConstraints = new ContactVelocityConstraint[_count]; for (var i = 0; i < _count; i++) { _positionConstraints[i] = new ContactPositionConstraint(); VelocityConstraints[i] = new ContactVelocityConstraint(); } _positions = def.Positions; _velocities = def.Velocities; _contacts = def.Contacts; // Initialize position independent portions of the constraints. for (var i = 0; i < _count; ++i) { var contact = _contacts[i]; var fixtureA = contact.FixtureA; var fixtureB = contact.FixtureB; var shapeA = fixtureA.GetShape(); var shapeB = fixtureB.GetShape(); var radiusA = shapeA.Radius; var radiusB = shapeB.Radius; var bodyA = fixtureA.GetBody(); var bodyB = fixtureB.GetBody(); var manifold = contact.GetManifold(); var pointCount = manifold.PointCount; Debug.Assert(pointCount > 0); var vc = VelocityConstraints[i]; vc.Friction = contact.Friction; vc.Restitution = contact.Restitution; vc.TangentSpeed = contact.TangentSpeed; vc.IndexA = bodyA.IslandIndex; vc.IndexB = bodyB.IslandIndex; vc.InvMassA = bodyA.InvMass; vc.InvMassB = bodyB.InvMass; vc.InvIa = bodyA.InverseInertia; vc.InvIb = bodyB.InverseInertia; vc.ContactIndex = i; vc.PointCount = pointCount; vc.K.SetZero(); vc.NormalMass.SetZero(); var pc = _positionConstraints[i]; pc.IndexA = bodyA.IslandIndex; pc.IndexB = bodyB.IslandIndex; pc.InvMassA = bodyA.InvMass; pc.InvMassB = bodyB.InvMass; pc.LocalCenterA = bodyA.Sweep.LocalCenter; pc.LocalCenterB = bodyB.Sweep.LocalCenter; pc.InvIa = bodyA.InverseInertia; pc.InvIb = bodyB.InverseInertia; pc.LocalNormal = manifold.LocalNormal; pc.LocalPoint = manifold.LocalPoint; pc.PointCount = pointCount; pc.RadiusA = radiusA; pc.RadiusB = radiusB; pc.Type = manifold.Type; for (var j = 0; j < pointCount; ++j) { var cp = manifold.Points[j]; var vcp = vc.Points[j]; if (step.WarmStarting) { vcp.NormalImpulse = step.DtRatio * cp.NormalImpulse; vcp.TangentImpulse = 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; } } }