Examples with Manifold com.almasb.fxgl.physics.box2d.collision.Manifold used on opensource projects

Example 1 with Manifold

use of com.almasb.fxgl.physics.box2d.collision.Manifold in project FXGL by AlmasB.

the class ContactSolver method storeImpulses.

public void storeImpulses() {
    for (int i = 0; i < m_count; i++) {
        final ContactVelocityConstraint vc = m_velocityConstraints[i];
        final Manifold manifold = m_contacts[vc.contactIndex].getManifold();
        for (int j = 0; j < vc.pointCount; j++) {
            manifold.points[j].normalImpulse = vc.points[j].normalImpulse;
            manifold.points[j].tangentImpulse = vc.points[j].tangentImpulse;
        }
    }
}

Example 2 with Manifold

use of com.almasb.fxgl.physics.box2d.collision.Manifold in project FXGL by AlmasB.

the class ContactSolver method initializeVelocityConstraints.

public 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 Rotation xfAq = xfA.q;
        final Rotation 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 < -JBoxSettings.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 3 with Manifold

use of com.almasb.fxgl.physics.box2d.collision.Manifold in project FXGL by AlmasB.

the class ContactSolver method init.

public void init(ContactSolverDef def) {
    TimeStep step = def.step;
    m_count = def.count;
    if (m_positionConstraints.length < m_count) {
        ContactPositionConstraint[] old = m_positionConstraints;
        m_positionConstraints = new ContactPositionConstraint[Math.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[Math.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) {
        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.getRadius();
        final float radiusB = shapeB.getRadius();
        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.getFriction();
        vc.restitution = contact.getRestitution();
        vc.tangentSpeed = contact.getTangentSpeed();
        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;
        for (int j = 0; j < pointCount; j++) {
            ManifoldPoint cp = manifold.points[j];
            VelocityConstraintPoint vcp = vc.points[j];
            if (step.warmStarting) {
                vcp.normalImpulse = step.dtRatio * cp.normalImpulse;
                vcp.tangentImpulse = 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;
        }
    }
}