Examples with ContactEdge spacegraph.space2d.phys.dynamics.contacts.ContactEdge used on opensource projects

Example 6 with ContactEdge

use of spacegraph.space2d.phys.dynamics.contacts.ContactEdge in project narchy by automenta.

the class WorldRayCastWrapper method solve.

private void solve(TimeStep step) {
    m_profile.solveInit.startAccum();
    m_profile.solveVelocity.startAccum();
    m_profile.solvePosition.startAccum();
    List<Body2D> preRemove = new FasterList(0);
    bodies(b -> {
        // Clear all the island flags.
        b.flags &= ~Body2D.e_islandFlag;
        if (!b.preUpdate()) {
            preRemove.add(b);
        } else {
            // update previous transforms
            b.transformPrev.set(b);
        }
    });
    preRemove.forEach(this::removeBody);
    preRemove.clear();
    // Size the island for the worst case.
    int bodyCount = bodies.size();
    island.init(bodyCount, contactManager.m_contactCount, jointCount, contactManager.contactListener);
    for (Contact c = contactManager.m_contactList; c != null; c = c.m_next) {
        c.m_flags &= ~Contact.ISLAND_FLAG;
    }
    joints(j -> j.islandFlag = false);
    // Build and simulate all awake islands.
    int stackSize = bodyCount;
    // TODO djm find a good initial stack number;
    Body2D[] stack = new Body2D[stackSize];
    bodies(seed -> {
        if ((seed.flags & Body2D.e_islandFlag) == Body2D.e_islandFlag)
            return;
        if (!seed.isAwake() || !seed.isActive())
            return;
        // The seed can be dynamic or kinematic.
        if (seed.getType() == BodyType.STATIC)
            return;
        // Reset island and stack.
        island.clear();
        int stackCount = 0;
        stack[stackCount++] = seed;
        seed.flags |= Body2D.e_islandFlag;
        // Perform a depth first search (DFS) on the constraint graph.
        while (stackCount > 0) {
            // Grab the next body off the stack and add it to the island.
            Body2D b = stack[--stackCount];
            if (!b.isActive())
                continue;
            // assert (b.isActive());
            island.add(b);
            // Make sure the body is awake.
            b.setAwake(true);
            // propagate islands across static bodies.
            if (b.getType() == BodyType.STATIC)
                continue;
            // Search all contacts connected to this body.
            for (ContactEdge ce = b.contacts; ce != null; ce = ce.next) {
                Contact contact = ce.contact;
                // Has this contact already been added to an island?
                if ((contact.m_flags & Contact.ISLAND_FLAG) == Contact.ISLAND_FLAG) {
                    continue;
                }
                // Is this contact solid and touching?
                if (!contact.isEnabled() || !contact.isTouching()) {
                    continue;
                }
                // Skip sensors.
                boolean sensorA = contact.aFixture.isSensor;
                boolean sensorB = contact.bFixture.isSensor;
                if (sensorA || sensorB) {
                    continue;
                }
                island.add(contact);
                contact.m_flags |= Contact.ISLAND_FLAG;
                Body2D other = ce.other;
                // Was the other body already added to this island?
                if ((other.flags & Body2D.e_islandFlag) == Body2D.e_islandFlag) {
                    continue;
                }
                assert (stackCount < stackSize);
                stack[stackCount++] = other;
                other.flags |= Body2D.e_islandFlag;
            }
            // Search all joints connect to this body.
            for (JointEdge je = b.joints; je != null; je = je.next) {
                if (je.joint.islandFlag) {
                    continue;
                }
                Body2D other = je.other;
                // Don't simulate joints connected to inactive bodies.
                if (!other.isActive()) {
                    continue;
                }
                island.add(je.joint);
                je.joint.islandFlag = true;
                if ((other.flags & Body2D.e_islandFlag) == Body2D.e_islandFlag) {
                    continue;
                }
                assert (stackCount < stackSize);
                stack[stackCount++] = other;
                other.flags |= Body2D.e_islandFlag;
            }
        }
        island.solve(m_profile, step, m_gravity, m_allowSleep);
        // Post solve cleanup.
        for (int i = 0; i < island.m_bodyCount; ++i) {
            // Allow static bodies to participate in other islands.
            Body2D b = island.bodies[i];
            if (b.getType() == BodyType.STATIC) {
                b.flags &= ~Body2D.e_islandFlag;
            }
        }
    });
    m_profile.solveInit.endAccum();
    m_profile.solveVelocity.endAccum();
    m_profile.solvePosition.endAccum();
    broadphaseTimer.reset();
    // Synchronize fixtures, check for out of range bodies.
    bodies(b -> {
        // If a body was not in an island then it did not move.
        if ((b.flags & Body2D.e_islandFlag) == 0 || b.getType() == BodyType.STATIC)
            return;
        // Update fixtures (for broad-phase).
        b.synchronizeFixtures();
        b.postUpdate();
    });
    // Look for new contacts.
    contactManager.findNewContacts();
    m_profile.broadphase.record(broadphaseTimer.getMilliseconds());
}

