Not working

src/Octree.java

@@ -0,0 +1,167 @@
+
+/**
+ * + -> >= 0
+ * - -> < 0
+ * 0 -> x+ y+ z+
+ * 1 -> x- y+ z+
+ * 2 -> x+ y- z+
+ * 3 -> x- y- z+
+ * 4 -> x+ y+ z-
+ * 5 -> x- y+ z-
+ * 6 -> x+ y- z-
+ * 7 -> x- y- z-
+ */
+public class Octree {
+    private OctreeItem root;
+    private final Vector center;
+    private final double size;
+
+    public Octree(double size) {
+        this(new Vector(), size);
+    }
+
+    public Octree(Vector center, double size) {
+        this.center = center;
+        this.size = size;
+    }
+
+    public void add(Body body) {
+        if (root == null) {
+            root = new OctreeLeaf(center, size, body);
+        } else {
+            root = root.add(body);
+        }
+    }
+
+    public void applyForces(Body[] bodies, double t) {
+        if (root == null) return;
+        root.preCalc();
+        Vector[] forces = new Vector[bodies.length];
+        for (int i = 0; i < bodies.length; i++) {
+            forces[i] = root.getForcesOnBody(bodies[i], t);
+        }
+        for (int i = 0; i < bodies.length; i++) {
+            bodies[i].move(forces[i]);
+        }
+    }
+}
+
+abstract class OctreeItem {
+    protected Vector center;
+    protected double size;
+
+    protected Body pseudoBody;
+
+    protected OctreeItem(Vector center, double size) {
+        this.center = center;
+        this.size = size;
+    }
+
+    abstract protected OctreeNode add(Body body);
+
+    abstract protected void preCalc();
+
+    abstract protected Vector getForcesOnBody(Body body, double t);
+}
+
+class OctreeNode extends OctreeItem {
+    private final OctreeItem[] children = new OctreeItem[8];;
+
+    protected OctreeNode(Vector center, double size) {
+        super(center, size);
+    }
+
+    @Override
+    protected OctreeNode add(Body body) {
+        int num = getOctantNum(body.massCenter());
+        if (num < 0) return this;
+        OctreeItem oct = children[num];
+        if (oct == null) {
+            children[num] = new OctreeLeaf(getOctantCenter(num), size / 2.0, body);
+        } else {
+            children[num] = oct.add(body);
+        }
+        return this;
+    }
+
+    @Override
+    protected void preCalc() {
+        double mass = 0;
+        for (OctreeItem oct : children) {
+            if (oct == null) continue;
+            oct.preCalc();
+            mass += oct.pseudoBody.mass();
+        }
+        Vector massCenter = new Vector();
+        for (OctreeItem oct : children) {
+            if (oct == null) continue;
+            massCenter = massCenter.plus(oct.pseudoBody.massCenter().times(oct.pseudoBody.mass() / mass));
+        }
+        this.pseudoBody = new Body(mass, massCenter);
+    }
+
+    @Override
+    protected Vector getForcesOnBody(Body body, double t) {
+        double r = pseudoBody.massCenter().distanceTo(body.massCenter());
+        if (r == 0) {
+            return new Vector();
+        } else if (size / r < t) {
+            return body.gravitationalForce(pseudoBody);
+        }
+        Vector force = new Vector();
+        for (OctreeItem child : children) {
+            if (child == null) continue;
+            force = force.plus(child.getForcesOnBody(body, t));
+        }
+        return force;
+    }
+
+    private int getOctantNum(Vector bodyPos) {
+        Vector pos = bodyPos.minus(this.center);
+        if (!isInOctant(bodyPos)) return -1;
+        return ((pos.getX() < 0) ? 1 : 0) | ((pos.getY() < 0) ? 2 : 0) | ((pos.getZ() < 0) ? 4 : 0);
+    }
+
+    private boolean isInOctant(Vector pos) {
+        Vector p1 = center.plus(new Vector(size / 2));
+        Vector p2 = center.minus(new Vector(size / 2));
+        return  pos.getX() <= p1.getX() && pos.getY() <= p1.getY() && pos.getZ() <= p1.getZ() &&
+                pos.getX() >= p2.getX() && pos.getY() >= p2.getY() && pos.getZ() >= p2.getZ();
+    }
+
+    private Vector getOctantCenter(int octNum) {
+        return this.center.plus(new Vector(
+                ((octNum & 1) != 0) ? -0.5 : 0.5,
+                ((octNum & 2) != 0) ? -0.5 : 0.5,
+                ((octNum & 4) != 0) ? -0.5 : 0.5)
+                .times(this.size));
+    }
+}
+
+class OctreeLeaf extends OctreeItem {
+    private final Body body;
+
+    public OctreeLeaf(Vector center, double size, Body body) {
+        super(center, size);
+        this.body = body;
+    }
+
+    @Override
+    protected OctreeNode add(Body body) {
+        OctreeNode replace = new OctreeNode(this.center, this.size);
+        replace.add(this.body);
+        replace.add(body);
+        return replace;
+    }
+
+    @Override
+    protected void preCalc() {
+        System.out.println(this.body);
+        this.pseudoBody = new Body(this.body);
+    }
+
+    @Override
+    protected Vector getForcesOnBody(Body body, double t) {
+        return body.gravitationalForce(pseudoBody);
+    }
+}