Examples with VelocityConstraintPoint org.jbox2d.dynamics.contacts.ContactVelocityConstraint.VelocityConstraintPoint used on opensource projects

Example 1 with VelocityConstraintPoint

use of org.jbox2d.dynamics.contacts.ContactVelocityConstraint.VelocityConstraintPoint in project libgdx by libgdx.

the class PositionSolverManifold method initializeVelocityConstraints.

public final void initializeVelocityConstraints() {
    // Warm start.
    for (int i = 0; i < m_count; ++i) {
        ContactVelocityConstraint vc = m_velocityConstraints[i];
        ContactPositionConstraint pc = m_positionConstraints[i];
        float radiusA = pc.radiusA;
        float radiusB = pc.radiusB;
        Manifold manifold = m_contacts[vc.contactIndex].getManifold();
        int indexA = vc.indexA;
        int indexB = vc.indexB;
        float mA = vc.invMassA;
        float mB = vc.invMassB;
        float iA = vc.invIA;
        float iB = vc.invIB;
        Vec2 localCenterA = pc.localCenterA;
        Vec2 localCenterB = pc.localCenterB;
        Vec2 cA = m_positions[indexA].c;
        float aA = m_positions[indexA].a;
        Vec2 vA = m_velocities[indexA].v;
        float wA = m_velocities[indexA].w;
        Vec2 cB = m_positions[indexB].c;
        float aB = m_positions[indexB].a;
        Vec2 vB = m_velocities[indexB].v;
        float wB = m_velocities[indexB].w;
        assert (manifold.pointCount > 0);
        final Rot xfAq = xfA.q;
        final Rot xfBq = xfB.q;
        xfAq.set(aA);
        xfBq.set(aB);
        xfA.p.x = cA.x - (xfAq.c * localCenterA.x - xfAq.s * localCenterA.y);
        xfA.p.y = cA.y - (xfAq.s * localCenterA.x + xfAq.c * localCenterA.y);
        xfB.p.x = cB.x - (xfBq.c * localCenterB.x - xfBq.s * localCenterB.y);
        xfB.p.y = cB.y - (xfBq.s * localCenterB.x + xfBq.c * localCenterB.y);
        worldManifold.initialize(manifold, xfA, radiusA, xfB, radiusB);
        final Vec2 vcnormal = vc.normal;
        vcnormal.x = worldManifold.normal.x;
        vcnormal.y = worldManifold.normal.y;
        int pointCount = vc.pointCount;
        for (int j = 0; j < pointCount; ++j) {
            VelocityConstraintPoint vcp = vc.points[j];
            Vec2 wmPj = worldManifold.points[j];
            final Vec2 vcprA = vcp.rA;
            final Vec2 vcprB = vcp.rB;
            vcprA.x = wmPj.x - cA.x;
            vcprA.y = wmPj.y - cA.y;
            vcprB.x = wmPj.x - cB.x;
            vcprB.y = wmPj.y - cB.y;
            float rnA = vcprA.x * vcnormal.y - vcprA.y * vcnormal.x;
            float rnB = vcprB.x * vcnormal.y - vcprB.y * vcnormal.x;
            float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
            vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
            float tangentx = 1.0f * vcnormal.y;
            float tangenty = -1.0f * vcnormal.x;
            float rtA = vcprA.x * tangenty - vcprA.y * tangentx;
            float rtB = vcprB.x * tangenty - vcprB.y * tangentx;
            float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
            vcp.tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
            // Setup a velocity bias for restitution.
            vcp.velocityBias = 0.0f;
            float tempx = vB.x + -wB * vcprB.y - vA.x - (-wA * vcprA.y);
            float tempy = vB.y + wB * vcprB.x - vA.y - (wA * vcprA.x);
            float vRel = vcnormal.x * tempx + vcnormal.y * tempy;
            if (vRel < -Settings.velocityThreshold) {
                vcp.velocityBias = -vc.restitution * vRel;
            }
        }
        // If we have two points, then prepare the block solver.
        if (vc.pointCount == 2) {
            VelocityConstraintPoint vcp1 = vc.points[0];
            VelocityConstraintPoint vcp2 = vc.points[1];
            float rn1A = vcp1.rA.x * vcnormal.y - vcp1.rA.y * vcnormal.x;
            float rn1B = vcp1.rB.x * vcnormal.y - vcp1.rB.y * vcnormal.x;
            float rn2A = vcp2.rA.x * vcnormal.y - vcp2.rA.y * vcnormal.x;
            float rn2B = vcp2.rB.x * vcnormal.y - vcp2.rB.y * vcnormal.x;
            float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
            float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
            float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
            if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) {
                // K is safe to invert.
                vc.K.ex.x = k11;
                vc.K.ex.y = k12;
                vc.K.ey.x = k12;
                vc.K.ey.y = k22;
                vc.K.invertToOut(vc.normalMass);
            } else {
                // The constraints are redundant, just use one.
                // TODO_ERIN use deepest?
                vc.pointCount = 1;
            }
        }
    }
}

