cquadtree/lock_quadtree.cpp at master · rob05c/cquadtree · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
#include <iostream>
#include <vector>
#include "quadtree.h"
#include "lock_quadtree.h"
#include <atomic>
#include <memory>
#include <functional> //debug
#include <thread> //debug
#include <algorithm>
#include <mutex>
namespace
{
using std::vector;
using std::cout;
using std::endl;
}

namespace quadtree
{
LockQuadtree::LockQuadtree(BoundingBox boundary_, size_t capacity_)
  : boundary(boundary_)
  , capacity(capacity_)
  , Nw(nullptr)
  , Ne(nullptr)
  , Sw(nullptr)
  , Se(nullptr)
{}

bool LockQuadtree::Insert(const Point& p)
{
  if(!boundary.Contains(p))
    return false;

  pointsMutex.lock();
  if(points.size() < capacity)
  {
    points.push_back(p);
    pointsMutex.unlock();
    return true;
  }

  if(!points.empty())
    subdivide();

  const bool ok = Nw->Insert(p) || Ne->Insert(p) || Sw->Insert(p) || Se->Insert(p);
  pointsMutex.unlock();
  return ok;
}

void LockQuadtree::subdivide()
{
  const double dx = 0.000001;
  // don't subdivide further if we reach the limits of double precision
  if(fabs(boundary.HalfDimension.X/2.0) < dx || fabs(boundary.HalfDimension.Y/2.0) < dx)
    capacity = std::numeric_limits<size_t>::max();

  const Point newHalf = {boundary.HalfDimension.X / 2.0, boundary.HalfDimension.Y / 2.0};
  Point newCenter = {boundary.Center.X - boundary.HalfDimension.X/2.0, boundary.Center.Y - boundary.HalfDimension.Y/2.0};
  BoundingBox newBoundary = {newCenter, newHalf};
  Nw = new LockQuadtree(newBoundary, capacity);;

  newCenter = {boundary.Center.X + boundary.HalfDimension.X/2.0, boundary.Center.Y - boundary.HalfDimension.Y/2.0};
  newBoundary = {newCenter, newHalf};
  Ne = new LockQuadtree(newBoundary, capacity);

  newCenter = {boundary.Center.X + boundary.HalfDimension.X/2.0, boundary.Center.Y + boundary.HalfDimension.Y/2.0};
  newBoundary = {newCenter, newHalf};
  Se = new LockQuadtree(newBoundary, capacity);

  newCenter = {boundary.Center.X - boundary.HalfDimension.X/2.0, boundary.Center.Y + boundary.HalfDimension.Y/2.0};
  newBoundary = {newCenter, newHalf};
  Sw = new LockQuadtree(newBoundary, capacity);

  disperse();
  capacity = 0;
}

void LockQuadtree::disperse()
{
  for(auto i = points.begin(), end = points.end(); i != end; ++i)
  {
    Point p = *i;
    const bool ok = Nw->Insert(p) || Ne->Insert(p) || Sw->Insert(p) || Se->Insert(p);
    if(!ok)
      cout << "disperse insert failed.";
  }
  points.clear();
}

vector<Point> LockQuadtree::Query(const BoundingBox& b)
{
  pointsMutex.lock();
  vector<Point> found;

  if(!boundary.Intersects(b))
    return found;

  for(auto i = points.begin(), end = points.end(); i != end; ++i)
  {
    if(b.Contains(*i))
      found.push_back(*i);
  }
  pointsMutex.unlock();

  if(Nw != nullptr)
  {
    vector<Point> f = Nw->Query(b);
    found.insert(found.end(), f.begin(), f.end());
  }
  if(Ne != nullptr)
  {
    vector<Point> f = Ne->Query(b);
    found.insert(found.end(), f.begin(), f.end());
  }
  if(Sw != nullptr)
  {
    vector<Point> f = Sw->Query(b);
    found.insert(found.end(), f.begin(), f.end());
  }
  if(Se != nullptr)
  {
    vector<Point> f = Se->Query(b);
    found.insert(found.end(), f.begin(), f.end());
  }
  return found;
}
}