Exemple #1
0
        public void WarmStart()
        {
            // Warm start.
            for (int i = 0; i < _constraintCount; ++i)
            {
                ContactConstraint c = _constraints[i];

                Body    bodyA    = c.bodyA;
                Body    bodyB    = c.bodyB;
                float   invMassA = bodyA._invMass;
                float   invIA    = bodyA._invI;
                float   invMassB = bodyB._invMass;
                float   invIB    = bodyB._invI;
                Vector2 normal   = c.normal;

#if MATH_OVERLOADS
                Vector2 tangent = MathUtils.Cross(normal, 1.0f);
#else
                Vector2 tangent = new Vector2(normal.y, -normal.x);
#endif

                for (int j = 0; j < c.pointCount; ++j)
                {
                    ContactConstraintPoint ccp = c.points[j];
#if MATH_OVERLOADS
                    Vector2 P = ccp.normalImpulse * normal + ccp.tangentImpulse * tangent;
                    bodyA._angularVelocity -= invIA * MathUtils.Cross(ccp.rA, P);
                    bodyA._linearVelocity  -= invMassA * P;
                    bodyB._angularVelocity += invIB * MathUtils.Cross(ccp.rB, P);
                    bodyB._linearVelocity  += invMassB * P;
#else
                    Vector2 P = new Vector2(ccp.normalImpulse * normal.x + ccp.tangentImpulse * tangent.x,
                                            ccp.normalImpulse * normal.y + ccp.tangentImpulse * tangent.y);
                    bodyA._angularVelocity  -= invIA * (ccp.rA.x * P.y - ccp.rA.y * P.x);
                    bodyA._linearVelocity.x -= invMassA * P.x;
                    bodyA._linearVelocity.y -= invMassA * P.y;
                    bodyB._angularVelocity  += invIB * (ccp.rB.x * P.y - ccp.rB.y * P.x);
                    bodyB._linearVelocity.x += invMassB * P.x;
                    bodyB._linearVelocity.y += invMassB * P.y;
#endif
                    c.points[j] = ccp;
                }

                _constraints[i] = c;
            }
        }
Exemple #2
0
        public void Reset(Contact[] contacts, int contactCount, float impulseRatio)
        {
            _contacts = contacts;

            _constraintCount = contactCount;

            // grow the array
            if (_constraints == null || _constraints.Length < _constraintCount)
            {
                _constraints = new ContactConstraint[_constraintCount * 2];
            }

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

                Fixture  fixtureA = contact._fixtureA;
                Fixture  fixtureB = contact._fixtureB;
                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(out manifold);

                float friction    = Settings.b2MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction());
                float restitution = Settings.b2MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution());

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

                //Debug.Assert(manifold._pointCount > 0);

                WorldManifold worldManifold = new WorldManifold(ref manifold, ref bodyA._xf, radiusA, ref bodyB._xf, radiusB);

                ContactConstraint cc = _constraints[i];
                cc.bodyA      = bodyA;
                cc.bodyB      = bodyB;
                cc.manifold   = manifold;
                cc.normal     = worldManifold._normal;
                cc.pointCount = manifold._pointCount;
                cc.friction   = friction;

                cc.localNormal = manifold._localNormal;
                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  = impulseRatio * cp.NormalImpulse;
                    ccp.tangentImpulse = impulseRatio * cp.TangentImpulse;

                    ccp.localPoint = cp.LocalPoint;

                    ccp.rA = worldManifold._points[j] - bodyA._sweep.c;
                    ccp.rB = worldManifold._points[j] - bodyB._sweep.c;

#if MATH_OVERLOADS
                    float rnA = MathUtils.Cross(ccp.rA, cc.normal);
                    float rnB = MathUtils.Cross(ccp.rB, cc.normal);
#else
                    float rnA = ccp.rA.x * cc.normal.y - ccp.rA.y * cc.normal.x;
                    float rnB = ccp.rB.x * cc.normal.y - ccp.rB.y * cc.normal.x;
#endif
                    rnA *= rnA;
                    rnB *= rnB;

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

                    //Debug.Assert(kNormal > Settings.b2_epsilon);
                    ccp.normalMass = 1.0f / kNormal;

#if MATH_OVERLOADS
                    Vector2 tangent = MathUtils.Cross(cc.normal, 1.0f);

                    float rtA = MathUtils.Cross(ccp.rA, tangent);
                    float rtB = MathUtils.Cross(ccp.rB, tangent);
