Examples with Mat22 org.jbox2d.common.Mat22 used on opensource projects

Example 1 with Mat22

use of org.jbox2d.common.Mat22 in project libgdx by libgdx.

the class MotorJoint method initVelocityConstraints.

@Override
public void initVelocityConstraints(SolverData data) {
    m_indexA = m_bodyA.m_islandIndex;
    m_indexB = m_bodyB.m_islandIndex;
    m_localCenterA.set(m_bodyA.m_sweep.localCenter);
    m_localCenterB.set(m_bodyB.m_sweep.localCenter);
    m_invMassA = m_bodyA.m_invMass;
    m_invMassB = m_bodyB.m_invMass;
    m_invIA = m_bodyA.m_invI;
    m_invIB = m_bodyB.m_invI;
    final Vec2 cA = data.positions[m_indexA].c;
    float aA = data.positions[m_indexA].a;
    final Vec2 vA = data.velocities[m_indexA].v;
    float wA = data.velocities[m_indexA].w;
    final Vec2 cB = data.positions[m_indexB].c;
    float aB = data.positions[m_indexB].a;
    final Vec2 vB = data.velocities[m_indexB].v;
    float wB = data.velocities[m_indexB].w;
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    final Vec2 temp = pool.popVec2();
    Mat22 K = pool.popMat22();
    qA.set(aA);
    qB.set(aB);
    // Compute the effective mass matrix.
    // m_rA = b2Mul(qA, -m_localCenterA);
    // m_rB = b2Mul(qB, -m_localCenterB);
    m_rA.x = qA.c * -m_localCenterA.x - qA.s * -m_localCenterA.y;
    m_rA.y = qA.s * -m_localCenterA.x + qA.c * -m_localCenterA.y;
    m_rB.x = qB.c * -m_localCenterB.x - qB.s * -m_localCenterB.y;
    m_rB.y = qB.s * -m_localCenterB.x + qB.c * -m_localCenterB.y;
    // J = [-I -r1_skew I r2_skew]
    // [ 0 -1 0 1]
    // r_skew = [-ry; rx]
    // Matlab
    // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
    // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
    // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;
    K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
    K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
    K.ey.x = K.ex.y;
    K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
    K.invertToOut(m_linearMass);
    m_angularMass = iA + iB;
    if (m_angularMass > 0.0f) {
        m_angularMass = 1.0f / m_angularMass;
    }
    // m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset);
    Rot.mulToOutUnsafe(qA, m_linearOffset, temp);
    m_linearError.x = cB.x + m_rB.x - cA.x - m_rA.x - temp.x;
    m_linearError.y = cB.y + m_rB.y - cA.y - m_rA.y - temp.y;
    m_angularError = aB - aA - m_angularOffset;
    if (data.step.warmStarting) {
        // Scale impulses to support a variable time step.
        m_linearImpulse.x *= data.step.dtRatio;
        m_linearImpulse.y *= data.step.dtRatio;
        m_angularImpulse *= data.step.dtRatio;
        final Vec2 P = m_linearImpulse;
        vA.x -= mA * P.x;
        vA.y -= mA * P.y;
        wA -= iA * (m_rA.x * P.y - m_rA.y * P.x + m_angularImpulse);
        vB.x += mB * P.x;
        vB.y += mB * P.y;
        wB += iB * (m_rB.x * P.y - m_rB.y * P.x + m_angularImpulse);
    } else {
        m_linearImpulse.setZero();
        m_angularImpulse = 0.0f;
    }
    pool.pushVec2(1);
    pool.pushMat22(1);
    pool.pushRot(2);
    // data.velocities[m_indexA].v = vA;
    data.velocities[m_indexA].w = wA;
    // data.velocities[m_indexB].v = vB;
    data.velocities[m_indexB].w = wB;
}

Example 2 with Mat22

use of org.jbox2d.common.Mat22 in project libgdx by libgdx.

the class MouseJoint method initVelocityConstraints.

