1 files changed, 144 insertions, 0 deletions
diff --git a/quadtree.cpp b/quadtree.cpp
new file mode 100644
index 0000000..0ff0bf6
--- /dev/null
+++ b/quadtree.cpp
@@ -0,0 +1,144 @@
+#include <limits>
+
+#include "quadtree.h"
+
+static const float BRUSHING_MAX_DIST = 20.0f;
+
+QuadTree::QuadTree(const RectF &bounds)
+    : m_bounds(bounds)
+    , m_value(-1)
+{
+}
+
+bool QuadTree::subdivide()
+{
+    float halfWidth = m_bounds.width() / 2;
+    float halfHeight = m_bounds.height() / 2;
+
+    m_nw.reset(new QuadTree(RectF(m_bounds.x(),
+                                  m_bounds.y(),
+                                  halfWidth,
+                                  halfHeight)));
+    m_ne.reset(new QuadTree(RectF(m_bounds.x() + halfWidth,
+                                  m_bounds.y(),
+                                  halfWidth,
+                                  halfHeight)));
+    m_sw.reset(new QuadTree(RectF(m_bounds.x(),
+                                  m_bounds.y() + halfHeight,
+                                  halfWidth,
+                                  halfHeight)));
+    m_se.reset(new QuadTree(RectF(m_bounds.x() + halfWidth,
+                                  m_bounds.y() + halfHeight,
+                                  halfWidth,
+                                  halfHeight)));
+
+    int value = m_value;
+    m_value = -1;
+    return m_nw->insert(m_x, m_y, value)
+        || m_ne->insert(m_x, m_y, value)
+        || m_sw->insert(m_x, m_y, value)
+        || m_se->insert(m_x, m_y, value);
+}
+
+bool QuadTree::insert(float x, float y, int value)
+{
+    if (!m_bounds.contains(x, y)) {
+        return false;
+    }
+
+    if (m_nw) {
+        return m_nw->insert(x, y, value)
+            || m_ne->insert(x, y, value)
+            || m_sw->insert(x, y, value)
+            || m_se->insert(x, y, value);
+    }
+
+    if (m_value >= 0) {
+        subdivide();
+        return insert(x, y, value);
+    }
+
+    m_x = x;
+    m_y = y;
+    m_value = value;
+    return true;
+}
+
+int QuadTree::nearestTo(float x, float y) const
+{
+    if (!m_bounds.contains(x, y)) {
+        return -1;
+    }
+
+    int q;
+    if (m_nw) {
+        q = m_nw->nearestTo(x, y);
+        if (q >= 0) return q;
+        q = m_ne->nearestTo(x, y);
+        if (q >= 0) return q;
+        q = m_sw->nearestTo(x, y);
+        if (q >= 0) return q;
+        q = m_se->nearestTo(x, y);
+        if (q >= 0) return q;
+    }
+
+    float dist = std::numeric_limits<float>::infinity();
+    nearestTo(x, y, q, dist);
+    if (dist < BRUSHING_MAX_DIST * BRUSHING_MAX_DIST)
+        return q;
+    return -1;
+}
+
+void QuadTree::nearestTo(float x, float y, int &nearest, float &dist) const
+{
+    if (m_nw) {
+        m_nw->nearestTo(x, y, nearest, dist);
+        m_ne->nearestTo(x, y, nearest, dist);
+        m_sw->nearestTo(x, y, nearest, dist);
+        m_se->nearestTo(x, y, nearest, dist);
+    } else if (m_value >= 0) {
+        float d = (m_x - x)*(m_x - x) + (m_y - y)*(m_y - y);
+        if (d < dist) {
+            nearest = m_value;
+            dist = d;
+        }
+    }
+}
+
+int QuadTree::query(float x, float y) const
+{
+    if (!m_bounds.contains(x, y)) {
+        // There is no way we could find the point
+        return -1;
+    }
+
+    if (m_nw) {
+        int q = -1;
+        q = m_nw->query(x, y);
+        if (q >= 0) return q;
+        q = m_ne->query(x, y);
+        if (q >= 0) return q;
+        q = m_sw->query(x, y);
+        if (q >= 0) return q;
+        q = m_se->query(x, y);
+        return q;
+    }
+
+    return m_value;
+}
+
+void QuadTree::query(const RectF &rect, std::vector<int> &result) const
+{
+    if (!m_bounds.intersects(rect)) {
+        return;
+    }
+
+    if (m_nw) {
+        m_nw->query(rect, result);
+        m_ne->query(rect, result);
+        m_sw->query(rect, result);
+        m_se->query(rect, result);
+    } else if (rect.contains(m_x, m_y) && m_value != -1) {
+        result.push_back(m_value);
+    }
+}