#else
                    Vector2 tangent = new Vector2(cc.normal.y, -cc.normal.x);

                    float rtA = ccp.rA.x * tangent.y - ccp.rA.y * tangent.x;
                    float rtB = ccp.rB.x * tangent.y - ccp.rB.y * tangent.x;
#endif
                    rtA *= rtA;
                    rtB *= rtB;
                    float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;

                    //Debug.Assert(kTangent > Settings.b2_epsilon);
                    ccp.tangentMass = 1.0f / kTangent;

                    // Setup a velocity bias for restitution.
                    ccp.velocityBias = 0.0f;
                    float vRel = Vector2.Dot(cc.normal, vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA));
                    if (vRel < -Settings.b2_velocityThreshold)
                    {
                        ccp.velocityBias = -restitution * vRel;
                    }

                    cc.points[j] = ccp;
                }

                // 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 = MathUtils.Cross(ccp1.rA, cc.normal);
                    float rn1B = MathUtils.Cross(ccp1.rB, cc.normal);
                    float rn2A = MathUtils.Cross(ccp2.rA, cc.normal);
                    float rn2B = MathUtils.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          = new Mat22(new Vector2(k11, k12), new Vector2(k12, k22));
                        cc.normalMass = cc.K.GetInverse();
                    }
                    else
                    {
                        // The constraints are redundant, just use one.
                        // TODO_ERIN use deepest?
                        cc.pointCount = 1;
                    }
                }

                _constraints[i] = cc;
            }
        }
