Box2D.Dynamics.Contacts.ContactSolver.Init C# (CSharp) Метод

        public void Init(ContactSolverDef def)
        {
            //Console.WriteLine("Initializing contact solver");
            Step = def.Step;
            Count = def.Count;

            if (PositionConstraints.Length < Count)
            {
                ContactPositionConstraint[] old = PositionConstraints;
                PositionConstraints = new ContactPositionConstraint[MathUtils.Max(old.Length * 2, Count)];
                Array.Copy(old, 0, PositionConstraints, 0, old.Length);
                for (int i = old.Length; i < PositionConstraints.Length; i++)
                {
                    PositionConstraints[i] = new ContactPositionConstraint();
                }
            }

            if (VelocityConstraints.Length < Count)
            {
                ContactVelocityConstraint[] old = VelocityConstraints;
                VelocityConstraints = new ContactVelocityConstraint[MathUtils.Max(old.Length * 2, Count)];
                Array.Copy(old, 0, VelocityConstraints, 0, old.Length);
                for (int i = old.Length; i < VelocityConstraints.Length; i++)
                {
                    VelocityConstraints[i] = new ContactVelocityConstraint();
                }
            }

            Positions = def.Positions;
            Velocities = def.Velocities;
            Contacts = def.Contacts;

            for (int i = 0; i < Count; ++i)
            {
                //Console.WriteLine("contacts: " + m_count);
                Contact contact = Contacts[i];

                Fixture fixtureA = contact.FixtureA;
                Fixture fixtureB = contact.FixtureB;
                Shape shapeA = fixtureA.Shape;
                Shape shapeB = fixtureB.Shape;
                float radiusA = shapeA.Radius;
                float radiusB = shapeB.Radius;
                Body bodyA = fixtureA.Body;
                Body bodyB = fixtureB.Body;
                Manifold manifold = contact.Manifold;

                int pointCount = manifold.PointCount;
                Debug.Assert(pointCount > 0);

                ContactVelocityConstraint 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.InvI;
                vc.InvIB = bodyB.InvI;
                vc.ContactIndex = i;
                vc.PointCount = pointCount;
                vc.K.SetZero();
                vc.NormalMass.SetZero();

                ContactPositionConstraint pc = PositionConstraints[i];
                pc.IndexA = bodyA.IslandIndex;
                pc.IndexB = bodyB.IslandIndex;
                pc.InvMassA = bodyA.InvMass;
                pc.InvMassB = bodyB.InvMass;
                pc.LocalCenterA.Set(bodyA.Sweep.LocalCenter);
                pc.LocalCenterB.Set(bodyB.Sweep.LocalCenter);
                pc.InvIA = bodyA.InvI;
                pc.InvIB = bodyB.InvI;
                pc.LocalNormal.Set(manifold.LocalNormal);
                pc.LocalPoint.Set(manifold.LocalPoint);
                pc.PointCount = pointCount;
                pc.RadiusA = radiusA;
                pc.RadiusB = radiusB;
                pc.Type = manifold.Type;

                //Console.WriteLine("contact point count: " + pointCount);
                for (int j = 0; j < pointCount; j++)
                {
                    ManifoldPoint cp = manifold.Points[j];
                    ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j];

                    if (Step.WarmStarting)
                    {
                        //Debug.Assert(cp.normalImpulse == 0);
                        //Console.WriteLine("contact normal impulse: " + cp.normalImpulse);
                        vcp.NormalImpulse = Step.DtRatio * cp.NormalImpulse;
                        vcp.TangentImpulse = Step.DtRatio * cp.TangentImpulse;
                    }
                    else
                    {
                        vcp.NormalImpulse = 0;
                        vcp.TangentImpulse = 0;
                    }

                    vcp.RA.SetZero();
                    vcp.RB.SetZero();
                    vcp.NormalMass = 0;
                    vcp.TangentMass = 0;
                    vcp.VelocityBias = 0;

                    pc.LocalPoints[j].Set(cp.LocalPoint);
                }
            }
        }