Example 2 with VelocityConstraintPoint

use of org.jbox2d.dynamics.contacts.ContactVelocityConstraint.VelocityConstraintPoint in project libgdx by libgdx.

the class PositionSolverManifold method init.

public final void init(ContactSolverDef def) {
    // System.out.println("Initializing contact solver");
    m_step = def.step;
    m_count = def.count;
    if (m_positionConstraints.length < m_count) {
        ContactPositionConstraint[] old = m_positionConstraints;
        m_positionConstraints = new ContactPositionConstraint[MathUtils.max(old.length * 2, m_count)];
        System.arraycopy(old, 0, m_positionConstraints, 0, old.length);
        for (int i = old.length; i < m_positionConstraints.length; i++) {
            m_positionConstraints[i] = new ContactPositionConstraint();
        }
    }
    if (m_velocityConstraints.length < m_count) {
        ContactVelocityConstraint[] old = m_velocityConstraints;
        m_velocityConstraints = new ContactVelocityConstraint[MathUtils.max(old.length * 2, m_count)];
        System.arraycopy(old, 0, m_velocityConstraints, 0, old.length);
        for (int i = old.length; i < m_velocityConstraints.length; i++) {
            m_velocityConstraints[i] = new ContactVelocityConstraint();
        }
    }
    m_positions = def.positions;
    m_velocities = def.velocities;
    m_contacts = def.contacts;
    for (int i = 0; i < m_count; ++i) {
        // System.out.println("contacts: " + m_count);
        final Contact contact = m_contacts[i];
        final Fixture fixtureA = contact.m_fixtureA;
        final Fixture fixtureB = contact.m_fixtureB;
        final Shape shapeA = fixtureA.getShape();
        final Shape shapeB = fixtureB.getShape();
        final float radiusA = shapeA.m_radius;
        final float radiusB = shapeB.m_radius;
        final Body bodyA = fixtureA.getBody();
        final Body bodyB = fixtureB.getBody();
        final Manifold manifold = contact.getManifold();
        int pointCount = manifold.pointCount;
        assert (pointCount > 0);
        ContactVelocityConstraint vc = m_velocityConstraints[i];
        vc.friction = contact.m_friction;
        vc.restitution = contact.m_restitution;
        vc.tangentSpeed = contact.m_tangentSpeed;
        vc.indexA = bodyA.m_islandIndex;
        vc.indexB = bodyB.m_islandIndex;
        vc.invMassA = bodyA.m_invMass;
        vc.invMassB = bodyB.m_invMass;
        vc.invIA = bodyA.m_invI;
        vc.invIB = bodyB.m_invI;
        vc.contactIndex = i;
        vc.pointCount = pointCount;
        vc.K.setZero();
        vc.normalMass.setZero();
        ContactPositionConstraint pc = m_positionConstraints[i];
        pc.indexA = bodyA.m_islandIndex;
        pc.indexB = bodyB.m_islandIndex;
        pc.invMassA = bodyA.m_invMass;
        pc.invMassB = bodyB.m_invMass;
        pc.localCenterA.set(bodyA.m_sweep.localCenter);
        pc.localCenterB.set(bodyB.m_sweep.localCenter);
        pc.invIA = bodyA.m_invI;
        pc.invIB = bodyB.m_invI;
        pc.localNormal.set(manifold.localNormal);
        pc.localPoint.set(manifold.localPoint);
        pc.pointCount = pointCount;
        pc.radiusA = radiusA;
        pc.radiusB = radiusB;
        pc.type = manifold.type;
        // System.out.println("contact point count: " + pointCount);
        for (int j = 0; j < pointCount; j++) {
            ManifoldPoint cp = manifold.points[j];
            VelocityConstraintPoint vcp = vc.points[j];
            if (m_step.warmStarting) {
                // assert(cp.normalImpulse == 0);
                // System.out.println("contact normal impulse: " + cp.normalImpulse);
                vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse;
                vcp.tangentImpulse = m_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].x = cp.localPoint.x;
            pc.localPoints[j].y = cp.localPoint.y;
        }
    }
}