Exemple #3
0
        public void SolveVelocityConstraints()
        {
            for (int i = 0; i < _constraintCount; ++i)
            {
                ContactConstraint c = _constraints[i];
                Body    bodyA       = c.bodyA;
                Body    bodyB       = c.bodyB;
                float   wA          = bodyA._angularVelocity;
                float   wB          = bodyB._angularVelocity;
                Vector2 vA          = bodyA._linearVelocity;
                Vector2 vB          = bodyB._linearVelocity;
                float   invMassA    = bodyA._invMass;
                float   invIA       = bodyA._invI;
                float   invMassB    = bodyB._invMass;
                float   invIB       = bodyB._invI;
                Vector2 normal      = c.normal;

#if MATH_OVERLOADS
                Vector2 tangent = zoomEngine.Physics.Common.Math.Cross(normal, 1.0f);
#else
                Vector2 tangent = new Vector2(normal.y, -normal.x);
#endif
                float friction = c.friction;

                //Debug.Assert(c.pointCount == 1 || c.pointCount == 2);

                // Solve tangent constraints
                for (int j = 0; j < c.pointCount; ++j)
                {
                    ContactConstraintPoint ccp = c.points[j];

#if MATH_OVERLOADS
                    // Relative velocity at contact
                    Vector2 dv = vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA);

                    // Compute tangent force
                    float vt = Vector2.Dot(dv, tangent);
#else
                    // Relative velocity at contact
                    Vector2 dv = new Vector2(vB.x + (-wB * ccp.rB.y) - vA.x - (-wA * ccp.rA.y),
                                             vB.y + (wB * ccp.rB.x) - vA.y - (wA * ccp.rA.x));

                    // Compute tangent force
                    float vt = dv.x * tangent.x + dv.y * tangent.y;
#endif
                    float lambda = ccp.tangentMass * (-vt);

                    // MathUtils.Clamp the accumulated force
                    float maxFriction = friction * ccp.normalImpulse;
                    float newImpulse  = MathUtils.Clamp(ccp.tangentImpulse + lambda, -maxFriction, maxFriction);
                    lambda = newImpulse - ccp.tangentImpulse;

#if MATH_OVERLOADS
                    // Apply contact impulse
                    Vector2 P = lambda * tangent;

                    vA -= invMassA * P;
                    wA -= invIA * MathUtils.Cross(ccp.rA, P);

                    vB += invMassB * P;
                    wB += invIB * MathUtils.Cross(ccp.rB, P);
#else
                    // Apply contact impulse
                    Vector2 P = new Vector2(lambda * tangent.x, lambda * tangent.y);

                    vA.x -= invMassA * P.x;
                    vA.y -= invMassA * P.y;
                    wA   -= invIA * (ccp.rA.x * P.y - ccp.rA.y * P.x);

                    vB.x += invMassB * P.x;
                    vB.y += invMassB * P.y;
                    wB   += invIB * (ccp.rB.x * P.y - ccp.rB.y * P.x);
#endif
                    ccp.tangentImpulse = newImpulse;
                    c.points[j]        = ccp;
                }

                // Solve normal constraints
                if (c.pointCount == 1)
                {
                    ContactConstraintPoint ccp = c.points[0];

#if MATH_OVERLOADS
                    // Relative velocity at contact
                    Vector2 dv = vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA);

                    // Compute normal impulse
                    float vn     = Vector2.Dot(dv, normal);
                    float lambda = -ccp.normalMass * (vn - ccp.velocityBias);

                    // MathUtils.Clamp the accumulated impulse
                    float newImpulse = Math.Max(ccp.normalImpulse + lambda, 0.0f);
                    lambda = newImpulse - ccp.normalImpulse;

                    // Apply contact impulse
                    Vector2 P = lambda * normal;
                    vA -= invMassA * P;
                    wA -= invIA * MathUtils.Cross(ccp.rA, P);

                    vB += invMassB * P;
                    wB += invIB * MathUtils.Cross(ccp.rB, P);
#else
                    // Relative velocity at contact
                    Vector2 dv = new Vector2(vB.x + (-wB * ccp.rB.y) - vA.x - (-wA * ccp.rA.y),
                                             vB.y + (wB * ccp.rB.x) - vA.y - (wA * ccp.rA.x));

                    // Compute normal impulse
                    float vn     = dv.x * normal.x + dv.y * normal.y;
                    float lambda = -ccp.normalMass * (vn - ccp.velocityBias);

                    // Clamp the accumulated impulse
                    float newImpulse = Math.Max(ccp.normalImpulse + lambda, 0.0f);
                    lambda = newImpulse - ccp.normalImpulse;

                    // Apply contact impulse
                    var P = new Vector2(lambda * normal.x, lambda * normal.y);

                    vA.x -= invMassA * P.x;
                    vA.y -= invMassA * P.y;
                    wA   -= invIA * (ccp.rA.x * P.y - ccp.rA.y * P.x);

                    vB.x += invMassB * P.x;
                    vB.y += invMassB * P.y;
                    wB   += invIB * (ccp.rB.x * P.y - ccp.rB.y * P.x);
#endif
                    ccp.normalImpulse = newImpulse;
                    c.points[0]       = ccp;
                }
                else
                {
                    // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
                    // Build the mini LCP for this contact patch
                    //
                    // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
                    //
                    // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
                    // b = vn_0 - velocityBias
                    //
                    // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
                    // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
                    // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
                    // solution that satisfies the problem is chosen.
                    //
                    // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
                    // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
                    //
                    // Substitute:
                    //
                    // x = x' - a
                    //
                    // Plug into above equation:
                    //
                    // vn = A * x + b
                    //    = A * (x' - a) + b
                    //    = A * x' + b - A * a
                    //    = A * x' + b'
                    // b' = b - A * a;

                    ContactConstraintPoint cp1 = c.points[0];
                    ContactConstraintPoint cp2 = c.points[1];

                    Vector2 a = new Vector2(cp1.normalImpulse, cp2.normalImpulse);
                    //Debug.Assert(a.x >= 0.0f && a.y >= 0.0f);

#if MATH_OVERLOADS
                    // Relative velocity at contact
                    Vector2 dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
                    Vector2 dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);

                    // Compute normal velocity
                    float vn1 = Vector2.Dot(dv1, normal);
                    float vn2 = Vector2.Dot(dv2, normal);

                    Vector2 b = new Vector2(vn1 - cp1.velocityBias, vn2 - cp2.velocityBias);
                    b -= MathUtils.Multiply(ref c.K, a);
#else
                    // Relative velocity at contact
                    Vector2 dv1 = new Vector2(vB.x + (-wB * cp1.rB.y) - vA.x - (-wA * cp1.rA.y),
                                              vB.y + (wB * cp1.rB.x) - vA.y - (wA * cp1.rA.x));
                    Vector2 dv2 = new Vector2(vB.x + (-wB * cp2.rB.y) - vA.x - (-wA * cp2.rA.y),
                                              vB.y + (wB * cp2.rB.x) - vA.y - (wA * cp2.rA.x));

                    // Compute normal velocity
                    float vn1 = dv1.x * normal.x + dv1.y * normal.y;
                    float vn2 = dv2.x * normal.x + dv2.y * normal.y;

                    Vector2 b = new Vector2(vn1 - cp1.velocityBias, vn2 - cp2.velocityBias);
                    b -= MathUtils.Multiply(ref c.K, a); // Inlining didn't help for the multiply.
