Quad tree draft

lib/graph/quad_tree.hpp

@@ -0,0 +1,217 @@
+#ifndef QUAD_TREE_HPP
+#define QUAD_TREE_HPP
+
+#include <vector>
+#include <utility> // move
+#include <cassert>
+#include <cstdlib> // abs
+
+// From wikipedia: http://en.wikipedia.org/wiki/Quadtree#Pseudo_code
+
+/**
+  P shall have x and y members and ctor with signature of: P(x, y)
+
+   ~~~{.cpp}
+  struct XY {
+    float x;
+    float y;
+    XY(float _x, float _y) {...}
+  }
+  ~~~
+ */
+
+// Axis-aligned bounding box with half dimension and center
+template <typename P>
+struct AABB
+{
+  const P m_center;
+  const typename P::value_type m_halfDimension;
+
+  constexpr AABB(const P& center, typename P::value_type halfDimension)
+    : m_center(center)
+    , m_halfDimension(halfDimension) {}
+
+  bool containsPoint(const P& p) const {
+    return p.x >= m_center.x - m_halfDimension &&
+           p.x <= m_center.x + m_halfDimension &&
+           p.y >= m_center.y - m_halfDimension &&
+           p.y <= m_center.y + m_halfDimension;
+
+//     return (abs(p.x - m_center.x) <= m_halfDimension) &&
+//            (abs(p.y - m_center.y) <= m_halfDimension);
+
+  }
+
+  bool intersectsAABB(const AABB& other) const {
+//     return containsPoint(P(other.m_center.x - other.m_halfDimension, other.m_center.y - other.m_halfDimension)) ||
+//            containsPoint(P(other.m_center.x - other.m_halfDimension, other.m_center.y + other.m_halfDimension)) ||
+//            containsPoint(P(other.m_center.x + other.m_halfDimension, other.m_center.y - other.m_halfDimension)) ||
+//            containsPoint(P(other.m_center.x + other.m_halfDimension, other.m_center.y + other.m_halfDimension));
+
+//       return (abs(m_center.x - other.m_center.x) <= m_halfDimension + other.m_halfDimension) &&
+//              (abs(m_center.y - other.m_center.y) <= m_halfDimension + other.m_halfDimension);
+
+//     return dist(m_center, other.m_center) < m_halfDimension + other.m_halfDimension;
+
+    return other.m_center.x + other.m_center.x >= m_center.x - m_halfDimension &&
+           other.m_center.x - other.m_center.x <= m_center.x + m_halfDimension &&
+           other.m_center.y + other.m_center.y >= m_center.y - m_halfDimension &&
+           other.m_center.y - other.m_center.y <= m_center.y + m_halfDimension;
+  }
+};
+
+template <typename P>
+class QuadTree {
+public:
+
+  QuadTree(const AABB<P>& boundary)
+    : m_boundary(boundary)
+    , m_points()
+    , m_northWest(0)
+    , m_northEast(0)
+    , m_southWest(0)
+    , m_southEast(0) { m_points.reserve(m_node_capacity); }
+
+  QuadTree(const QuadTree& other)
+    : m_boundary (other.m_boundary)
+    , m_points   (other.m_points)
+    , m_northWest(other.m_northWest)
+    , m_northEast(other.m_northEast)
+    , m_southWest(other.m_southWest)
+    , m_southEast(other.m_southEast) {}
+
+  QuadTree(QuadTree&& other)
+    : m_boundary (std::move(other.m_boundary))
+    , m_points   (std::move(other.m_points))
+    , m_northWest(std::move(other.m_northWest))
+    , m_northEast(std::move(other.m_northEast))
+    , m_southWest(std::move(other.m_southWest))
+    , m_southEast(std::move(other.m_southEast)) {}
+
+  QuadTree& operator=(const QuadTree other) { swap(other); return *this; }
+  void swap(QuadTree& other) {
+    std::swap(m_boundary,  other.m_boundary);
+    std::swap(m_points,    other.m_points);
+    std::swap(m_northWest, other.m_northWest);
+    std::swap(m_northEast, other.m_northEast);
+    std::swap(m_southWest, other.m_southWest);
+    std::swap(m_southEast, other.m_southEast);
+  }
+
+  ~QuadTree() {
+    delete m_northWest;
+    delete m_northEast;
+    delete m_southWest;
+    delete m_southEast;
+  }
+
+  bool insert(const P& p) {
+    // Ignore objects that do not belong in this quad tree
+    if (!m_boundary.containsPoint(p))
+      return false; // object cannot be added
+
+    // If there is space in this quad tree, add the object here
+    if (m_points.size() < m_node_capacity) {
+      m_points.push_back(p);
+      return true;
+    }
+
+    // Otherwise, subdivide and then add the point to whichever node will accept it
+    if (m_northWest == 0)
+      subdivide();
+
+    if (m_northWest->insert(p)) return true;
+    if (m_northEast->insert(p)) return true;
+    if (m_southWest->insert(p)) return true;
+    if (m_southEast->insert(p)) return true;
+
+    // Otherwise, the point cannot be inserted for some unknown reason (this should never happen)
+    assert(!"Programming error");
+    return false;
+  }
+
+   // create four children that fully divide this quad into four quads of equal area
+  void subdivide() {
+    const typename P::value_type h = m_boundary.m_halfDimension / 2;
+    const typename P::value_type x = m_boundary.m_center.x;
+    const typename P::value_type y = m_boundary.m_center.y;
+    m_northWest = new QuadTree<P>(AABB<P>(P(x - h, y - h), h));
+    m_northEast = new QuadTree<P>(AABB<P>(P(x + h, y - h), h));
+    m_southWest = new QuadTree<P>(AABB<P>(P(x - h, y + h), h));
+    m_southEast = new QuadTree<P>(AABB<P>(P(x + h, y + h), h));
+  }
+
+  std::vector<P> queryRange(const AABB<P>& range) const {
+
+    // Prepare an array of results
+    std::vector<P> pointsInRange;
+
+    // Automatically abort if the range does not intersect this quad
+    if (!m_boundary.intersectsAABB(range))
+      return pointsInRange; // empty list
+
+    // Check objects at this quad level
+    for (std::size_t p = 0; p < m_points.size(); ++p)
+      if (range.containsPoint(m_points[p]))
+        pointsInRange.push_back(m_points[p]);
+
+    // Terminate here, if there are no children
+    if (m_northWest == 0)
+      return pointsInRange;
+
+    // Otherwise, add the points from the children
+    const std::vector<P> nw = m_northWest->queryRange(range);
+    pointsInRange.insert(pointsInRange.end(), nw.begin(), nw.end());
+    const std::vector<P> ne = m_northEast->queryRange(range);
+    pointsInRange.insert(pointsInRange.end(), ne.begin(), ne.end());
+    const std::vector<P> sw = m_southWest->queryRange(range);
+    pointsInRange.insert(pointsInRange.end(), sw.begin(), sw.end());
+    const std::vector<P> se = m_southEast->queryRange(range);
+    pointsInRange.insert(pointsInRange.end(), se.begin(), se.end());
+
+    return pointsInRange;
+  }
+
+  std::vector<P> points() const {
+
+    std::vector<P> retval(m_points.begin(), m_points.end());
+
+    // Terminate here, if there are no children
+    if (m_northWest == 0)
+      return retval;
+
+    // Otherwise, add the points from the children
+    const std::vector<P> nw = m_northWest->points();
+    retval.insert(retval.end(), nw.begin(), nw.end());
+    const std::vector<P> ne = m_northEast->points();
+    retval.insert(retval.end(), ne.begin(), ne.end());
+    const std::vector<P> sw = m_southWest->points();
+    retval.insert(retval.end(), sw.begin(), sw.end());
+    const std::vector<P> se = m_southEast->points();
+    retval.insert(retval.end(), se.begin(), se.end());
+
+    return retval;
+  }
+
+
+private:
+
+  // Arbitrary constant to indicate how many elements can be stored in this quad tree node
+  static const std::size_t m_node_capacity = 4;
+
+  // Axis-aligned bounding box stored as a center with half-dimensions
+  // to represent the boundaries of this quad tree
+  const AABB<P> m_boundary;
+
+  // Points in this quad tree node
+  std::vector<P> m_points;
+
+  // Children
+  QuadTree* m_northWest;
+  QuadTree* m_northEast;
+  QuadTree* m_southWest;
+  QuadTree* m_southEast;
+};
+
+
+#endif // QUAD_TREE_HPP

