Search in sources :

Example 6 with FasterList

use of jcog.list.FasterList in project narchy by automenta.

the class Conj method term.

private Term term(long when, Object what, IntPredicate validator) {
    if (what == null)
        return null;
    final RoaringBitmap rb;
    final byte[] b;
    int n;
    if (what instanceof byte[]) {
        b = (byte[]) what;
        rb = null;
        n = indexOfZeroTerminated(b, (byte) 0);
        if (n == 1) {
            // simplest case
            return sub(b[0], null, validator);
        }
    } else {
        rb = (RoaringBitmap) what;
        b = null;
        n = rb.getCardinality();
    }
    final boolean[] negatives = { false };
    TreeSet<Term> t = new TreeSet();
    if (b != null) {
        for (byte x : b) {
            if (x == 0)
                // done
                break;
            t.add(sub(x, negatives, validator));
        }
    } else {
        rb.forEach((int termIndex) -> {
            t.add(sub(termIndex, negatives, validator));
        });
    }
    if (negatives[0] && n > 1) {
        // annihilate common terms inside and outside of disjunction
        // ex:
        // -X &&  ( X ||  Y)
        // -X && -(-X && -Y)  |-   -X && Y
        Iterator<Term> oo = t.iterator();
        List<Term> csa = null;
        while (oo.hasNext()) {
            Term x = oo.next();
            if (x.hasAll(NEG.bit | CONJ.bit)) {
                if (x.op() == NEG) {
                    Term x0 = x.sub(0);
                    if (x0.op() == CONJ && CONJ.commute(x0.dt(), x0.subs())) {
                        // DISJUNCTION
                        Term disj = x.unneg();
                        SortedSet<Term> disjSubs = disj.subterms().toSetSorted();
                        // factor out occurrences of the disj's contents outside the disjunction, so remove from inside it
                        if (disjSubs.removeAll(t)) {
                            // reconstruct disj if changed
                            oo.remove();
                            if (!disjSubs.isEmpty()) {
                                if (csa == null)
                                    csa = new FasterList(1);
                                csa.add(CONJ.the(disj.dt(), sorted(disjSubs)).neg());
                            }
                        }
                    }
                }
            }
        }
        if (csa != null)
            t.addAll(csa);
    }
    int ts = t.size();
    switch(ts) {
        case 0:
            // throw new RuntimeException("fault");
            return True;
        case 1:
            return t.first();
        default:
            return Op.instance(CONJ, when == ETERNAL ? DTERNAL : 0, sorted(t));
    }
}
Also used : FasterList(jcog.list.FasterList) Term(nars.term.Term) RoaringBitmap(org.roaringbitmap.RoaringBitmap)

Example 7 with FasterList

use of jcog.list.FasterList in project narchy by automenta.

the class Fracture method smash.

/**
 * Rozbije objekt. Upravi objekt world tak, ze vymaze triesteny objekt
 * a nahradi ho fragmentami na zaklade nastaveneho materialu a clenskych
 * premennych.
 *
 * @param dt casova dlzka framu
 */