Example 7 with ContactEdge

use of spacegraph.space2d.phys.dynamics.contacts.ContactEdge in project narchy by automenta.

the class Body2D method removeFixture.

/**
 * Destroy a fixture. This removes the fixture from the broad-phase and destroys all contacts
 * associated with this fixture. This will automatically adjust the mass of the body if the body
 * is dynamic and the fixture has positive density. All fixtures attached to a body are implicitly
 * destroyed when the body is destroyed.
 *
 * @param fixture the fixture to be removed.
 * @warning This function is locked during callbacks.
 */
public final void removeFixture(Fixture fixture) {
    W.invoke(() -> {
        assert (fixture.body == this);
        // Remove the fixture from this body's singly linked list.
        assert (fixtureCount > 0);
        // W.invokeLater(() -> {
        Fixture node = fixtures;
        // java change
        Fixture last = null;
        boolean found = false;
        while (node != null) {
            if (node == fixture) {
                node = fixture.next;
                found = true;
                break;
            }
            last = node;
            node = node.next;
        }
        // You tried to remove a shape that is not attached to this body.
        assert (found);
        // java change, remove it from the list
        if (last == null) {
            fixtures = fixture.next;
        } else {
            last.next = fixture.next;
        }
        // Destroy any contacts associated with the fixture.
        ContactEdge edge = contacts;
        while (edge != null) {
            Contact c = edge.contact;
            edge = edge.next;
            Fixture fixtureA = c.aFixture;
            Fixture fixtureB = c.bFixture;
            if (fixture == fixtureA || fixture == fixtureB) {
                // This destroys the contact and removes it from
                // this body's contact list.
                W.contactManager.destroy(c);
            }
        }
        if ((flags & e_activeFlag) == e_activeFlag) {
            BroadPhase broadPhase = W.contactManager.broadPhase;
            fixture.destroyProxies(broadPhase);
        }
        fixture.destroy();
        fixture.body = null;
        fixture.next = null;
        --fixtureCount;
        // Reset the mass data.
        resetMassData();
    });
}

Example 8 with ContactEdge

use of spacegraph.space2d.phys.dynamics.contacts.ContactEdge in project narchy by automenta.

the class WorldRayCastWrapper method addJoint.

public Joint addJoint(Joint j) {
    if (joints.add(j)) {
        invoke(() -> {
            ++jointCount;
            // Connect to the bodies' doubly linked lists.
            j.edgeA.joint = j;
            Body2D B = j.getBodyB();
            j.edgeA.other = B;
            j.edgeA.prev = null;
            Body2D A = j.getBodyA();
            j.edgeA.next = A.joints;
            if (A.joints != null) {
                A.joints.prev = j.edgeA;
            }
            A.joints = j.edgeA;
            j.edgeB.joint = j;
            j.edgeB.other = A;
            j.edgeB.prev = null;
            j.edgeB.next = B.joints;
            if (B.joints != null) {
                B.joints.prev = j.edgeB;
            }
            B.joints = j.edgeB;
            // If the joint prevents collisions, then flag any contacts for filtering.
            if (!j.getCollideConnected()) {
                Body2D bodyA = j.getBodyA();
                Body2D bodyB = j.getBodyB();
                ContactEdge edge = bodyB.contacts();
                while (edge != null) {
                    if (edge.other == bodyA) {
                        // Flag the contact for filtering at the next time step (where either
                        // body is awake).
                        edge.contact.flagForFiltering();
                    }
                    edge = edge.next;
                }
            }
        // Note: creating a joint doesn't wake the bodies.
        });
    }
    return j;
}

Example 9 with ContactEdge

use of spacegraph.space2d.phys.dynamics.contacts.ContactEdge in project narchy by automenta.

the class WorldRayCastWrapper method solveTOI.

