Simulant  21.12-194
A portable game engine for Windows, OSX, Linux, Dreamcast, and PSP
loose_quadtree.h
1 #pragma once
2 
3 #include <cstdint>
4 #include "../../math/aabb.h"
5 #include "../../frustum.h"
6 #include "../../generic/check_signature.h"
7 
8 namespace smlt {
9 
10 static constexpr int64_t ipow(int64_t base, int exp, int64_t result = 1) {
11  return exp < 1 ? result : ipow(base*base, exp/2, (exp % 2) ? result*base : result);
12 }
13 
14 /*
15  * Fast loose-Quadtree implementation with O(1) insertions, built on a linear
16  * array for good cache locality.
17  *
18  * TreeData is a structure which contains data needed across the whole tree,
19  * this may usually contain the vertex data for the mesh being inserted.
20  *
21  * NodeData is a structure for storing data on the individual nodes, this is usually
22  * index data for the mesh.
23  */
24 
25 typedef uint8_t Level;
26 typedef uint16_t GridCoord;
27 
28 template<typename TreeData, typename NodeData>
29 class Quadtree {
30 public:
31  struct Node {
32  Level level = 0;
33  GridCoord grid[2] = {0, 0};
34  NodeData* data = nullptr;
35  uint32_t child_indexes[4];
36 
37  Vec3 centre;
38  float size;
39  };
40 
41  using TraverseCallback = void(Quadtree::Node*);
42 
43  typedef TreeData tree_data_type;
44  typedef NodeData node_data_type;
45 
46  Quadtree(const AABB& bounds, uint8_t max_level_count=4, std::shared_ptr<TreeData> tree_data=std::shared_ptr<TreeData>()):
47  tree_data_(tree_data),
48  root_width_(bounds.max_dimension()),
49  bounds_(bounds),
50  centre_(bounds_.centre()) {
51 
52  /* Make sure the bounds are square */
53  float maxd = root_width_;
54  auto halfd = maxd / 2.0f;
55 
56  auto half = Vec3(halfd, 0, halfd);
57  bounds_.set_min(centre_ - half);
58  bounds_.set_max(centre_ + half);
59 
60  /* Grow the tree to whatever size we passed in */
61  grow(max_level_count);
62  }
63 
64  virtual ~Quadtree() {
65  /* Make sure we delete the node data */
66  for(auto& node: nodes_) {
67  delete node.data;
68  node.data = nullptr;
69  }
70  }
71 
72  bool is_leaf(Node& node) const {
73  return node.level == (levels_ - 1);
74  }
75 
76  Quadtree::Node* find_destination_for_sphere(const Vec3& centre, float radius) {
77  auto diameter = radius * 2.0f;
78  auto level_and_node_width = level_for_width(diameter);
79 
80  /* Calculate the cell index to insert the sphere */
81  auto half_width = root_width_ * 0.5f;
82 
83  assert(centre.x <= bounds_.max().x && centre.x >= bounds_.min().x);
84  assert(centre.z <= bounds_.max().z && centre.z >= bounds_.min().z);
85 
86  auto x = (GridCoord) ((centre.x + half_width - centre_.x) / level_and_node_width.second);
87  auto z = (GridCoord) ((centre.z + half_width - centre_.z) / level_and_node_width.second);
88 
89  assert(x >= 0);
90  assert(z >= 0);
91 
92  auto level_width = ipow(2, level_and_node_width.first);
93  assert(x <= level_width);
94  assert(z <= level_width);
95 
96  /* This handles the case that the center was on the cusp of the cell
97  * in which case the above calculation will result in an out of range
98  * index */
99  x = std::min<GridCoord>(x, level_width - 1);
100  z = std::min<GridCoord>(z, level_width - 1);
101 
102  auto idx = calc_index(level_and_node_width.first, x, z);
103  assert(idx < nodes_.size());
104 
105  return &nodes_[idx];
106  }
107 
108  Quadtree::Node* find_destination_for_triangle(const Vec3* vertices) {
109  /*
110  * Return the node that this triangle should be
111  * inserted into (the actual insertion won't happen as that's implementation specific
112  * depending on the NodeData).
113  */
114 
115  AABB bounds(vertices, 3);
116  auto centre = bounds.centre();
117  auto radius = bounds.max_dimension() / 2.0f;
118  return find_destination_for_sphere(centre, radius);
119  }
120 
121  void traverse(std::function<void (Quadtree::Node*)> cb) {
122  if(nodes_.empty()) {
123  return;
124  }
125 
126  std::function<void (Quadtree::Node&)> visitor = [&](Quadtree::Node& node) {
127  cb(&node);
128 
129  if(!is_leaf(node)) {
130  for(auto child: node.child_indexes) {
131  assert(child < nodes_.size());
132  visitor(nodes_[child]);
133  }
134  }
135  };
136 
137  visitor(nodes_[0]);
138  }
139 
140  template<typename Callback>
141  void traverse_visible(const Frustum& frustum, const Callback& cb) {
142  check_signature<Callback, TraverseCallback>();
143 
144  if(nodes_.empty()) {
145  return;
146  }
147 
148  _visible_visitor(frustum, cb, nodes_[0]);
149  }
150 
151  AABB bounds() const { return bounds_; }
152 
153  TreeData* data() const { return tree_data_.get(); }
154 private:
155  template<typename Callback>
156  void _visible_visitor(const Frustum& frustum, const Callback& callback, Quadtree::Node& node) {
157  if(frustum.intersects_cube(node.centre, node.size * 2.0f)) {
158  callback(&node);
159 
160  if(!is_leaf(node)) {
161  for(auto child: node.child_indexes) {
162  assert(child < nodes_.size());
163  _visible_visitor(frustum, callback, nodes_[child]);
164  }
165  }
166  }
167  }
168 
169  std::pair<Level, float> level_for_width(float obj_width) {
170  /*
171  * Given the diameter of the object
172  * this returns the level the object will fit, and the node width at that level
173  *
174  * I *know* there's a non-iterative way to calculate this, so if you know, let me know!
175  */
176 
177  assert(obj_width <= root_width_);
178 
179  if(obj_width > root_width_) {
180  // Should never happen, but this is the safest thing to do if it does
181  return std::make_pair(0, root_width_);
182  }
183 
184  auto node_width = root_width_;
185  Level depth = 0;
186 
187  while(node_width >= obj_width) {
188  ++depth;
189  node_width *= 0.5f;
190 
191  if(depth == levels_) {
192  break;
193  }
194  }
195 
196  assert(depth >= 1);
197 
198  // Off-by-one
199  return std::make_pair(depth - 1, node_width * 2);
200  }
201 
202  void calc_bounds(Quadtree::Node& node) const {
203  auto grid_width = ipow(2, node.level);
204  auto cell_width = bounds_.max_dimension() / grid_width;
205 
206  Vec3 min(node.grid[0] * cell_width, 0, node.grid[1] * cell_width);
207  min += bounds_.min();
208 
209  float hw = cell_width * 0.5f;
210  node.centre = min + Vec3(hw, hw, hw);
211  node.size = cell_width;
212  }
213 
214  static uint32_t calc_base(uint8_t level) {
215  if(level == 0) return 0;
216  if(level == 1) return 1;
217  return calc_base(level - 1) + ipow(ipow(2, level - 1), 2);
218  }
219 
220  static uint32_t calc_index(Level k, GridCoord x, GridCoord z) {
221  auto level_base = calc_base(k);
222  auto level_width = ipow(2, k);
223 
224  assert(x < level_width);
225  assert(z < level_width);
226 
227  auto idx = x + level_width * z;
228  return level_base + idx;
229  }
230 
231  static void calc_child_indexes(Quadtree::Node& node) {
232  uint8_t count = 0;
233  for(uint32_t z = 0; z <= 1; ++z) {
234  for(uint32_t x = 0; x <= 1; ++x) {
235  node.child_indexes[count++] = calc_index(
236  node.level + 1,
237  2 * node.grid[0] + x,
238  2 * node.grid[1] + z
239  );
240  }
241  }
242  }
243 
244  void grow(uint8_t required_levels) {
245 
246  auto required_nodes = 0;
247  for(uint8_t i = 0; i < required_levels; ++i) {
248  auto nodes_across = ipow(2, i);
249  required_nodes += ipow(nodes_across, 2);
250  }
251 
252  levels_ = required_levels;
253  nodes_.resize(required_nodes, Quadtree::Node());
254 
255  for(auto k = 0; k < levels_; ++k) {
256  auto nodes_across = ipow(2, k);
257 
258  for(GridCoord z = 0; z < nodes_across; ++z) {
259  for(GridCoord x = 0; x < nodes_across; ++x) {
260  auto idx = calc_index(k, x, z);
261  assert(idx < nodes_.size());
262  auto& new_node = nodes_[idx];
263 
264  assert(!new_node.data);
265 
266  new_node.grid[0] = x;
267  new_node.grid[1] = z;
268  new_node.level = k;
269  new_node.data = new NodeData();
270 
271  calc_child_indexes(new_node);
272  calc_bounds(new_node);
273  }
274  }
275  }
276  }
277 
278  std::shared_ptr<TreeData> tree_data_;
279 
280  float root_width_;
281  AABB bounds_;
282  Vec3 centre_;
283 
284  Level levels_ = 0;
285  std::vector<Quadtree::Node> nodes_;
286 
287  friend class LooseQuadtreeTests;
288 };
289 
290 }
smlt::Vec3
Definition: vec3.h:23
half_float::half
Definition: half.hpp:923
smlt::Quadtree
Definition: loose_quadtree.h:29
smlt
Definition: animation.cpp:25
half_float::detail::exp
expr exp(half arg)
Definition: half.hpp:2160
smlt::Quadtree::Node
Definition: loose_quadtree.h:31
smlt::AABB
Definition: aabb.h:22