2 files changed, 88 insertions, 3 deletions

diff --git a/modules/Geometry.js b/modules/Geometry.js
index c34f4d2..f32dc0e 100644
--- a/modules/Geometry.js
+++ b/modules/Geometry.js
@@ -55,12 +55,42 @@ class QTNode {
 
 	insert(point) {
 		if (this.aabb.contains(point)) {
-			this.type = QTNode.Leaf;
-			this.point = point;
-			return true;
+			if (this.type == QTNode.Empty) {
+				this.type = QTNode.Leaf;
+				this.point = point;
+				return true;
+			}
+			else if (this.type == QTNode.Leaf) {
+				this.type = QTNode.Branch;
+				this.subdivide();
+				let result = this.insert(this.point);
+				if (!result) return false;
+				this.point = undefined;
+				return this.insert(point);
+			}
+			else {
+				// Branch
+				if (this.subnode[0].insert(point)) return true;
+				if (this.subnode[1].insert(point)) return true;
+				if (this.subnode[2].insert(point)) return true;
+				if (this.subnode[3].insert(point)) return true;
+				return false; // something strange has happened!!
+			}
 		}
 		return false;
 	}
+
+	subdivide() {
+		const x = this.aabb.x; const y = this.aabb.y;
+		const w = this.aabb.width/2; const h = this.aabb.height/2;
+
+		this.subnode = [
+			new QTNode(x, y, w, h),
+			new QTNode(x+w, y, w, h),
+			new QTNode(x, y+h, w, h),
+			new QTNode(x+w, y+h, w, h),
+		];
+	}
 }
 
 export { AABB, QTNode };
diff --git a/modules/Geometry.test.js b/modules/Geometry.test.js
index 45ce397..b44f25d 100644
--- a/modules/Geometry.test.js
+++ b/modules/Geometry.test.js
@@ -75,6 +75,24 @@ test('AABB correctly intersects other AABBs', () => {
 });
 
 
+test('AABB correctly handles points at the edges', () => {
+	const box = new AABB(0, 0, 1, 1);
+
+	assert.ok(box.contains({ x: 0, y: 0 }));
+	assert.ok(box.contains({ x: 0.5, y: 0 }));
+	assert.ok(box.contains({ x: 0, y: 0.5 }));
+
+	// bad corners
+	assert.ok(!box.contains({ x: 1, y: 0 }));
+	assert.ok(!box.contains({ x: 0, y: 1 }));
+	assert.ok(!box.contains({ x: 1, y: 1 }));
+
+	// bad edges
+	assert.ok(!box.contains({ x: 1, y: 0.5 }));
+	assert.ok(!box.contains({ x: 0.5, y: 1 }));
+});
+
+
 test('QTNode correctly inserts points', () => {
 	const node = new QTNode(0, 0, 1, 1);
 	assert.equal(node.type.toString(), 'QTNode.Empty');
@@ -88,3 +106,40 @@ test('QTNode correctly inserts points', () => {
 	assert.equal(node.type.toString(), 'QTNode.Leaf');
 	assert.deepEqual(node.point, { x: 0.5, y: 0.5 });
 });
+
+
+test('QTNode correctly subdivides', () => {
+	const node = new QTNode(0, 0, 2, 2);
+	assert.ok(!node.subnode);
+	node.subdivide();
+	assert.equal(node.subnode.length, 4);
+
+	assert.deepEqual(node.subnode[0], new QTNode(0, 0, 1, 1));
+	assert.deepEqual(node.subnode[1], new QTNode(1, 0, 1, 1));
+	assert.deepEqual(node.subnode[2], new QTNode(0, 1, 1, 1));
+	assert.deepEqual(node.subnode[3], new QTNode(1, 1, 1, 1));
+});
+
+
+test('QTNode correctly inserts multiple points', () => {
+	const node = new QTNode(0, 0, 2, 2);
+	
+	const p0 = { x: 1, y: 1 };
+	const p1 = { x: 0.5, y: 0.5 };
+	const oob = { x: 10, y: 15 };
+
+	assert.ok(node.insert(p0));
+	assert.ok(node.insert(p1));
+	assert.ok(!node.insert(oob));
+
+	assert.equal(node.type.toString(), 'QTNode.Branch');
+	
+	assert.equal(node.subnode[0].type.toString(), 'QTNode.Leaf');
+	assert.deepEqual(node.subnode[0].point, p1);
+
+	assert.equal(node.subnode[1].type.toString(), 'QTNode.Empty');
+	assert.equal(node.subnode[2].type.toString(), 'QTNode.Empty');
+
+	assert.equal(node.subnode[3].type.toString(), 'QTNode.Leaf');
+	assert.deepEqual(node.subnode[3].point, p0);
+});