Exemplo n.º 1
0
        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;
                }
            }
        }