public void smash(Smasher smasher, float dt) {
    Shape s = f1.shape;
    if (s == null)
        return;
    if (contact == null) {
        // riesi sa staticky prvok, ktory ma priliz maly obsah
        b1.setType(BodyType.DYNAMIC);
        return;
    }
    Dynamics2D w = b1.W;
    Polygon p = f1.polygon;
    if (p == null) {
        switch(s.m_type) {
            case POLYGON:
                PolygonShape ps = (PolygonShape) s;
                Tuple2f[] vertices = ps.vertex;
                int n = ps.vertices;
                p = new Polygon(n);
                for (int i = 0; i < n; ++i) {
                    p.add(vertices[n - i - 1]);
                }
                break;
            case CIRCLE:
                CircleShape cs = (CircleShape) s;
                p = new Polygon(CIRCLEVERTICES);
                float radius = cs.radius;
                double u = Math.PI * 2 / CIRCLEVERTICES;
                // upravim radius tak, aby bola zachovana velkost obsahu
                radius = (float) Math.sqrt(u / Math.sin(u)) * radius;
                Tuple2f center = cs.center;
                for (int i = 0; i < CIRCLEVERTICES; ++i) {
                    // uhol
                    double j = u * i;
                    float sin = (float) Math.sin(j);
                    float cos = (float) Math.cos(j);
                    Tuple2f v = new v2(sin, cos).scaled(radius).added(center);
                    p.add(v);
                }
                break;
            default:
                throw new RuntimeException("Dany typ tvaru nepodporuje stiepenie");
        }
    }
    // sila v zavislosti na pevnosti telesa
    float mConst = f1.material.m_rigidity / normalImpulse;
    // true, ak f2 je v objekte contact ako m_fixtureA
    boolean fixA = f1 == contact.aFixture;
    float oldAngularVelocity = fixA ? contact.m_angularVelocity_bodyA : contact.m_angularVelocity_bodyB;
    Tuple2f oldLinearVelocity = fixA ? contact.m_linearVelocity_bodyA : contact.m_linearVelocity_bodyB;
    b1.setAngularVelocity((b1.velAngular - oldAngularVelocity) * mConst + oldAngularVelocity);
    b1.setLinearVelocity(b1.vel.sub(oldLinearVelocity).scaled(mConst).added(oldLinearVelocity));
    if (!w.isFractured(f2) && b2.type == BodyType.DYNAMIC && !b2.m_fractureTransformUpdate) {
        // ak sa druhy objekt nerozbija, tak sa jej nahodia povodne hodnoty (TREBA MODIFIKOVAT POHYB OBJEKTU, KTORY SPOSOBUJE ROZPAD)
        oldAngularVelocity = !fixA ? contact.m_angularVelocity_bodyA : contact.m_angularVelocity_bodyB;
        oldLinearVelocity = !fixA ? contact.m_linearVelocity_bodyA : contact.m_linearVelocity_bodyB;
        b2.setAngularVelocity((b2.velAngular - oldAngularVelocity) * mConst + oldAngularVelocity);
        b2.setLinearVelocity(b2.vel.sub(oldLinearVelocity).scaled(mConst).added(oldLinearVelocity));
        b2.setTransform(b2.transformPrev.pos.add(b2.vel.scale(dt)), b2.transformPrev.angle());
        // osetruje jbox2d od posuvania telesa pri rieseni kolizie
        b2.m_fractureTransformUpdate = true;
    }
    Tuple2f localPoint = Transform.mulTrans(b1, point);
    Tuple2f b1Vec = b1.getLinearVelocityFromWorldPoint(point);
    Tuple2f b2Vec = b2.getLinearVelocityFromWorldPoint(point);
    Tuple2f localVelocity = b2Vec.subbed(b1Vec);
    localVelocity.scaled(dt);
    // rodeli to
    Polygon[] fragment = m.split(smasher, p, localPoint, localVelocity, normalImpulse);
    if (fragment.length <= 1) {
        // nerozbilo to na ziadne fragmenty
        return;
    }
    // definuje tela fragmentov - tie maju vsetky rovnaku definiciu (preberaju parametre z povodneho objektu)
    BodyDef bodyDef = new BodyDef();
    // pozicia
    bodyDef.position.set(b1.pos);
    // otocenie
    bodyDef.angle = b1.angle();
    bodyDef.fixedRotation = b1.isFixedRotation();
    bodyDef.angularDamping = b1.m_angularDamping;
    bodyDef.allowSleep = b1.isSleepingAllowed();
    FixtureDef fd = new FixtureDef();
    // trenie
    fd.friction = f1.friction;
    // odrazivost
    fd.restitution = f1.restitution;
    fd.isSensor = f1.isSensor;
    fd.density = f1.density;
    // odstrani fragmentacne predmety/cele teleso
    List<Fixture> fixtures = new FasterList<>();
    if (f1.polygon != null) {
        for (Fixture f = b1.fixtures; f != null; f = f.next) {
            if (f.polygon == f1.polygon) {
                fixtures.add(f);
            }
        }
    } else {
        fixtures.add(f1);
    }
    for (Fixture f : fixtures) {
        b1.removeFixture(f);
    }
    if (b1.fixtureCount == 0) {
        w.removeBody(b1);
    }
    // prida fragmenty do simulacie
    MyList<Body2D> newbodies = new MyList<>();
    for (Polygon pg : fragment) {
        // vytvori tela, prida fixtury, poriesi konvexnu dekompoziciu
        if (pg.isCorrect()) {
            if (pg instanceof Fragment) {
                Polygon[] convex = pg.convexDecomposition();
                bodyDef.type = BodyType.DYNAMIC;
                for (Polygon pgx : convex) {
                    Body2D f_body = w.addBody(bodyDef);
                    pgx.flip();
                    PolygonShape ps = new PolygonShape();
                    ps.set(pgx.getArray(), pgx.size());
                    fd.shape = ps;
                    fd.polygon = null;
                    fd.material = f1.material;
                    // .m_fragments; //rekurzivne stiepenie
                    f_body.addFixture(fd);
                    f_body.setAngularVelocity(b1.velAngular);
                    f_body.setLinearVelocity(b1.getLinearVelocityFromLocalPoint(f_body.getLocalCenter()));
                    newbodies.add(f_body);
                }
            } else {
                fd.material = // .m_fragments; //rekurzivne stiepenie
                f1.material;
                bodyDef.type = b1.getType();
                Body2D f_body = w.addBody(bodyDef);
                PolygonFixture pf = new PolygonFixture(pg);
                f_body.addFixture(pf, fd);
                f_body.setLinearVelocity(b1.getLinearVelocityFromLocalPoint(f_body.getLocalCenter()));
                f_body.setAngularVelocity(b1.velAngular);
                newbodies.add(f_body);
            }
        }
    }
    // zavola sa funkcia z fraction listeneru (pokial je nadefinovany)
    FractureListener fl = w.getContactManager().m_fractureListener;
    if (fl != null) {
        fl.action(m, normalImpulse, newbodies);
    }
}
Also used : PolygonShape(spacegraph.space2d.phys.collision.shapes.PolygonShape) Shape(spacegraph.space2d.phys.collision.shapes.Shape) CircleShape(spacegraph.space2d.phys.collision.shapes.CircleShape) PolygonShape(spacegraph.space2d.phys.collision.shapes.PolygonShape) Tuple2f(spacegraph.util.math.Tuple2f) CircleShape(spacegraph.space2d.phys.collision.shapes.CircleShape) FasterList(jcog.list.FasterList) MyList(spacegraph.space2d.phys.fracture.util.MyList) spacegraph.util.math.v2(spacegraph.util.math.v2)