test/graph/test_quad_tree.cpp

@@ -0,0 +1,105 @@
+#include <graph/quad_tree.hpp>
+
+#include "../catch.hpp"
+
+#include "fixture.hpp"
+
+
+
+TEST_CASE( "Quad tree", "[quad_tree][data_structure]" ) {
+
+  const float2 center(0, 0);
+  constexpr float halfDimension(10);
+  const AABB<float2> boundary(center, halfDimension);
+  QuadTree<float2> t(boundary);
+
+  SECTION("Insert point") {
+    REQUIRE ( t.insert(float2(0, 0)) == true );
+  }
+  SECTION("Insert point 2x") {
+    REQUIRE ( t.insert(float2(0, 0)) == true );
+    REQUIRE ( t.insert(float2(0, 0)) == true );
+  }
+
+  SECTION("Insert point outside") {
+    REQUIRE ( t.insert(float2(20, 20)) == false );
+  }
+
+  SECTION("Insert some points") {
+    for (int c = -3; c < 3; ++c)
+      for (int r = -3; r < 3; ++r)
+        REQUIRE ( t.insert(float2(center.x + r, center.y + c)) == true );
+
+    const std::vector<float2> all_points = t.points();
+    REQUIRE ( all_points.size() == 6*6 );
+  }
+
+  SECTION("Insert many points") {
+    for (int c = -halfDimension; c < halfDimension; ++c)
+      for (int r = -halfDimension; r < halfDimension; ++r)
+        REQUIRE ( t.insert(float2(center.x + r, center.y + c)) == true );
+
+    const std::vector<float2> all_points = t.points();
+    REQUIRE ( all_points.size() == std::size_t(halfDimension*2 * halfDimension*2) ); // 400
+  }
+
+  SECTION("Query empty tree") {
+    REQUIRE ( t.queryRange(boundary).size() == 0 );
+  }
+
+  SECTION("Query 1") {
+    const float2 p1(0, 0);
+    t.insert(p1);
+    const std::vector<float2> points =  t.queryRange(boundary);
+
+    REQUIRE ( points.size() == 1);
+    REQUIRE ( points[0] == p1);
+  }
+
+  SECTION("Query 1, bad range") {
+    const float2 p1(0, 0);
+    t.insert(p1);
+    const std::vector<float2> points =  t.queryRange(AABB<float2>(float2(-halfDimension, -halfDimension), halfDimension/2));
+    REQUIRE ( points.size() == 0);
+  }
+
+  SECTION("Query intersection") {
+    const float2 p1(0, 0);
+    const float2 p2(-halfDimension, -halfDimension);
+    t.insert(p1);
+    t.insert(p2);
+    const std::vector<float2> points =  t.queryRange(AABB<float2>(float2(-halfDimension, -halfDimension), halfDimension/2));
+
+    REQUIRE ( points.size() == 1);
+    REQUIRE ( points[0] == p2);
+  }
+
+  SECTION("Query many points topLeft") {
+    for (int c = -halfDimension; c < halfDimension; ++c)
+      for (int r = -halfDimension; r < halfDimension; ++r)
+        REQUIRE ( t.insert(float2(center.x + r, center.y + c)) == true );
+
+    const float2 topLeft(-halfDimension, -halfDimension);
+    const std::vector<float2> points =  t.queryRange(AABB<float2>(topLeft, halfDimension/2));
+
+    REQUIRE ( points.size() == 25);
+    for (int c = -halfDimension; c < halfDimension/2; ++c)
+      for (int r = -halfDimension; r < halfDimension/2; ++r)
+        REQUIRE ( std::find(points.begin(), points.end(), float2(r, c)) != points.end() );
+  }
+
+  SECTION("Query many points inside") {
+    for (int c = -halfDimension; c < halfDimension; ++c)
+      for (int r = -halfDimension; r < halfDimension; ++r)
+        REQUIRE ( t.insert(float2(center.x + r, center.y + c)) == true );
+
+    const float2 right(0, halfDimension/2);
+    const std::vector<float2> points =  t.queryRange(AABB<float2>(right, halfDimension/4));
+
+    REQUIRE ( points.size() == 25);
+    const float2 range_topleft(halfDimension/4, -halfDimension/4);
+    for (int c = 0; c < halfDimension/2; ++c)
+      for (int r = 0; r < halfDimension/2; ++r)
+        REQUIRE ( std::find(points.begin(), points.end(), float2(range_topleft.x + r, range_topleft.y + c)) != points.end() );
+  }
+}