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));
}
}
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);
}
}
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);
}
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];
}
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());
}
Aggregations