Example 8 with FasterList

use of jcog.list.FasterList in project narchy by automenta.

the class Smasher method calculate.

/**
 * Vrati prienik voronoi diagramu a polygonu.
 *
 * @param focee
 * @param p            Kopia polygonu, moze byt modifikovana
 * @param contactPoint Bod dotyku
 * @param ic           Funkcionalny interface, ktory definuje, ci fragment patri,
 *                     alebo nepatri do mnoziny ulomkov
 */
public void calculate(Polygon p, Tuple2f[] focee, Tuple2f contactPoint, IContains ic) {
    this.focee = focee;
    this.p = p;
    // Geometry geom = new Geometry(foceeAll, p);
    List<Fragment> list = getVoronoi();
    List<EdgePolygon> polygonEdgesList = new FasterList<>();
    HashTabulka<EdgeDiagram> diagramEdges = new HashTabulka<>();
    HashTabulka<EdgePolygon> polygonEdges = new HashTabulka<>();
    // vlozim hrany polygonu do hashovacej tabulky hran polygonu
    int count = p.size();
    for (int i = 1; i <= count; i++) {
        Tuple2f p1 = p.get(i - 1);
        Tuple2f p2 = p.get(i == count ? 0 : i);
        EdgePolygon e = new EdgePolygon(p1, p2);
        polygonEdges.add(e);
        polygonEdgesList.add(e);
    }
    // vlozim hrany diagramu do hashovacej tabulky hran diagramu
    for (Fragment pp : list) {
        count = pp.size();
        for (int i = 1; i <= count; i++) {
            Tuple2f p1 = pp.get(i - 1);
            Tuple2f p2 = pp.get(i == count ? 0 : i);
            EdgeDiagram e = new EdgeDiagram(p1, p2);
            EdgeDiagram alternative = diagramEdges.get(e);
            if (alternative == null) {
                diagramEdges.add(e);
                e.d1 = pp;
            } else {
                alternative.d2 = pp;
            }
        }
    }
    AEdge[][] allEdges = new AEdge[][] { diagramEdges.toArray(new AEdge[diagramEdges.size()]), polygonEdges.toArray(new AEdge[polygonEdges.size()]) };
    diagramEdges.clear();
    polygonEdges.clear();
    List<EVec2> vectorList = new FasterList<>();
    for (AEdge[] array : allEdges) {
        for (AEdge e : array) {
            EVec2 v1 = new EVec2(e.p1);
            EVec2 v2 = new EVec2(e.p2);
            v1.e = e;
            v2.e = e;
            if (v1.p.y < v2.p.y) {
                v1.start = true;
            } else {
                v2.start = true;
            }
            vectorList.add(v1);
            vectorList.add(v2);
        }
    }
    EVec2[] vectors = vectorList.toArray(new EVec2[vectorList.size()]);
    // zotriedim body
    Arrays.sort(vectors);
    for (EVec2 e : vectors) {
        if (e.e instanceof EdgeDiagram) {
            if (e.start) {
                EdgeDiagram ex = (EdgeDiagram) e.e;
                diagramEdges.add(ex);
                // for (EdgePolygon px : polygonEdges.toArray(new EdgePolygon[polygonEdges.size()])) {
                // process(px, ex);
                // }
                polygonEdges.forEach(px -> process(px, ex));
            } else {
                diagramEdges.remove(e.e);
            }
        } else {
            // je instanciou EdgePolygon
            if (e.start) {
                EdgePolygon px = (EdgePolygon) e.e;
                polygonEdges.add(px);
                diagramEdges.forEach(ex -> process(px, ex));
            // for (EdgeDiagram ex : diagramEdges.toArray(new EdgeDiagram[diagramEdges.size()]))
            // process(px, ex);
            } else {
                polygonEdges.remove(e.e);
            }
        }
    }
    for (Fragment pol : list) {
        pol.resort();
        int pn = pol.size();
        for (int i = 0; i < pn; i++) {
            Tuple2f v = pol.get(i);
            if (v instanceof Vec2Intersect) {
                Vec2Intersect vi = (Vec2Intersect) v;
                if (vi.p1 == pol) {
                    vi.i1 = i;
                } else {
                    vi.i2 = i;
                }
            }
        }
    }
    Polygon polygonAll = new Polygon();
    for (EdgePolygon ex : polygonEdgesList) {
        polygonAll.add(ex.p1);
        ex.list.sort(c);
        polygonAll.add(ex.list);
    }
    for (int i = 0; i < polygonAll.size(); i++) {
        Tuple2f v = polygonAll.get(i);
        if (v instanceof Vec2Intersect) {
            ((Vec2Intersect) v).index = i;
        }
    }
    MyList<Fragment> allIntersections = new MyList<>();
    // ostatne algoritmy generovali diery - tento je najlepsi - najdem najblizsi bod na hrane polygonu a zistim kolizny fargment - od neho prehladavam do sirky a kontrolujem vzdialenost a viditelnost (jednoduche, ciste)
    precalc_values();
    for (Fragment ppp : list) {
        List<Fragment> intsc = getIntersections(ppp, polygonAll);
        if (intsc == null) {
            // cely polygon sa nachadza vnutri fragmentu
            fragments = new Polygon[] { p };
            return;
        }
        allIntersections.addAll(intsc);
    }
    table.clear();
    // vytvorim hashovaciu tabulku hran
    for (Fragment f : allIntersections) {
        for (int i = 0; i < f.size(); ++i) {
            Tuple2f v1 = f.get(i);
            Tuple2f v2 = f.cycleGet(i + 1);
            EdgeDiagram e = new EdgeDiagram(v1, v2);
            EdgeDiagram e2 = table.get(e);
            if (e2 != null) {
                e = e2;
                e.d2 = f;
            } else {
                e.d1 = f;
                table.add(e);
            }
        }
    }
    // rozdelim polygony na 2 mnoziny - na tie, ktore budu ulomky a tie, ktore budu spojene a drzat spolu
    final double[] distance = { Double.MAX_VALUE };
    final Fragment[] startPolygon = { null };
    final Tuple2f[] kolmicovyBod = { null };
    MyList<EdgeDiagram> allEdgesPolygon = new MyList<>();
    // EdgeDiagram[] ee = table.toArray(new EdgeDiagram[table.size()]);
    table.forEach(ep -> {
        if (ep.d2 == null) {
            // toto sa nahradi vzorcom na vypocet vzdialenosti body od usecky
            Tuple2f vv = ep.kolmicovyBod(contactPoint);
            double newDistance = contactPoint.distanceSq(vv);
            if (newDistance <= distance[0]) {
                distance[0] = newDistance;
                kolmicovyBod[0] = vv;
                startPolygon[0] = ep.d1;
            }
            allEdgesPolygon.add(ep);
        }
    });
    MyList<Fragment> ppx = new MyList<>();
    ppx.add(startPolygon[0]);
    EdgeDiagram epx = new EdgeDiagram(null, null);
    HashTabulka<Fragment> vysledneFragmenty = new HashTabulka<>();
    startPolygon[0].visited = true;
    while (!ppx.isEmpty()) {
        Fragment px = ppx.get(0);
        vysledneFragmenty.add(px);
        for (int i = 0; i < px.size(); ++i) {
            Tuple2f v1 = px.get(i);
            Tuple2f v2 = px.cycleGet(i + 1);
            epx.p1 = v1;
            epx.p2 = v2;
            EdgeDiagram ep = table.get(epx);
            Fragment opposite = ep.d1 == px ? ep.d2 : ep.d1;
            if (opposite != null && !opposite.visited) {
                Tuple2f centroid = opposite.centroid();
                opposite.visited = true;
                if (ic.contains(centroid)) {
                    boolean intersection = false;
                    for (EdgeDiagram edge : allEdgesPolygon) {
                        // neberie do uvahy hrany polygonu
                        if (edge.d1 != startPolygon[0] && edge.d2 != startPolygon[0] && edge.intersectAre(centroid, kolmicovyBod[0])) {
                            intersection = true;
                            break;
                        }
                    }
                    // tu bude podmienka - ci ten polygon vezmem do uvahy, ak hej, priplnim ho do MyListu
                    if (!intersection) {
                        ppx.add(opposite);
                    }
                }
            }
        }
        ppx.removeAt(0);
    }
    Fragment[] fragmentsArray = vysledneFragmenty.toArray(new Fragment[vysledneFragmenty.size()]);
    MyList<Fragment> fragmentsBody = new MyList<>();
    for (Fragment fx : allIntersections) {
        if (!vysledneFragmenty.contains(fx)) {
            fragmentsBody.add(fx);
        }
    }
    MyList<Polygon> result = zjednotenie(fragmentsBody);
    result.add(fragmentsArray);
    fragments = new Polygon[result.size()];
    result.addToArray(fragments);
}
Also used : Fragment(spacegraph.space2d.phys.fracture.Fragment) Tuple2f(spacegraph.util.math.Tuple2f) Polygon(spacegraph.space2d.phys.fracture.Polygon) FasterList(jcog.list.FasterList) MyList(spacegraph.space2d.phys.fracture.util.MyList) HashTabulka(spacegraph.space2d.phys.fracture.util.HashTabulka)

