public boolean add(Entity entity, ComponentMapper<Mass> mm, ComponentMapper<Position> pm) {
if (!contains(pm.get(entity).vec)) { // TODO shouldn't be necessary
return false;
}
if (isEmpty()) {
this.entity = entity;
this.mass = mm.get(entity).mass;
this.massVector = pm.get(entity).vec.cpy().mul(mass);
++size;
return true;
} else {
if (bl == null) {
addSubtrees();
quadrantOf(this.entity, mm, pm).add(this.entity, mm, pm);
this.entity = null;
}
quadrantOf(entity, mm, pm).add(entity, mm, pm);
double m = mm.get(entity).mass;
VectorD2 p = pm.get(entity).vec;
this.massVector.add(p.cpy().mul(m));
this.mass += m;
++size;
return true;
}
}
private void update(List<Entity> moved, ComponentMapper<Mass> mm, ComponentMapper<Position> pm) {
if (hasEntity()) {
if (entity.isActive()) {
VectorD2 p = pm.get(entity).vec;
if (!contains(p)) {
moved.add(entity);
--size;
this.mass = 0f;
this.massVector.set(0f, 0f);
entity = null;
} else {
mass = mm.get(entity).mass;
massVector.set(p).mul(mass);
}
}
} else if (hasChildren()) {
bl.update(moved, mm, pm);
br.update(moved, mm, pm);
tl.update(moved, mm, pm);
tr.update(moved, mm, pm);
size = bl.size + br.size + tl.size + tr.size;
mass = bl.mass + br.mass + tl.mass + tr.mass;
massVector.set(0f, 0f);
massVector.add(bl.massVector).add(br.massVector);
massVector.add(tl.massVector).add(tr.massVector);
}
}
public void updateAcceleration(
Entity e,
float theta,
float G,
ComponentMapper<Mass> mm,
ComponentMapper<Position> pm,
ComponentMapper<Acceleration> am,
float delta) {
if (isEmpty()) { // TODO also check if M is to small?
return;
}
double M = this.mass;
VectorD2 p = pm.get(e).vec;
VectorD2 diff;
// if (contains(p)) { // TODO this is only necessary if theta>=1
// float m = mm.get(e).mass;
// diff = this.massVector.cpy().sub(p.cpy().mul(m)).div(M-m).sub(p);
// } else
diff = this.massVector.cpy().div(M).sub(p);
double d2 = diff.len2();
// s/d < theta
if (this.entity != null || side * side < theta * theta * d2) { // TODO weight with mass.
if (this.entity == e) { // TODO temporary hack
return;
}
// F = G*m*M/d^2
// F = m*a
// a = G*M/d^2
double a = G * M / d2;
double k = (float) (a * FastMath.inverseSqrt(d2)); // normalizes diff
if (!Double.isNaN(k)) {
VectorD2 accVec = am.get(e).vec;
accVec.add(diff.mul(k).mul(delta));
}
} else {
bl.updateAcceleration(e, theta, G, mm, pm, am, delta);
br.updateAcceleration(e, theta, G, mm, pm, am, delta);
tl.updateAcceleration(e, theta, G, mm, pm, am, delta);
tr.updateAcceleration(e, theta, G, mm, pm, am, delta);
}
}
public boolean remove(Entity entity, ComponentMapper<Mass> mm, ComponentMapper<Position> pm) {
if (hasEntity()) {
if (this.entity == entity) {
this.entity = null;
mass = 0f;
massVector.set(0f, 0f);
--size;
return true;
}
} else if (hasChildren()) {
GravQuadTree sub = quadrantOf(entity, mm, pm);
double oldMass = sub.mass;
VectorD2 oldVector = sub.massVector.cpy();
if (sub.remove(entity, mm, pm)) {
--size;
mass -= oldMass - sub.mass;
massVector.sub(oldVector).add(sub.massVector);
}
}
return false;
}
private void addSubtrees() {
double subside = side / 2;
bl = new GravQuadTree(pos.cpy(), subside);
br = new GravQuadTree(pos.cpy(), subside);
br.pos.x += subside;
tl = new GravQuadTree(pos.cpy(), subside);
tl.pos.y += subside;
tr = new GravQuadTree(pos.cpy(), subside);
tr.pos.x += subside;
tr.pos.y += subside;
}
/**
* Creates a new QuadTree spanning the given cube.
*
* @param pos bottomLeft corner of cube
* @param side length of each side in cube
*/
public GravQuadTree(VectorD2 pos, double side) {
this.pos = pos;
this.side = side;
massVector = pos.cpy();
}