From 0fdfa7a77f322aabf9689c6d62d63913c8f1a1f0 Mon Sep 17 00:00:00 2001 From: dmatetelki Date: Wed, 23 Sep 2015 16:42:46 +0200 Subject: [PATCH] Quad tree draft --- lib/graph/quad_tree.hpp | 217 ++++++++++++++++++++++++++++++++++ lib/qtgraph/first_map.png | Bin 954 -> 0 bytes test/graph/test_quad_tree.cpp | 105 ++++++++++++++++ 3 files changed, 322 insertions(+) create mode 100644 lib/graph/quad_tree.hpp delete mode 100644 lib/qtgraph/first_map.png create mode 100644 test/graph/test_quad_tree.cpp diff --git a/lib/graph/quad_tree.hpp b/lib/graph/quad_tree.hpp new file mode 100644 index 0000000..2d0a3bc --- /dev/null +++ b/lib/graph/quad_tree.hpp @@ -0,0 +1,217 @@ +#ifndef QUAD_TREE_HPP +#define QUAD_TREE_HPP + +#include +#include // move +#include +#include // 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 +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 +class QuadTree { +public: + + QuadTree(const AABB

& 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

(AABB

(P(x - h, y - h), h)); + m_northEast = new QuadTree

(AABB

(P(x + h, y - h), h)); + m_southWest = new QuadTree

(AABB

(P(x - h, y + h), h)); + m_southEast = new QuadTree

(AABB

(P(x + h, y + h), h)); + } + + std::vector

queryRange(const AABB

& range) const { + + // Prepare an array of results + std::vector

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

nw = m_northWest->queryRange(range); + pointsInRange.insert(pointsInRange.end(), nw.begin(), nw.end()); + const std::vector

ne = m_northEast->queryRange(range); + pointsInRange.insert(pointsInRange.end(), ne.begin(), ne.end()); + const std::vector

sw = m_southWest->queryRange(range); + pointsInRange.insert(pointsInRange.end(), sw.begin(), sw.end()); + const std::vector

se = m_southEast->queryRange(range); + pointsInRange.insert(pointsInRange.end(), se.begin(), se.end()); + + return pointsInRange; + } + + std::vector

points() const { + + std::vector

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

nw = m_northWest->points(); + retval.insert(retval.end(), nw.begin(), nw.end()); + const std::vector

ne = m_northEast->points(); + retval.insert(retval.end(), ne.begin(), ne.end()); + const std::vector

sw = m_southWest->points(); + retval.insert(retval.end(), sw.begin(), sw.end()); + const std::vector

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

m_boundary; + + // Points in this quad tree node + std::vector

m_points; + + // Children + QuadTree* m_northWest; + QuadTree* m_northEast; + QuadTree* m_southWest; + QuadTree* m_southEast; +}; + + +#endif // QUAD_TREE_HPP diff --git a/lib/qtgraph/first_map.png b/lib/qtgraph/first_map.png deleted file mode 100644 index 70b536da7b6b115113819de872458b614f12cc0a..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 954 zcmeAS@N?(olHy`uVBq!ia0y~yU<5K5893O0R7}x|GzJFdMo$;VkcwMx?>hD!a^P`w zlt1YH@vlDntJH0cZDld4f8Ta^&Qd>OpcHo?;-OFj0|OHS2ZI6wUPg?(OUvDeP|JfH z2c9$nm2)&OI2>k3S*HHEckOGox6H^U%;lfC|Ax$O&j3aT2DW-W7X|^K`xpfnZX8gz zJ^xo_1;c?S;puve1nr#jiTig{z5qk~D^4Z_kc}J+K$;0iHvpYWl90pk-$rlm?`L&j zxHJ9W_h<$gj_OqPb6Q&5?n=|*vvh^VE#S!5>VuBr64X#)!*>O`aIxJLg9fN@>LpCssK%qkv z6+dNW^t?Z>pDW>ydt^soMj9?>;Z06|{vC3e5gQf?j6}0aAfX0|PP!8x7M>}2@OD$F n8VfKs9YDE=d + +#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 boundary(center, halfDimension); + QuadTree 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 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 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 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 points = t.queryRange(AABB(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 points = t.queryRange(AABB(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 points = t.queryRange(AABB(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 points = t.queryRange(AABB(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() ); + } +}