#endif
                    while (true)
                    {
                        //
                        // Case 1: vn = 0
                        //
                        // 0 = A * x' + b'
                        //
                        // Solve for x':
                        //
                        // x' = - inv(A) * b'
                        //
                        Vector2 x = -MathUtils.Multiply(ref c.normalMass, b);

                        if (x.x >= 0.0f && x.y >= 0.0f)
                        {
#if MATH_OVERLOADS
                            // Resubstitute for the incremental impulse
                            Vector2 d = x - a;

                            // Apply incremental impulse
                            Vector2 P1 = d.x * normal;
                            Vector2 P2 = d.y * normal;
                            vA -= invMassA * (P1 + P2);
                            wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));

                            vB += invMassB * (P1 + P2);
                            wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
#else
                            // Resubstitute for the incremental impulse
                            Vector2 d = new Vector2(x.x - a.x, x.y - a.y);

                            // Apply incremental impulse
                            Vector2 P1  = new Vector2(d.x * normal.x, d.x * normal.y);
                            Vector2 P2  = new Vector2(d.y * normal.x, d.y * normal.y);
                            Vector2 P12 = new Vector2(P1.x + P2.x, P1.y + P2.y);

                            vA.x -= invMassA * P12.x;
                            vA.y -= invMassA * P12.y;
                            wA   -= invIA * ((cp1.rA.x * P1.y - cp1.rA.y * P1.x) + (cp2.rA.x * P2.y - cp2.rA.y * P2.x));

                            vB.x += invMassB * P12.x;
                            vB.y += invMassB * P12.y;
                            wB   += invIB * ((cp1.rB.x * P1.y - cp1.rB.y * P1.x) + (cp2.rB.x * P2.y - cp2.rB.y * P2.x));
#endif
                            // Accumulate
                            cp1.normalImpulse = x.x;
                            cp2.normalImpulse = x.y;

#if B2_DEBUG_SOLVER
                            float k_errorTol = 1e-3f;

                            // Postconditions
                            dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
                            dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);

                            // Compute normal velocity
                            vn1 = Vector2.Dot(dv1, normal);
                            vn2 = Vector2.Dot(dv2, normal);

                            //Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol);
                            //Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol);
#endif
                            break;
                        }

                        //
                        // Case 2: vn1 = 0 and x2 = 0
                        //
                        //   0 = a11 * x1' + a12 * 0 + b1'
                        // vn2 = a21 * x1' + a22 * 0 + b2'
                        //
                        x.x = -cp1.normalMass * b.x;
                        x.y = 0.0f;
                        vn1 = 0.0f;
                        vn2 = c.K.col1.y * x.x + b.y;

                        if (x.x >= 0.0f && vn2 >= 0.0f)
                        {
#if MATH_OVERLOADS
                            // Resubstitute for the incremental impulse
                            Vector2 d = x - a;

                            // Apply incremental impulse
                            Vector2 P1 = d.x * normal;
                            Vector2 P2 = d.y * normal;
                            vA -= invMassA * (P1 + P2);
                            wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));

                            vB += invMassB * (P1 + P2);
                            wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
#else
                            // Resubstitute for the incremental impulse
                            Vector2 d = new Vector2(x.x - a.x, x.y - a.y);

                            // Apply incremental impulse
                            Vector2 P1  = new Vector2(d.x * normal.x, d.x * normal.y);
                            Vector2 P2  = new Vector2(d.y * normal.x, d.y * normal.y);
                            Vector2 P12 = new Vector2(P1.x + P2.x, P1.y + P2.y);

                            vA.x -= invMassA * P12.x;
                            vA.y -= invMassA * P12.y;
                            wA   -= invIA * ((cp1.rA.x * P1.y - cp1.rA.y * P1.x) + (cp2.rA.x * P2.y - cp2.rA.y * P2.x));

                            vB.x += invMassB * P12.x;
                            vB.y += invMassB * P12.y;
                            wB   += invIB * ((cp1.rB.x * P1.y - cp1.rB.y * P1.x) + (cp2.rB.x * P2.y - cp2.rB.y * P2.x));
