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;
}
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;
}
}
/**
* 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();
}