Example 3 with VelocityConstraintPoint

use of org.jbox2d.dynamics.contacts.ContactVelocityConstraint.VelocityConstraintPoint in project libgdx by libgdx.

the class PositionSolverManifold method warmStart.

public void warmStart() {
    // Warm start.
    for (int i = 0; i < m_count; ++i) {
        final ContactVelocityConstraint vc = m_velocityConstraints[i];
        int indexA = vc.indexA;
        int indexB = vc.indexB;
        float mA = vc.invMassA;
        float iA = vc.invIA;
        float mB = vc.invMassB;
        float iB = vc.invIB;
        int pointCount = vc.pointCount;
        Vec2 vA = m_velocities[indexA].v;
        float wA = m_velocities[indexA].w;
        Vec2 vB = m_velocities[indexB].v;
        float wB = m_velocities[indexB].w;
        Vec2 normal = vc.normal;
        float tangentx = 1.0f * normal.y;
        float tangenty = -1.0f * normal.x;
        for (int j = 0; j < pointCount; ++j) {
            VelocityConstraintPoint vcp = vc.points[j];
            float Px = tangentx * vcp.tangentImpulse + normal.x * vcp.normalImpulse;
            float Py = tangenty * vcp.tangentImpulse + normal.y * vcp.normalImpulse;
            wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px);
            vA.x -= Px * mA;
            vA.y -= Py * mA;
            wB += iB * (vcp.rB.x * Py - vcp.rB.y * Px);
            vB.x += Px * mB;
            vB.y += Py * mB;
        }
        m_velocities[indexA].w = wA;
        m_velocities[indexB].w = wB;
    }
}

Example 4 with VelocityConstraintPoint

use of org.jbox2d.dynamics.contacts.ContactVelocityConstraint.VelocityConstraintPoint in project libgdx by libgdx.

the class PositionSolverManifold method solveVelocityConstraints.

