Examples with Manifold spacegraph.space2d.phys.collision.Manifold used on opensource projects

Example 1 with Manifold

use of spacegraph.space2d.phys.collision.Manifold in project narchy by automenta.

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;
        Tuple2f localCenterA = pc.localCenterA;
        Tuple2f localCenterB = pc.localCenterB;
        Tuple2f cA = m_positions[indexA];
        float aA = m_positions[indexA].a;
        Tuple2f vA = m_velocities[indexA];
        float wA = m_velocities[indexA].w;
        Tuple2f cB = m_positions[indexB];
        float aB = m_positions[indexB].a;
        Tuple2f vB = m_velocities[indexB];
        float wB = m_velocities[indexB].w;
        assert (manifold.pointCount > 0);
        final Rot xfAq = xfA;
        final Rot xfBq = xfB;
        xfAq.set(aA);
        xfBq.set(aB);
        xfA.pos.x = cA.x - (xfAq.c * localCenterA.x - xfAq.s * localCenterA.y);
        xfA.pos.y = cA.y - (xfAq.s * localCenterA.x + xfAq.c * localCenterA.y);
        xfB.pos.x = cB.x - (xfBq.c * localCenterB.x - xfBq.s * localCenterB.y);
        xfB.pos.y = cB.y - (xfBq.s * localCenterB.x + xfBq.c * localCenterB.y);
        worldManifold.initialize(manifold, xfA, radiusA, xfB, radiusB);
        final Tuple2f 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];
            Tuple2f wmPj = worldManifold.points[j];
            final Tuple2f vcprA = vcp.rA;
            final Tuple2f 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 Manifold

use of spacegraph.space2d.phys.collision.Manifold in project narchy by automenta.

the class PositionSolverManifold 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 3 with Manifold

use of spacegraph.space2d.phys.collision.Manifold in project narchy by automenta.

the class TestQueryCallback method preSolve.

public void preSolve(Contact contact, Manifold oldManifold) {
    Manifold manifold = contact.getManifold();
    if (manifold.pointCount == 0) {
        return;
    }
    Fixture fixtureA = contact.getFixtureA();
    Fixture fixtureB = contact.getFixtureB();
    Collision.getPointStates(state1, state2, oldManifold, manifold);
    contact.getWorldManifold(worldManifold);
    for (int i = 0; i < manifold.pointCount && pointCount < MAX_CONTACT_POINTS; i++) {
        ContactPoint cp = points[pointCount];
        cp.fixtureA = fixtureA;
        cp.fixtureB = fixtureB;
        cp.position.set(worldManifold.points[i]);
        cp.normal.set(worldManifold.normal);
        cp.state = state2[i];
        cp.normalImpulse = manifold.points[i].normalImpulse;
        cp.tangentImpulse = manifold.points[i].tangentImpulse;
        // cp.separation = worldManifold.separations[i];
        ++pointCount;
    }
}

Example 4 with Manifold

use of spacegraph.space2d.phys.collision.Manifold in project narchy by automenta.

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.aFixture;
        final Fixture fixtureB = contact.bFixture;
        final Shape shapeA = fixtureA.shape();
        final Shape shapeB = fixtureB.shape();
        final float radiusA = shapeA.radius;
        final float radiusB = shapeB.radius;
        final Body2D bodyA = fixtureA.getBody();
        final Body2D 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.island;
        vc.indexB = bodyB.island;
        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.island;
        pc.indexB = bodyB.island;
        pc.invMassA = bodyA.m_invMass;
        pc.invMassB = bodyB.m_invMass;
        pc.localCenterA.set(bodyA.sweep.localCenter);
        pc.localCenterB.set(bodyB.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;
        }
    }
}