Example 9 with FasterList

use of jcog.list.FasterList in project narchy by automenta.

the class Dbvt method topdown.

private static Node topdown(Dbvt pdbvt, FasterList<Node> leaves, int bu_treshold) {
    if (leaves.size() > 1) {
        if (leaves.size() > bu_treshold) {
            DbvtAabbMm vol = bounds(leaves);
            v3 org = vol.center(new v3());
            FasterList[] sets = new FasterList[2];
            for (int i = 0; i < sets.length; i++) {
                sets[i] = new FasterList();
            }
            int bestaxis = -1;
            int bestmidp = leaves.size();
            int[][] splitcount = { { 0, 0 }, { 0, 0 }, { 0, 0 } };
            v3 x = new v3();
            for (int i = 0; i < leaves.size(); i++) {
                // return array[index];
                leaves.get(i).volume.center(x);
                x.sub(org);
                for (int j = 0; j < 3; j++) {
                    splitcount[j][x.dot(axis[j]) > 0f ? 1 : 0]++;
                }
            }
            for (int i = 0; i < 3; i++) {
                if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0)) {
                    int midp = Math.abs(splitcount[i][0] - splitcount[i][1]);
                    if (midp < bestmidp) {
                        bestaxis = i;
                        bestmidp = midp;
                    }
                }
            }
            if (bestaxis >= 0) {
                // sets[0].reserve(splitcount[bestaxis][0]);
                // sets[1].reserve(splitcount[bestaxis][1]);
                split(leaves, sets[0], sets[1], org, axis[bestaxis]);
            } else {
                // sets[1].reserve(leaves.size()/2);
                for (int i = 0, ni = leaves.size(); i < ni; i++) {
                    // return array[index];
                    sets[i & 1].add(leaves.get(i));
                }
            }
            Node node = set(pdbvt, null, vol, null);
            node.childs[0] = topdown(pdbvt, sets[0], bu_treshold);
            node.childs[1] = topdown(pdbvt, sets[1], bu_treshold);
            node.childs[0].parent = node;
            node.childs[1].parent = node;
            return node;
        } else {
            bottomup(pdbvt, leaves);
            return leaves.get(0);
        // return array[index];
        }
    }
    return leaves.get(0);
