コード例 #1
0
        public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
        {
            Step = step;

            ConstraintCount = contactCount;
            Constraints     = new ContactConstraint[ConstraintCount];

            for (int i = 0; i < ConstraintCount; ++i)
            {
                Contact contact = contacts[i];

                Fixture  fixtureA = contact.GetFixtureA();
                Fixture  fixtureB = contact.GetFixtureB();
                Shape    shapeA   = fixtureA.GetShape();
                Shape    shapeB   = fixtureB.GetShape();
                float    radiusA  = shapeA._radius;
                float    radiusB  = shapeB._radius;
                Body     bodyA    = fixtureA.GetBody();
                Body     bodyB    = fixtureB.GetBody();
                Manifold manifold = contact.GetManifold();

                float friction    = Settings.MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction());
                float restitution = Settings.MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution());

                Vec2  vA = bodyA._linearVelocity;
                Vec2  vB = bodyB._linearVelocity;
                float wA = bodyA._angularVelocity;
                float wB = bodyB._angularVelocity;

                Box2DXDebug.Assert(manifold.PointCount > 0);

                WorldManifold worldManifold = new WorldManifold();
                worldManifold.Initialize(manifold, bodyA.GetTransform(), radiusA, bodyB.GetTransform(), radiusB);

                ContactConstraint cc = new ContactConstraint();
                Constraints[i] = cc;
                cc.BodyA       = bodyA;
                cc.BodyB       = bodyB;
                cc.Manifold    = manifold;
                cc.Normal      = worldManifold.Normal;
                cc.PointCount  = manifold.PointCount;
                cc.Friction    = friction;
                cc.Restitution = restitution;

                cc.LocalPlaneNormal = manifold.LocalPlaneNormal;
                cc.LocalPoint       = manifold.LocalPoint;
                cc.Radius           = radiusA + radiusB;
                cc.Type             = manifold.Type;

                for (int j = 0; j < cc.PointCount; ++j)
                {
                    ManifoldPoint          cp  = manifold.Points[j];
                    ContactConstraintPoint ccp = cc.Points[j];

                    ccp.NormalImpulse  = cp.NormalImpulse;
                    ccp.TangentImpulse = cp.TangentImpulse;

                    ccp.LocalPoint = cp.LocalPoint;

                    ccp.RA = worldManifold.Points[j] - bodyA._sweep.C;
                    ccp.RB = worldManifold.Points[j] - bodyB._sweep.C;

                    float rnA = Vec2.Cross(ccp.RA, cc.Normal);
                    float rnB = Vec2.Cross(ccp.RB, cc.Normal);
                    rnA *= rnA;
                    rnB *= rnB;

                    float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;

                    Box2DXDebug.Assert(kNormal > Settings.FLT_EPSILON);
                    ccp.NormalMass = 1.0f / kNormal;

                    float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass;
                    kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB;

                    Box2DXDebug.Assert(kEqualized > Settings.FLT_EPSILON);
                    ccp.EqualizedMass = 1.0f / kEqualized;

                    Vec2 tangent = Vec2.Cross(cc.Normal, 1.0f);

                    float rtA = Vec2.Cross(ccp.RA, tangent);
                    float rtB = Vec2.Cross(ccp.RB, tangent);
                    rtA *= rtA;
                    rtB *= rtB;

                    float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;

                    Box2DXDebug.Assert(kTangent > Settings.FLT_EPSILON);
                    ccp.TangentMass = 1.0f / kTangent;

                    // Setup a velocity bias for restitution.
                    ccp.VelocityBias = 0.0f;
                    float vRel = Vec2.Dot(cc.Normal, vB + Vec2.Cross(wB, ccp.RB) - vA - Vec2.Cross(wA, ccp.RA));
                    if (vRel < -Settings.VelocityThreshold)
                    {
                        ccp.VelocityBias = -cc.Restitution * vRel;
                    }
                }

                // If we have two points, then prepare the block solver.
                if (cc.PointCount == 2)
                {
                    ContactConstraintPoint ccp1 = cc.Points[0];
                    ContactConstraintPoint ccp2 = cc.Points[1];

                    float invMassA = bodyA._invMass;
                    float invIA    = bodyA._invI;
                    float invMassB = bodyB._invMass;
                    float invIB    = bodyB._invI;

                    float rn1A = Vec2.Cross(ccp1.RA, cc.Normal);
                    float rn1B = Vec2.Cross(ccp1.RB, cc.Normal);
                    float rn2A = Vec2.Cross(ccp2.RA, cc.Normal);
                    float rn2B = Vec2.Cross(ccp2.RB, cc.Normal);

                    float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
                    float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
                    float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;

                    // Ensure a reasonable condition number.
                    const float k_maxConditionNumber = 100.0f;
                    if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
                    {
                        // K is safe to invert.
                        cc.K.Col1.Set(k11, k12);
                        cc.K.Col2.Set(k12, k22);
                        cc.NormalMass = cc.K.Invert();
                    }
                    else
                    {
                        // The constraints are redundant, just use one.
                        // TODO_ERIN use deepest?
                        cc.PointCount = 1;
                    }
                }
            }
        }