1 files changed, 59 insertions, 1 deletions
diff --git a/modules/Geometry.js b/modules/Geometry.js
index f32dc0e..48e682a 100644
--- a/modules/Geometry.js
+++ b/modules/Geometry.js
@@ -91,6 +91,64 @@ class QTNode {
 			new QTNode(x+w, y+h, w, h),
 		];
 	}
+
+	getPointsInRegion(aabb) {
+		if (this.type == QTNode.Empty) return [];
+		if (!this.aabb.intersects(aabb)) return [];
+		if (this.type == QTNode.Leaf) return [this.point];
+		
+		return [
+			...this.subnode[0].getPointsInRegion(aabb),
+			...this.subnode[1].getPointsInRegion(aabb),
+			...this.subnode[2].getPointsInRegion(aabb),
+			...this.subnode[3].getPointsInRegion(aabb),
+		];
+	}
+}
+
+
+class QuadTree {
+	constructor(width, height) {
+		this.root = new QTNode(0, 0, width, height);
+	}
+
+	insert(point) {
+		return this.root.insert(point);
+	}
+
+	closest(point) {
+		const getLeaf = (point, node) => {
+			if (!node.aabb.contains(point)) return null;
+			if (node.type == QTNode.Empty ||
+			    node.type == QTNode.Leaf) return node;
+			
+			for (let subnode of node.subnode) {
+				const leaf = getLeaf(point, subnode);
+				if (leaf !== null) return leaf;
+			}
+
+			return null;
+		}
+
+		const leaf = getLeaf(point, this.root);
+		if (leaf === null) return leaf;
+
+		const dim = 2*Math.max(leaf.aabb.width, leaf.aabb.height);
+		const region = new AABB(point.x - dim, point.y - dim, 2*dim, 2*dim);
+		const points = this.root.getPointsInRegion(region);
+
+		let closest = null;
+		let min_dist = 100 * this.root.aabb.width * this.root.aabb.height;
+		const dist = (a, b) => (a.x - b.x)**2 + (a.y - b.y)**2;
+		for (let pt of points) {
+			const d = dist(point, pt);
+			if (d < min_dist) {
+				closest = pt;
+				min_dist = d;
+			}
+		}
+		return closest;
+	}
 }
 
-export { AABB, QTNode };
+export { AABB, QTNode, QuadTree };