// return array[index];
}
Also used : FasterList(jcog.list.FasterList) spacegraph.util.math.v3(spacegraph.util.math.v3)

Example 10 with FasterList

use of jcog.list.FasterList 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());
}
Also used : FasterList(jcog.list.FasterList) JointEdge(spacegraph.space2d.phys.dynamics.joints.JointEdge) Joint(spacegraph.space2d.phys.dynamics.joints.Joint) Contact(spacegraph.space2d.phys.dynamics.contacts.Contact) ContactEdge(spacegraph.space2d.phys.dynamics.contacts.ContactEdge)

Aggregations

FasterList (jcog.list.FasterList)48 Term (nars.term.Term)17 List (java.util.List)7 Truth (nars.truth.Truth)5 Pair (org.eclipse.collections.api.tuple.Pair)5 Nullable (org.jetbrains.annotations.Nullable)5 Map (java.util.Map)4 Predicate (java.util.function.Predicate)4 NAR (nars.NAR)4 Task (nars.Task)4 LongObjectPair (org.eclipse.collections.api.tuple.primitive.LongObjectPair)4 java.util (java.util)3 Supplier (java.util.function.Supplier)3 Util (jcog.Util)3 MapNodeGraph (jcog.data.graph.MapNodeGraph)3 NALTask (nars.task.NALTask)3 Bool (nars.term.atom.Bool)3 Test (org.junit.jupiter.api.Test)3 RoaringBitmap (org.roaringbitmap.RoaringBitmap)3 MultimapBuilder (com.google.common.collect.MultimapBuilder)2