@Override
public void initVelocityConstraints(final SolverData data) {
    m_indexB = m_bodyB.m_islandIndex;
    m_localCenterB.set(m_bodyB.m_sweep.localCenter);
    m_invMassB = m_bodyB.m_invMass;
    m_invIB = m_bodyB.m_invI;
    Vec2 cB = data.positions[m_indexB].c;
    float aB = data.positions[m_indexB].a;
    Vec2 vB = data.velocities[m_indexB].v;
    float wB = data.velocities[m_indexB].w;
    final Rot qB = pool.popRot();
    qB.set(aB);
    float mass = m_bodyB.getMass();
    // Frequency
    float omega = 2.0f * MathUtils.PI * m_frequencyHz;
    // Damping coefficient
    float d = 2.0f * mass * m_dampingRatio * omega;
    // Spring stiffness
    float k = mass * (omega * omega);
    // magic formulas
    // gamma has units of inverse mass.
    // beta has units of inverse time.
    float h = data.step.dt;
    assert (d + h * k > Settings.EPSILON);
    m_gamma = h * (d + h * k);
    if (m_gamma != 0.0f) {
        m_gamma = 1.0f / m_gamma;
    }
    m_beta = h * k * m_gamma;
    Vec2 temp = pool.popVec2();
    // Compute the effective mass matrix.
    Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);
    // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
    // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
    // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
    final Mat22 K = pool.popMat22();
    K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
    K.ex.y = -m_invIB * m_rB.x * m_rB.y;
    K.ey.x = K.ex.y;
    K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
    K.invertToOut(m_mass);
    m_C.set(cB).addLocal(m_rB).subLocal(m_targetA);
    m_C.mulLocal(m_beta);
    // Cheat with some damping
    wB *= 0.98f;
    if (data.step.warmStarting) {
        m_impulse.mulLocal(data.step.dtRatio);
        vB.x += m_invMassB * m_impulse.x;
        vB.y += m_invMassB * m_impulse.y;
        wB += m_invIB * Vec2.cross(m_rB, m_impulse);
    } else {
        m_impulse.setZero();
    }
    //    data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;
    pool.pushVec2(1);
    pool.pushMat22(1);
    pool.pushRot(1);
}

Example 3 with Mat22

use of org.jbox2d.common.Mat22 in project libgdx by libgdx.

the class FrictionJoint method initVelocityConstraints.

/**
   * @see org.jbox2d.dynamics.joints.Joint#initVelocityConstraints(org.jbox2d.dynamics.TimeStep)
   */
@Override
public void initVelocityConstraints(final SolverData data) {
    m_indexA = m_bodyA.m_islandIndex;
    m_indexB = m_bodyB.m_islandIndex;
    m_localCenterA.set(m_bodyA.m_sweep.localCenter);
    m_localCenterB.set(m_bodyB.m_sweep.localCenter);
    m_invMassA = m_bodyA.m_invMass;
    m_invMassB = m_bodyB.m_invMass;
    m_invIA = m_bodyA.m_invI;
    m_invIB = m_bodyB.m_invI;
    float aA = data.positions[m_indexA].a;
    Vec2 vA = data.velocities[m_indexA].v;
    float wA = data.velocities[m_indexA].w;
    float aB = data.positions[m_indexB].a;
    Vec2 vB = data.velocities[m_indexB].v;
    float wB = data.velocities[m_indexB].w;
    final Vec2 temp = pool.popVec2();
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    qA.set(aA);
    qB.set(aB);
    // Compute the effective mass matrix.
    Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA);
    Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);
    // J = [-I -r1_skew I r2_skew]
    // [ 0 -1 0 1]
    // r_skew = [-ry; rx]
    // Matlab
    // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
    // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
    // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;
    final Mat22 K = pool.popMat22();
    K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
    K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
    K.ey.x = K.ex.y;
    K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
    K.invertToOut(m_linearMass);
    m_angularMass = iA + iB;
    if (m_angularMass > 0.0f) {
        m_angularMass = 1.0f / m_angularMass;
    }
    if (data.step.warmStarting) {
        // Scale impulses to support a variable time step.
        m_linearImpulse.mulLocal(data.step.dtRatio);
        m_angularImpulse *= data.step.dtRatio;
        final Vec2 P = pool.popVec2();
        P.set(m_linearImpulse);
        temp.set(P).mulLocal(mA);
        vA.subLocal(temp);
        wA -= iA * (Vec2.cross(m_rA, P) + m_angularImpulse);
        temp.set(P).mulLocal(mB);
        vB.addLocal(temp);
        wB += iB * (Vec2.cross(m_rB, P) + m_angularImpulse);
        pool.pushVec2(1);
    } else {
        m_linearImpulse.setZero();
        m_angularImpulse = 0.0f;
    }
    //    data.velocities[m_indexA].v.set(vA);
    if (data.velocities[m_indexA].w != wA) {
        assert (data.velocities[m_indexA].w != wA);
    }
    data.velocities[m_indexA].w = wA;
    //    data.velocities[m_indexB].v.set(vB);
    data.velocities[m_indexB].w = wB;
    pool.pushRot(2);
    pool.pushVec2(1);
    pool.pushMat22(1);
}

Example 4 with Mat22

use of org.jbox2d.common.Mat22 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;
    }
}

Example 5 with Mat22

use of org.jbox2d.common.Mat22 in project libgdx by libgdx.

the class PrismaticJoint method solvePositionConstraints.