#endif
                            // Accumulate
                            cp1.normalImpulse = x.x;
                            cp2.normalImpulse = x.y;

        #if B2_DEBUG_SOLVER
                            // Postconditions
                            dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);

                            // Compute normal velocity
                            vn1 = Vector2.Dot(dv1, normal);

                            //Debug.Assert(MathUtils.Abs(vn1 - cp1.velocityBias) < k_errorTol);
        #endif
                            break;
                        }


                        //
                        // Case 3: vn2 = 0 and x1 = 0
                        //
                        // vn1 = a11 * 0 + a12 * x2' + b1'
                        //   0 = a21 * 0 + a22 * x2' + b2'
                        //
                        x.x = 0.0f;
                        x.y = -cp2.normalMass * b.y;
                        vn1 = c.K.col2.x * x.y + b.x;
                        vn2 = 0.0f;

                        if (x.y >= 0.0f && vn1 >= 0.0f)
                        {
#if MATH_OVERLOADS
                            // Resubstitute for the incremental impulse
                            Vector2 d = x - a;

                            // Apply incremental impulse
                            Vector2 P1 = d.x * normal;
                            Vector2 P2 = d.y * normal;
                            vA -= invMassA * (P1 + P2);
                            wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));

                            vB += invMassB * (P1 + P2);
                            wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
#else
                            // Resubstitute for the incremental impulse
                            Vector2 d = new Vector2(x.x - a.x, x.y - a.y);

                            // Apply incremental impulse
                            Vector2 P1  = new Vector2(d.x * normal.x, d.x * normal.y);
                            Vector2 P2  = new Vector2(d.y * normal.x, d.y * normal.y);
                            Vector2 P12 = new Vector2(P1.x + P2.x, P1.y + P2.y);

                            vA.x -= invMassA * P12.x;
                            vA.y -= invMassA * P12.y;
                            wA   -= invIA * ((cp1.rA.x * P1.y - cp1.rA.y * P1.x) + (cp2.rA.x * P2.y - cp2.rA.y * P2.x));

                            vB.x += invMassB * P12.x;
                            vB.y += invMassB * P12.y;
                            wB   += invIB * ((cp1.rB.x * P1.y - cp1.rB.y * P1.x) + (cp2.rB.x * P2.y - cp2.rB.y * P2.x));
#endif
                            // Accumulate
                            cp1.normalImpulse = x.x;
                            cp2.normalImpulse = x.y;

        #if B2_DEBUG_SOLVER
                            // Postconditions
                            dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);

                            // Compute normal velocity
                            vn2 = Vector2.Dot(dv2, normal);

                            //Debug.Assert(MathUtils.Abs(vn2 - cp2.velocityBias) < k_errorTol);
        #endif
                            break;
                        }

                        //
                        // Case 4: x1 = 0 and x2 = 0
                        //
                        // vn1 = b1
                        // vn2 = b2;
                        x.x = 0.0f;
                        x.y = 0.0f;
                        vn1 = b.x;
                        vn2 = b.y;

                        if (vn1 >= 0.0f && vn2 >= 0.0f)
                        {
#if MATH_OVERLOADS
                            // Resubstitute for the incremental impulse
                            Vector2 d = x - a;

                            // Apply incremental impulse
                            Vector2 P1 = d.x * normal;
                            Vector2 P2 = d.y * normal;
                            vA -= invMassA * (P1 + P2);
                            wA -= invIA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));

                            vB += invMassB * (P1 + P2);
                            wB += invIB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
#else
                            // Resubstitute for the incremental impulse
                            Vector2 d = new Vector2(x.x - a.x, x.y - a.y);

                            // Apply incremental impulse
                            Vector2 P1  = new Vector2(d.x * normal.x, d.x * normal.y);
                            Vector2 P2  = new Vector2(d.y * normal.x, d.y * normal.y);
                            Vector2 P12 = new Vector2(P1.x + P2.x, P1.y + P2.y);

                            vA.x -= invMassA * P12.x;
                            vA.y -= invMassA * P12.y;
                            wA   -= invIA * ((cp1.rA.x * P1.y - cp1.rA.y * P1.x) + (cp2.rA.x * P2.y - cp2.rA.y * P2.x));

                            vB.x += invMassB * P12.x;
                            vB.y += invMassB * P12.y;
                            wB   += invIB * ((cp1.rB.x * P1.y - cp1.rB.y * P1.x) + (cp2.rB.x * P2.y - cp2.rB.y * P2.x));
#endif
                            // Accumulate
                            cp1.normalImpulse = x.x;
                            cp2.normalImpulse = x.y;

                            break;
                        }

                        // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
                        break;
                    }

                    c.points[0] = cp1;
                    c.points[1] = cp2;
                }

                _constraints[i] = c;

                bodyA._linearVelocity  = vA;
                bodyA._angularVelocity = wA;
                bodyB._linearVelocity  = vB;
                bodyB._angularVelocity = wB;
            }
        }