public final void solveVelocityConstraints() {
    for (int i = 0; i < m_count; ++i) {
        final ContactVelocityConstraint vc = m_velocityConstraints[i];
        int indexA = vc.indexA;
        int indexB = vc.indexB;
        float mA = vc.invMassA;
        float mB = vc.invMassB;
        float iA = vc.invIA;
        float iB = vc.invIB;
        int pointCount = vc.pointCount;
        Vec2 vA = m_velocities[indexA].v;
        float wA = m_velocities[indexA].w;
        Vec2 vB = m_velocities[indexB].v;
        float wB = m_velocities[indexB].w;
        Vec2 normal = vc.normal;
        final float normalx = normal.x;
        final float normaly = normal.y;
        float tangentx = 1.0f * vc.normal.y;
        float tangenty = -1.0f * vc.normal.x;
        final float friction = vc.friction;
        assert (pointCount == 1 || pointCount == 2);
        // Solve tangent constraints
        for (int j = 0; j < pointCount; ++j) {
            final VelocityConstraintPoint vcp = vc.points[j];
            final Vec2 a = vcp.rA;
            float dvx = -wB * vcp.rB.y + vB.x - vA.x + wA * a.y;
            float dvy = wB * vcp.rB.x + vB.y - vA.y - wA * a.x;
            // Compute tangent force
            final float vt = dvx * tangentx + dvy * tangenty - vc.tangentSpeed;
            float lambda = vcp.tangentMass * (-vt);
            // Clamp the accumulated force
            final float maxFriction = friction * vcp.normalImpulse;
            final float newImpulse = MathUtils.clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction);
            lambda = newImpulse - vcp.tangentImpulse;
            vcp.tangentImpulse = newImpulse;
            // Apply contact impulse
            // Vec2 P = lambda * tangent;
            final float Px = tangentx * lambda;
            final float Py = tangenty * lambda;
            // vA -= invMassA * P;
            vA.x -= Px * mA;
            vA.y -= Py * mA;
            wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px);
            // vB += invMassB * P;
            vB.x += Px * mB;
            vB.y += Py * mB;
            wB += iB * (vcp.rB.x * Py - vcp.rB.y * Px);
        }
        // Solve normal constraints
        if (vc.pointCount == 1) {
            final VelocityConstraintPoint vcp = vc.points[0];
            // Relative velocity at contact
            // Vec2 dv = vB + Cross(wB, vcp.rB) - vA - Cross(wA, vcp.rA);
            float dvx = -wB * vcp.rB.y + vB.x - vA.x + wA * vcp.rA.y;
            float dvy = wB * vcp.rB.x + vB.y - vA.y - wA * vcp.rA.x;
            // Compute normal impulse
            final float vn = dvx * normalx + dvy * normaly;
            float lambda = -vcp.normalMass * (vn - vcp.velocityBias);
            // Clamp the accumulated impulse
            float a = vcp.normalImpulse + lambda;
            final float newImpulse = (a > 0.0f ? a : 0.0f);
            lambda = newImpulse - vcp.normalImpulse;
            vcp.normalImpulse = newImpulse;
            // Apply contact impulse
            float Px = normalx * lambda;
            float Py = normaly * lambda;
            // vA -= invMassA * P;
            vA.x -= Px * mA;
            vA.y -= Py * mA;
            wA -= iA * (vcp.rA.x * Py - vcp.rA.y * Px);
            // vB += invMassB * P;
            vB.x += Px * mB;
            vB.y += Py * mB;
            wB += iB * (vcp.rB.x * Py - vcp.rB.y * Px);
        } 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 = a + d
            //
            // a := old total impulse
            // x := new total impulse
            // d := incremental impulse
            //
            // For the current iteration we extend the formula for the incremental impulse
            // to compute the new total impulse:
            //
            // vn = A * d + b
            // = A * (x - a) + b
            // = A * x + b - A * a
            // = A * x + b'
            // b' = b - A * a;
            final VelocityConstraintPoint cp1 = vc.points[0];
            final VelocityConstraintPoint cp2 = vc.points[1];
            final Vec2 cp1rA = cp1.rA;
            final Vec2 cp1rB = cp1.rB;
            final Vec2 cp2rA = cp2.rA;
            final Vec2 cp2rB = cp2.rB;
            float ax = cp1.normalImpulse;
            float ay = cp2.normalImpulse;
            assert (ax >= 0.0f && ay >= 0.0f);
            // Relative velocity at contact
            // Vec2 dv1 = vB + Cross(wB, cp1.rB) - vA - Cross(wA, cp1.rA);
            float dv1x = -wB * cp1rB.y + vB.x - vA.x + wA * cp1rA.y;
            float dv1y = wB * cp1rB.x + vB.y - vA.y - wA * cp1rA.x;
            // Vec2 dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA);
            float dv2x = -wB * cp2rB.y + vB.x - vA.x + wA * cp2rA.y;
            float dv2y = wB * cp2rB.x + vB.y - vA.y - wA * cp2rA.x;
            // Compute normal velocity
            float vn1 = dv1x * normalx + dv1y * normaly;
            float vn2 = dv2x * normalx + dv2y * normaly;
            float bx = vn1 - cp1.velocityBias;
            float by = vn2 - cp2.velocityBias;
            // Compute b'
            Mat22 R = vc.K;
            bx -= R.ex.x * ax + R.ey.x * ay;
            by -= R.ex.y * ax + R.ey.y * ay;
            // B2_NOT_USED(k_errorTol);
            for (; ; ) {
                //
                // Case 1: vn = 0
                //
                // 0 = A * x' + b'
                //
                // Solve for x':
                //
                // x' = - inv(A) * b'
                //
                // Vec2 x = - Mul(c.normalMass, b);
                Mat22 R1 = vc.normalMass;
                float xx = R1.ex.x * bx + R1.ey.x * by;
                float xy = R1.ex.y * bx + R1.ey.y * by;
                xx *= -1;
                xy *= -1;
                if (xx >= 0.0f && xy >= 0.0f) {
                    // Get the incremental impulse
                    // Vec2 d = x - a;
                    float dx = xx - ax;
                    float dy = xy - ay;
                    // Apply incremental impulse
                    // Vec2 P1 = d.x * normal;
                    // Vec2 P2 = d.y * normal;
                    float P1x = dx * normalx;
                    float P1y = dx * normaly;
                    float P2x = dy * normalx;
                    float P2y = dy * normaly;
                    /*
             * vA -= invMassA * (P1 + P2); wA -= invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
             * 
             * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
             */
                    vA.x -= mA * (P1x + P2x);
                    vA.y -= mA * (P1y + P2y);
                    vB.x += mB * (P1x + P2x);
                    vB.y += mB * (P1y + P2y);
                    wA -= iA * (cp1rA.x * P1y - cp1rA.y * P1x + (cp2rA.x * P2y - cp2rA.y * P2x));
                    wB += iB * (cp1rB.x * P1y - cp1rB.y * P1x + (cp2rB.x * P2y - cp2rB.y * P2x));
                    // Accumulate
                    cp1.normalImpulse = xx;
                    cp2.normalImpulse = xy;
                    /*
             * #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA -
             * Cross(wA, cp1.rA); dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA);
             * 
             * // Compute normal velocity vn1 = Dot(dv1, normal); vn2 = Dot(dv2, normal);
             * 
             * assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); assert(Abs(vn2 - cp2.velocityBias)
             * < k_errorTol); #endif
             */
                    if (DEBUG_SOLVER) {
                        // Postconditions
                        Vec2 dv1 = vB.add(Vec2.cross(wB, cp1rB).subLocal(vA).subLocal(Vec2.cross(wA, cp1rA)));
                        Vec2 dv2 = vB.add(Vec2.cross(wB, cp2rB).subLocal(vA).subLocal(Vec2.cross(wA, cp2rA)));
                        // Compute normal velocity
                        vn1 = Vec2.dot(dv1, normal);
                        vn2 = Vec2.dot(dv2, normal);
                        assert (MathUtils.abs(vn1 - cp1.velocityBias) < k_errorTol);
                        assert (MathUtils.abs(vn2 - cp2.velocityBias) < k_errorTol);
                    }
                    break;
                }
                //
                // Case 2: vn1 = 0 and x2 = 0
                //
                // 0 = a11 * x1' + a12 * 0 + b1'
                // vn2 = a21 * x1' + a22 * 0 + '
                //
                xx = -cp1.normalMass * bx;
                xy = 0.0f;
                vn1 = 0.0f;
                vn2 = vc.K.ex.y * xx + by;
                if (xx >= 0.0f && vn2 >= 0.0f) {
                    // Get the incremental impulse
                    float dx = xx - ax;
                    float dy = xy - ay;
                    // Apply incremental impulse
                    // Vec2 P1 = d.x * normal;
                    // Vec2 P2 = d.y * normal;
                    float P1x = normalx * dx;
                    float P1y = normaly * dx;
                    float P2x = normalx * dy;
                    float P2y = normaly * dy;
                    /*
             * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
             * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
             * 
             * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
             */
                    vA.x -= mA * (P1x + P2x);
                    vA.y -= mA * (P1y + P2y);
                    vB.x += mB * (P1x + P2x);
                    vB.y += mB * (P1y + P2y);
                    wA -= iA * (cp1rA.x * P1y - cp1rA.y * P1x + (cp2rA.x * P2y - cp2rA.y * P2x));
                    wB += iB * (cp1rB.x * P1y - cp1rB.y * P1x + (cp2rB.x * P2y - cp2rB.y * P2x));
                    // Accumulate
                    cp1.normalImpulse = xx;
                    cp2.normalImpulse = xy;
                    /*
             * #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA -
             * Cross(wA, cp1.rA);
             * 
             * // Compute normal velocity vn1 = Dot(dv1, normal);
             * 
             * assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); #endif
             */
                    if (DEBUG_SOLVER) {
                        // Postconditions
                        Vec2 dv1 = vB.add(Vec2.cross(wB, cp1rB).subLocal(vA).subLocal(Vec2.cross(wA, cp1rA)));
                        // Compute normal velocity
                        vn1 = Vec2.dot(dv1, normal);
                        assert (MathUtils.abs(vn1 - cp1.velocityBias) < k_errorTol);
                    }
                    break;
                }
                //
                // Case 3: wB = 0 and x1 = 0
                //
                // vn1 = a11 * 0 + a12 * x2' + b1'
                // 0 = a21 * 0 + a22 * x2' + '
                //
                xx = 0.0f;
                xy = -cp2.normalMass * by;
                vn1 = vc.K.ey.x * xy + bx;
                vn2 = 0.0f;
                if (xy >= 0.0f && vn1 >= 0.0f) {
                    // Resubstitute for the incremental impulse
                    float dx = xx - ax;
                    float dy = xy - ay;
                    // Apply incremental impulse
                    /*
             * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
             * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
             * 
             * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
             */
                    float P1x = normalx * dx;
                    float P1y = normaly * dx;
                    float P2x = normalx * dy;
                    float P2y = normaly * dy;
                    vA.x -= mA * (P1x + P2x);
                    vA.y -= mA * (P1y + P2y);
                    vB.x += mB * (P1x + P2x);
                    vB.y += mB * (P1y + P2y);
                    wA -= iA * (cp1rA.x * P1y - cp1rA.y * P1x + (cp2rA.x * P2y - cp2rA.y * P2x));
                    wB += iB * (cp1rB.x * P1y - cp1rB.y * P1x + (cp2rB.x * P2y - cp2rB.y * P2x));
                    // Accumulate
                    cp1.normalImpulse = xx;
                    cp2.normalImpulse = xy;
                    /*
             * #if B2_DEBUG_SOLVER == 1 // Postconditions dv2 = vB + Cross(wB, cp2.rB) - vA -
             * Cross(wA, cp2.rA);
             * 
             * // Compute normal velocity vn2 = Dot(dv2, normal);
             * 
             * assert(Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif
             */
                    if (DEBUG_SOLVER) {
                        // Postconditions
                        Vec2 dv2 = vB.add(Vec2.cross(wB, cp2rB).subLocal(vA).subLocal(Vec2.cross(wA, cp2rA)));
                        // Compute normal velocity
                        vn2 = Vec2.dot(dv2, normal);
                        assert (MathUtils.abs(vn2 - cp2.velocityBias) < k_errorTol);
                    }
                    break;
                }
                //
                // Case 4: x1 = 0 and x2 = 0
                //
                // vn1 = b1
                // vn2 = ;
                xx = 0.0f;
                xy = 0.0f;
                vn1 = bx;
                vn2 = by;
                if (vn1 >= 0.0f && vn2 >= 0.0f) {
                    // Resubstitute for the incremental impulse
                    float dx = xx - ax;
                    float dy = xy - ay;
                    // Apply incremental impulse
                    /*
             * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
             * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
             * 
             * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
             */
                    float P1x = normalx * dx;
                    float P1y = normaly * dx;
                    float P2x = normalx * dy;
                    float P2y = normaly * dy;
                    vA.x -= mA * (P1x + P2x);
                    vA.y -= mA * (P1y + P2y);
                    vB.x += mB * (P1x + P2x);
                    vB.y += mB * (P1y + P2y);
                    wA -= iA * (cp1rA.x * P1y - cp1rA.y * P1x + (cp2rA.x * P2y - cp2rA.y * P2x));
                    wB += iB * (cp1rB.x * P1y - cp1rB.y * P1x + (cp2rB.x * P2y - cp2rB.y * P2x));
                    // Accumulate
                    cp1.normalImpulse = xx;
                    cp2.normalImpulse = xy;
                    break;
                }
                // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
                break;
            }
        }
        // m_velocities[indexA].v.set(vA);
        m_velocities[indexA].w = wA;
        // m_velocities[indexB].v.set(vB);
        m_velocities[indexB].w = wB;
    }
}