@Override
public boolean solvePositionConstraints(final SolverData data) {
    final Rot qA = pool.popRot();
    final Rot qB = pool.popRot();
    final Vec2 rA = pool.popVec2();
    final Vec2 rB = pool.popVec2();
    final Vec2 d = pool.popVec2();
    final Vec2 axis = pool.popVec2();
    final Vec2 perp = pool.popVec2();
    final Vec2 temp = pool.popVec2();
    final Vec2 C1 = pool.popVec2();
    final Vec3 impulse = pool.popVec3();
    Vec2 cA = data.positions[m_indexA].c;
    float aA = data.positions[m_indexA].a;
    Vec2 cB = data.positions[m_indexB].c;
    float aB = data.positions[m_indexB].a;
    qA.set(aA);
    qB.set(aB);
    float mA = m_invMassA, mB = m_invMassB;
    float iA = m_invIA, iB = m_invIB;
    // Compute fresh Jacobians
    Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
    Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
    d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
    Rot.mulToOutUnsafe(qA, m_localXAxisA, axis);
    float a1 = Vec2.cross(temp.set(d).addLocal(rA), axis);
    float a2 = Vec2.cross(rB, axis);
    Rot.mulToOutUnsafe(qA, m_localYAxisA, perp);
    float s1 = Vec2.cross(temp.set(d).addLocal(rA), perp);
    float s2 = Vec2.cross(rB, perp);
    C1.x = Vec2.dot(perp, d);
    C1.y = aB - aA - m_referenceAngle;
    float linearError = MathUtils.abs(C1.x);
    float angularError = MathUtils.abs(C1.y);
    boolean active = false;
    float C2 = 0.0f;
    if (m_enableLimit) {
        float translation = Vec2.dot(axis, d);
        if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings.linearSlop) {
            // Prevent large angular corrections
            C2 = MathUtils.clamp(translation, -Settings.maxLinearCorrection, Settings.maxLinearCorrection);
            linearError = MathUtils.max(linearError, MathUtils.abs(translation));
            active = true;
        } else if (translation <= m_lowerTranslation) {
            // Prevent large linear corrections and allow some slop.
            C2 = MathUtils.clamp(translation - m_lowerTranslation + Settings.linearSlop, -Settings.maxLinearCorrection, 0.0f);
            linearError = MathUtils.max(linearError, m_lowerTranslation - translation);
            active = true;
        } else if (translation >= m_upperTranslation) {
            // Prevent large linear corrections and allow some slop.
            C2 = MathUtils.clamp(translation - m_upperTranslation - Settings.linearSlop, 0.0f, Settings.maxLinearCorrection);
            linearError = MathUtils.max(linearError, translation - m_upperTranslation);
            active = true;
        }
    }
    if (active) {
        float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
        float k12 = iA * s1 + iB * s2;
        float k13 = iA * s1 * a1 + iB * s2 * a2;
        float k22 = iA + iB;
        if (k22 == 0.0f) {
            // For fixed rotation
            k22 = 1.0f;
        }
        float k23 = iA * a1 + iB * a2;
        float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
        final Mat33 K = pool.popMat33();
        K.ex.set(k11, k12, k13);
        K.ey.set(k12, k22, k23);
        K.ez.set(k13, k23, k33);
        final Vec3 C = pool.popVec3();
        C.x = C1.x;
        C.y = C1.y;
        C.z = C2;
        K.solve33ToOut(C.negateLocal(), impulse);
        pool.pushVec3(1);
        pool.pushMat33(1);
    } else {
        float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
        float k12 = iA * s1 + iB * s2;
        float k22 = iA + iB;
        if (k22 == 0.0f) {
            k22 = 1.0f;
        }
        final Mat22 K = pool.popMat22();
        K.ex.set(k11, k12);
        K.ey.set(k12, k22);
        // temp is impulse1
        K.solveToOut(C1.negateLocal(), temp);
        C1.negateLocal();
        impulse.x = temp.x;
        impulse.y = temp.y;
        impulse.z = 0.0f;
        pool.pushMat22(1);
    }
    float Px = impulse.x * perp.x + impulse.z * axis.x;
    float Py = impulse.x * perp.y + impulse.z * axis.y;
    float LA = impulse.x * s1 + impulse.y + impulse.z * a1;
    float LB = impulse.x * s2 + impulse.y + impulse.z * a2;
    cA.x -= mA * Px;
    cA.y -= mA * Py;
    aA -= iA * LA;
    cB.x += mB * Px;
    cB.y += mB * Py;
    aB += iB * LB;
    // data.positions[m_indexA].c.set(cA);
    data.positions[m_indexA].a = aA;
    // data.positions[m_indexB].c.set(cB);
    data.positions[m_indexB].a = aB;
    pool.pushVec2(7);
    pool.pushVec3(1);
    pool.pushRot(2);
    return linearError <= Settings.linearSlop && angularError <= Settings.angularSlop;
}