private void solveTOI(final TimeStep step) {
    final Island island = toiIsland;
    island.init(2 * Settings.maxTOIContacts, Settings.maxTOIContacts, 0, contactManager.contactListener);
    if (m_stepComplete) {
        bodies(b -> {
            b.flags &= ~Body2D.e_islandFlag;
            b.sweep.alpha0 = 0.0f;
        });
        for (Contact c = contactManager.m_contactList; c != null; c = c.m_next) {
            // Invalidate TOI
            c.m_flags &= ~(Contact.TOI_FLAG | Contact.ISLAND_FLAG);
            c.m_toiCount = 0;
            c.m_toi = 1.0f;
        }
    }
    // Find TOI events and solve them.
    for (; ; ) {
        // Find the first TOI.
        Contact minContact = null;
        float minAlpha = 1.0f;
        for (Contact c = contactManager.m_contactList; c != null; c = c.m_next) {
            // Is this contact disabled?
            if (!c.isEnabled()) {
                continue;
            }
            // Prevent excessive sub-stepping.
            if (c.m_toiCount > Settings.maxSubSteps) {
                continue;
            }
            float alpha = 1.0f;
            if ((c.m_flags & Contact.TOI_FLAG) != 0) {
                // This contact has a valid cached TOI.
                alpha = c.m_toi;
            } else {
                Fixture fA = c.aFixture;
                Fixture fB = c.bFixture;
                // Is there a sensor?
                if (fA.isSensor() || fB.isSensor()) {
                    continue;
                }
                Body2D bA = fA.getBody();
                Body2D bB = fB.getBody();
                BodyType typeA = bA.type;
                BodyType typeB = bB.type;
                assert (typeA == BodyType.DYNAMIC || typeB == BodyType.DYNAMIC);
                boolean activeA = bA.isAwake() && typeA != BodyType.STATIC;
                boolean activeB = bB.isAwake() && typeB != BodyType.STATIC;
                // Is at least one body active (awake and dynamic or kinematic)?
                if (!activeA && !activeB) {
                    continue;
                }
                boolean collideA = bA.isBullet() || typeA != BodyType.DYNAMIC;
                boolean collideB = bB.isBullet() || typeB != BodyType.DYNAMIC;
                // Are these two non-bullet dynamic bodies?
                if (!collideA && !collideB) {
                    continue;
                }
                // Compute the TOI for this contact.
                // Put the sweeps onto the same time interval.
                float alpha0 = bA.sweep.alpha0;
                if (bA.sweep.alpha0 < bB.sweep.alpha0) {
                    alpha0 = bB.sweep.alpha0;
                    bA.sweep.advance(alpha0);
                } else if (bB.sweep.alpha0 < bA.sweep.alpha0) {
                    alpha0 = bA.sweep.alpha0;
                    bB.sweep.advance(alpha0);
                }
                assert (alpha0 < 1.0f);
                int indexA = c.aIndex;
                int indexB = c.bIndex;
                // Compute the time of impact in interval [0, minTOI]
                final TimeOfImpact.TOIInput input = toiInput;
                input.proxyA.set(fA.shape(), indexA);
                input.proxyB.set(fB.shape(), indexB);
                input.sweepA.set(bA.sweep);
                input.sweepB.set(bB.sweep);
                input.tMax = 1.0f;
                pool.getTimeOfImpact().timeOfImpact(toiOutput, input);
                // Beta is the fraction of the remaining portion of the .
                float beta = toiOutput.t;
                if (toiOutput.state == TimeOfImpact.TOIOutputState.TOUCHING) {
                    alpha = MathUtils.min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
                } else {
                    alpha = 1.0f;
                }
                c.m_toi = alpha;
                c.m_flags |= Contact.TOI_FLAG;
            }
            if (alpha < minAlpha) {
                // This is the minimum TOI found so far.
                minContact = c;
                minAlpha = alpha;
            }
        }
        if (minContact == null || 1.0f - 10.0f * Settings.EPSILON < minAlpha) {
            // No more TOI events. Done!
            m_stepComplete = true;
            break;
        }
        // Advance the bodies to the TOI.
        Fixture fA = minContact.aFixture;
        Fixture fB = minContact.bFixture;
        Body2D bA = fA.getBody();
        Body2D bB = fB.getBody();
        backup1.set(bA.sweep);
        backup2.set(bB.sweep);
        bA.advance(minAlpha);
        bB.advance(minAlpha);
        // The TOI contact likely has some new contact points.
        minContact.update(contactManager.contactListener);
        minContact.m_flags &= ~Contact.TOI_FLAG;
        ++minContact.m_toiCount;
        // Is the contact solid?
        if (!minContact.isEnabled() || !minContact.isTouching()) {
            // Restore the sweeps.
            minContact.setEnabled(false);
            bA.sweep.set(backup1);
            bB.sweep.set(backup2);
            bA.synchronizeTransform();
            bB.synchronizeTransform();
            continue;
        }
        bA.setAwake(true);
        bB.setAwake(true);
        // Build the island
        island.clear();
        island.add(bA);
        island.add(bB);
        island.add(minContact);
        bA.flags |= Body2D.e_islandFlag;
        bB.flags |= Body2D.e_islandFlag;
        minContact.m_flags |= Contact.ISLAND_FLAG;
        // Get contacts on bodyA and bodyB.
        tempBodies[0] = bA;
        tempBodies[1] = bB;
        for (int i = 0; i < 2; ++i) {
            Body2D body = tempBodies[i];
            if (body.type == BodyType.DYNAMIC) {
                for (ContactEdge ce = body.contacts; ce != null; ce = ce.next) {
                    if (island.m_bodyCount == island.m_bodyCapacity) {
                        break;
                    }
                    if (island.m_contactCount == island.m_contactCapacity) {
                        break;
                    }
                    Contact contact = ce.contact;
                    // Has this contact already been added to the island?
                    if ((contact.m_flags & Contact.ISLAND_FLAG) != 0) {
                        continue;
                    }
                    // Only add static, kinematic, or bullet bodies.
                    Body2D other = ce.other;
                    if (other.type == BodyType.DYNAMIC && !body.isBullet() && !other.isBullet()) {
                        continue;
                    }
                    // Skip sensors.
                    boolean sensorA = contact.aFixture.isSensor;
                    boolean sensorB = contact.bFixture.isSensor;
                    if (sensorA || sensorB) {
                        continue;
                    }
                    // Tentatively advance the body to the TOI.
                    backup1.set(other.sweep);
                    if ((other.flags & Body2D.e_islandFlag) == 0) {
                        other.advance(minAlpha);
                    }
                    // Update the contact points
                    contact.update(contactManager.contactListener);
                    // Was the contact disabled by the user?
                    if (!contact.isEnabled()) {
                        other.sweep.set(backup1);
                        other.synchronizeTransform();
                        continue;
                    }
                    // Are there contact points?
                    if (!contact.isTouching()) {
                        other.sweep.set(backup1);
                        other.synchronizeTransform();
                        continue;
                    }
                    // Add the contact to the island
                    contact.m_flags |= Contact.ISLAND_FLAG;
                    island.add(contact);
                    // Has the other body already been added to the island?
                    if ((other.flags & Body2D.e_islandFlag) != 0) {
                        continue;
                    }
                    // Add the other body to the island.
                    other.flags |= Body2D.e_islandFlag;
                    if (other.type != BodyType.STATIC) {
                        other.setAwake(true);
                    }
                    island.add(other);
                }
            }
        }
        subStep.dt = (1.0f - minAlpha) * step.dt;
        subStep.inv_dt = 1.0f / subStep.dt;
        subStep.dtRatio = 1.0f;
        subStep.positionIterations = 20;
        subStep.velocityIterations = step.velocityIterations;
        subStep.warmStarting = false;
        island.solveTOI(subStep, bA.island, bB.island);
        // Reset island flags and synchronize broad-phase proxies.
        for (int i = 0; i < island.m_bodyCount; ++i) {
            Body2D body = island.bodies[i];
            body.flags &= ~Body2D.e_islandFlag;
            if (body.type != BodyType.DYNAMIC) {
                continue;
            }
            body.synchronizeFixtures();
            // Invalidate all contact TOIs on this displaced body.
            for (ContactEdge ce = body.contacts; ce != null; ce = ce.next) {
                ce.contact.m_flags &= ~(Contact.TOI_FLAG | Contact.ISLAND_FLAG);
            }
        }
        // Commit fixture proxy movements to the broad-phase so that new contacts are created.
        // Also, some contacts can be destroyed.
        contactManager.findNewContacts();
        if (m_subStepping) {
            m_stepComplete = false;
            break;
        }
    }
}

Example 10 with ContactEdge

use of spacegraph.space2d.phys.dynamics.contacts.ContactEdge in project narchy by automenta.

the class Fixture method refilter.

/**
 * Call this if you want to establish collision that was previously disabled by
 * ContactFilter::ShouldCollide.
 */
public void refilter() {
    if (body == null) {
        return;
    }
    // Flag associated contacts for filtering.
    ContactEdge edge = body.contacts();
    while (edge != null) {
        Contact contact = edge.contact;
        Fixture fixtureA = contact.aFixture;
        Fixture fixtureB = contact.bFixture;
        if (fixtureA == this || fixtureB == this) {
            contact.flagForFiltering();
        }
        edge = edge.next;
    }
    Dynamics2D world = body.W;
    if (world == null) {
        return;
    }
    // Touch each proxy so that new pairs may be created
    BroadPhase broadPhase = world.contactManager.broadPhase;
    for (int i = 0; i < m_proxyCount; ++i) {
        broadPhase.touchProxy(proxies[i].id);
    }
}