Merging r43130 through r43500 from trunk into soc-2011-tomato
[blender.git] / extern / carve / include / carve / rtree.hpp
1 // Begin License:
2 // Copyright (C) 2006-2011 Tobias Sargeant (tobias.sargeant@gmail.com).
3 // All rights reserved.
4 //
5 // This file is part of the Carve CSG Library (http://carve-csg.com/)
6 //
7 // This file may be used under the terms of the GNU General Public
8 // License version 2.0 as published by the Free Software Foundation
9 // and appearing in the file LICENSE.GPL2 included in the packaging of
10 // this file.
11 //
12 // This file is provided "AS IS" with NO WARRANTY OF ANY KIND,
13 // INCLUDING THE WARRANTIES OF DESIGN, MERCHANTABILITY AND FITNESS FOR
14 // A PARTICULAR PURPOSE.
15 // End:
16
17
18 #pragma once
19
20 #include <carve/carve.hpp>
21
22 #include <carve/geom.hpp>
23 #include <carve/aabb.hpp>
24
25 #include <iostream>
26
27 #include <cmath>
28 #include <limits>
29
30 #if defined(HAVE_STDINT_H)
31 # include <stdint.h>
32 #endif
33
34 namespace carve {
35   namespace geom {
36
37     template<unsigned ndim,
38              typename data_t,
39              typename aabb_calc_t = carve::geom::get_aabb<ndim, data_t> >
40     struct RTreeNode {
41       typedef aabb<ndim> aabb_t;
42       typedef vector<ndim> vector_t;
43       typedef RTreeNode<ndim, data_t, aabb_calc_t> node_t;
44
45       aabb_t bbox;
46       node_t *child;
47       node_t *sibling;
48       std::vector<data_t> data;
49
50       aabb_t getAABB() const { return bbox; }
51
52
53
54       struct data_aabb_t {
55         aabb_t bbox;
56         data_t data;
57
58         data_aabb_t() { }
59         data_aabb_t(const data_t &_data) : bbox(aabb_calc_t()(_data)), data(_data) {
60         }
61
62         aabb_t getAABB() const { return bbox; }
63
64         struct cmp {
65           size_t dim;
66           cmp(size_t _dim) : dim(_dim) { }
67           bool operator()(const data_aabb_t &a, const data_aabb_t &b) {
68             return a.bbox.pos.v[dim] < b.bbox.pos.v[dim];
69           }
70         };
71       };
72
73       // Fill an rtree node with a set of (data, aabb) pairs.
74       template<typename iter_t>
75       void _fill(iter_t begin, iter_t end, data_aabb_t) {
76         data.reserve(std::distance(begin, end));
77         for (iter_t i = begin; i != end; ++i) {
78           data.push_back((*i).data);
79         }
80         bbox.fit(begin, end);
81       }
82
83       // Fill an rtree node with a set of data.
84       template<typename iter_t>
85       void _fill(iter_t begin, iter_t end, data_t) {
86         data.reserve(std::distance(begin, end));
87         std::copy(begin, end, std::back_inserter(data));
88         bbox.fit(begin, end, aabb_calc_t());
89       }
90
91       // Fill an rtree node with a set of child nodes.
92       template<typename iter_t>
93       void _fill(iter_t begin, iter_t end, node_t *) {
94         iter_t i = begin;
95         node_t *curr = child = *i;
96         while (++i != end) {
97           curr->sibling = *i;
98           curr = curr->sibling;
99         }
100         bbox.fit(begin, end);
101       }
102
103       // Search the rtree for objects that intersect obj (generally an aabb).
104       // The aabb class must provide a method intersects(obj_t).
105       template<typename obj_t, typename out_iter_t>
106       void search(const obj_t &obj, out_iter_t out) const {
107         if (!bbox.intersects(obj)) return;
108         if (child) {
109           for (node_t *node = child; node; node = node->sibling) {
110             node->search(obj, out);
111           }
112         } else {
113           std::copy(data.begin(), data.end(), out);
114         }
115       }
116
117       // update the bounding box extents of nodes that intersect obj (generally an aabb).
118       // The aabb class must provide a method intersects(obj_t).
119       template<typename obj_t>
120       void updateExtents(const obj_t &obj) {
121         if (!bbox.intersects(obj)) return;
122
123         if (child) {
124           node_t *node = child;
125           node->updateExtents(obj);
126           bbox = node->bbox;
127           for (node = node->sibling; node; node = node->sibling) {
128             node->updateExtents(obj);
129             bbox.unionAABB(node->bbox);
130           }
131         } else {
132           bbox.fit(data.begin(), data.end());
133         }
134       }
135
136       // update the bounding box extents of nodes that intersect obj (generally an aabb).
137       // The aabb class must provide a method intersects(obj_t).
138       bool remove(const data_t &val, const aabb_t &val_aabb) {
139         if (!bbox.intersects(val_aabb)) return false;
140
141         if (child) {
142           node_t *node = child;
143           node->remove(val, val_aabb);
144           bbox = node->bbox;
145           bool removed = false;
146           for (node = node->sibling; node; node = node->sibling) {
147             if (!removed) removed = node->remove(val, val_aabb);
148             bbox.unionAABB(node->bbox);
149           }
150           return removed;
151         } else {
152           typename std::vector<data_t>::iterator i = std::remove(data.begin(), data.end(), val);
153           if (i == data.end()) {
154             return false;
155           }
156           data.erase(i, data.end());
157           bbox.fit(data.begin(), data.end());
158           return true;
159         }
160       }
161
162       template<typename iter_t>
163       RTreeNode(iter_t begin, iter_t end) : bbox(), child(NULL), sibling(NULL), data() {
164         _fill(begin, end, typename std::iterator_traits<iter_t>::value_type());
165       }
166
167
168
169       // functor for ordering nodes by increasing aabb midpoint, along a specified axis.
170       struct aabb_cmp_mid {
171         size_t dim;
172         aabb_cmp_mid(size_t _dim) : dim(_dim) { }
173
174         bool operator()(const node_t *a, const node_t *b) {
175           return a->bbox.mid(dim) < b->bbox.mid(dim);
176         }
177         bool operator()(const data_aabb_t &a, const data_aabb_t &b) {
178           return a.bbox.mid(dim) < b.bbox.mid(dim);
179         }
180       };
181
182       // functor for ordering nodes by increasing aabb minimum, along a specified axis.
183       struct aabb_cmp_min {
184         size_t dim;
185         aabb_cmp_min(size_t _dim) : dim(_dim) { }
186
187         bool operator()(const node_t *a, const node_t *b) {
188           return a->bbox.min(dim) < b->bbox.min(dim);
189         }
190         bool operator()(const data_aabb_t &a, const data_aabb_t &b) {
191           return a.bbox.min(dim) < b.bbox.min(dim);
192         }
193       };
194
195       // functor for ordering nodes by increasing aabb maximum, along a specified axis.
196       struct aabb_cmp_max {
197         size_t dim;
198         aabb_cmp_max(size_t _dim) : dim(_dim) { }
199
200         bool operator()(const node_t *a, const node_t *b) {
201           return a->bbox.max(dim) < b->bbox.max(dim);
202         }
203         bool operator()(const data_aabb_t &a, const data_aabb_t &b) {
204           return a.bbox.max(dim) < b.bbox.max(dim);
205         }
206       };
207
208       // facade for projecting node bounding box onto an axis.
209       struct aabb_extent {
210         size_t dim;
211         aabb_extent(size_t _dim) : dim(_dim) { }
212
213         double min(const node_t *a) { return a->bbox.pos.v[dim] - a->bbox.extent.v[dim]; }
214         double max(const node_t *a) { return a->bbox.pos.v[dim] + a->bbox.extent.v[dim]; }
215         double len(const node_t *a) { return 2.0 * a->bbox.extent.v[dim]; }
216         double min(const data_aabb_t &a) { return a.bbox.pos.v[dim] - a.bbox.extent.v[dim]; }
217         double max(const data_aabb_t &a) { return a.bbox.pos.v[dim] + a.bbox.extent.v[dim]; }
218         double len(const data_aabb_t &a) { return 2.0 * a.bbox.extent.v[dim]; }
219       };
220
221       template<typename iter_t>
222       static void makeNodes(const iter_t begin,
223                             const iter_t end,
224                             size_t dim_num,
225                             uint32_t dim_mask,
226                             size_t child_size,
227                             std::vector<node_t *> &out) {
228         const size_t N = std::distance(begin, end);
229
230         size_t dim = ndim;
231         double r_best = N+1;
232
233         // find the sparsest remaining dimension to partition by.
234         for (size_t i = 0; i < ndim; ++i) {
235           if (dim_mask & (1U << i)) continue;
236           aabb_extent extent(i);
237           double dmin, dmax, dsum;
238
239           dmin = extent.min(*begin);
240           dmax = extent.max(*begin);
241           dsum = 0.0;
242           for (iter_t j = begin; j != end; ++j) {
243             dmin = std::min(dmin, extent.min(*j));
244             dmax = std::max(dmax, extent.max(*j));
245             dsum += extent.len(*j);
246           }
247           double r = dsum ? dsum / (dmax - dmin) : 0.0;
248           if (r_best > r) {
249             dim = i;
250             r_best = r;
251           }
252         }
253
254         CARVE_ASSERT(dim < ndim);
255
256         // dim = dim_num;
257
258         const size_t P = (N + child_size - 1) / child_size;
259         const size_t n_parts = (size_t)std::ceil(std::pow((double)P, 1.0 / (ndim - dim_num)));
260
261         std::sort(begin, end, aabb_cmp_mid(dim));
262
263         if (dim_num == ndim - 1 || n_parts == 1) {
264           for (size_t i = 0, s = 0, e = 0; i < P; ++i, s = e) {
265             e = N * (i+1) / P;
266             CARVE_ASSERT(e - s <= child_size);
267             out.push_back(new node_t(begin + s, begin + e));
268           }
269         } else {
270           for (size_t i = 0, s = 0, e = 0; i < n_parts; ++i, s = e) {
271             e = N * (i+1) / n_parts;
272             makeNodes(begin + s, begin + e, dim_num + 1, dim_mask | (1U << dim), child_size, out);
273           }
274         }
275       }
276
277       static node_t *construct_STR(std::vector<data_aabb_t> &data, size_t leaf_size, size_t internal_size) {
278         std::vector<node_t *> out;
279         makeNodes(data.begin(), data.end(), 0, 0, leaf_size, out);
280
281         while (out.size() > 1) {
282           std::vector<node_t *> next;
283           makeNodes(out.begin(), out.end(), 0, 0, internal_size, next);
284           std::swap(out, next);
285         }
286
287         CARVE_ASSERT(out.size() == 1);
288         return out[0];
289       }
290
291       template<typename iter_t>
292       static node_t *construct_STR(const iter_t &begin,
293                                    const iter_t &end,
294                                    size_t leaf_size,
295                                    size_t internal_size) {
296         std::vector<data_aabb_t> data;
297         data.reserve(std::distance(begin, end));
298         for (iter_t i = begin; i != end; ++i) {
299           data.push_back(*i);
300         }
301         return construct_STR(data, leaf_size, internal_size);
302       }
303
304
305       template<typename iter_t>
306       static node_t *construct_STR(const iter_t &begin1,
307                                    const iter_t &end1,
308                                    const iter_t &begin2,
309                                    const iter_t &end2,
310                                    size_t leaf_size,
311                                    size_t internal_size) {
312         std::vector<data_aabb_t> data;
313         data.reserve(std::distance(begin1, end1) + std::distance(begin2, end2));
314         for (iter_t i = begin1; i != end1; ++i) {
315           data.push_back(*i);
316         }
317         for (iter_t i = begin2; i != end2; ++i) {
318           data.push_back(*i);
319         }
320         return construct_STR(data, leaf_size, internal_size);
321       }
322
323
324       struct partition_info {
325         double score;
326         size_t partition_pos;
327
328         partition_info() : score(std::numeric_limits<double>::max()), partition_pos(0) {
329         }
330         partition_info(double _score, size_t _partition_pos) :
331           score(_score),
332           partition_pos(_partition_pos) {
333         }
334       };
335
336       static partition_info findPartition(typename std::vector<data_aabb_t>::iterator base,
337                                           std::vector<size_t>::iterator begin,
338                                           std::vector<size_t>::iterator end,
339                                           size_t part_size) {
340         partition_info best(std::numeric_limits<double>::max(), 0);
341         const size_t N = std::distance(begin, end);
342
343         std::vector<double> rhs_vol(N, 0.0);
344
345         aabb_t rhs = base[begin[N-1]].aabb;
346         rhs_vol[N-1] = rhs.volume();
347         for (size_t i = N - 1; i > 0; ) {
348           rhs.unionAABB(base[begin[--i]].aabb);
349           rhs_vol[i] = rhs.volume();
350         }
351
352         aabb_t lhs = base[begin[0]].aabb;
353         for (size_t i = 1; i < N; ++i) {
354           lhs.unionAABB(base[begin[i]].aabb);
355           if (i % part_size == 0 || (N - i) % part_size == 0) {
356             partition_info curr(lhs.volume() + rhs_vol[i], i);
357             if (best.score > curr.score) best = curr;
358           }
359         }
360         return best;
361       }
362
363       static void partition(typename std::vector<data_aabb_t>::iterator base,
364                             std::vector<size_t>::iterator begin,
365                             std::vector<size_t>::iterator end,
366                             size_t part_size,
367                             std::vector<size_t> &part_num,
368                             size_t &part_next) {
369         const size_t N = std::distance(begin, end);
370
371         partition_info best;
372         partition_info curr;
373         size_t part_curr = part_num[*begin];
374
375         std::vector<size_t> tmp(begin, end);
376
377         for (size_t dim = 0; dim < ndim; ++dim) {
378           std::sort(tmp.begin(), tmp.end(), make_index_sort(base, aabb_cmp_min(dim)));
379           curr = findPartition(base, tmp.begin(), tmp.end(), part_size);
380           if (best.score > curr.score) {
381             best = curr;
382             std::copy(tmp.begin(), tmp.end(), begin);
383           }
384
385           std::sort(tmp.begin(), tmp.end(), make_index_sort(base, aabb_cmp_mid(dim)));
386           curr = findPartition(base, tmp.begin(), tmp.end(), part_size);
387           if (best.score > curr.score) {
388             best = curr;
389             std::copy(tmp.begin(), tmp.end(), begin);
390           }
391
392           std::sort(tmp.begin(), tmp.end(), make_index_sort(base, aabb_cmp_max(dim)));
393           curr = findPartition(base, tmp.begin(), tmp.end(), part_size);
394           if (best.score > curr.score) {
395             best = curr;
396             std::copy(tmp.begin(), tmp.end(), begin);
397           }
398         }
399
400         for (size_t j = 0; j < best.partition_pos; ++j) part_num[begin[j]] = part_curr;
401         for (size_t j = best.partition_pos; j < N; ++j) part_num[begin[j]] = part_next;
402         ++part_next;
403
404         if (best.partition_pos > part_size) {
405           partition(base, begin, begin + best.partition_pos, part_size, part_num, part_next);
406         }
407         if (N - best.partition_pos > part_size) {
408           partition(base, begin + best.partition_pos, end, part_size, part_num, part_next);
409         }
410       }
411
412       static size_t makePartitions(typename std::vector<data_aabb_t>::iterator begin,
413                                    typename std::vector<data_aabb_t>::iterator end,
414                                    size_t part_size,
415                                    std::vector<size_t> &part_num) {
416         const size_t N = std::distance(begin, end);
417         std::vector<size_t> idx;
418         idx.reserve(N);
419         for (size_t i = 0; i < N; ++i) { idx.push_back(i); }
420         size_t part_next = 1;
421
422         partition(begin, idx.begin(), idx.end(), part_size, part_num, part_next);
423         return part_next;
424       }
425
426       static node_t *construct_TGS(typename std::vector<data_aabb_t>::iterator begin,
427                                    typename std::vector<data_aabb_t>::iterator end,
428                                    size_t leaf_size,
429                                    size_t internal_size) {
430         size_t N = std::distance(begin, end);
431
432         if (N <= leaf_size) {
433           return new node_t(begin, end);
434         } else {
435           size_t P = (N + internal_size - 1) / internal_size;
436           std::vector<size_t> part_num(N, 0);
437           P = makePartitions(begin, end, P, part_num);
438
439           size_t S = 0, E = 0;
440           std::vector<node_t *> children;
441           for (size_t i = 0; i < P; ++i) {
442             size_t j = S, k = N;
443             while (true) {
444               while (true) {
445                 if (j == k) goto done;
446                 else if (part_num[j] == i) ++j;
447                 else break;
448               }
449               --k;
450               while (true) {
451                 if (j == k) goto done;
452                 else if (part_num[k] != i) --k;
453                 else break;
454               }
455               std::swap(*(begin+j), *(begin+k));
456               std::swap(part_num[j], part_num[k]);
457               ++j;
458             }
459           done:
460             E = j;
461             children.push_back(construct_TGS(begin + S, begin + E, leaf_size, internal_size));
462             S = E;
463           }
464           return new node_t(children.begin(), children.end());
465         }
466       }
467
468       template<typename iter_t>
469       static node_t *construct_TGS(const iter_t &begin,
470                                    const iter_t &end,
471                                    size_t leaf_size,
472                                    size_t internal_size) {
473         std::vector<data_aabb_t> data;
474         data.reserve(std::distance(begin, end));
475         for (iter_t i = begin; i != end; ++i) {
476           data.push_back(*i);
477         }
478         return construct_TGS(data.begin(), data.end(), leaf_size, internal_size);
479       }
480
481       template<typename iter_t>
482       static node_t *construct_TGS(const iter_t &begin1,
483                                    const iter_t &end1,
484                                    const iter_t &begin2,
485                                    const iter_t &end2,
486                                    size_t leaf_size,
487                                    size_t internal_size) {
488         std::vector<data_aabb_t> data;
489         data.reserve(std::distance(begin1, end1) + std::distance(begin2, end2));
490         for (iter_t i = begin1; i != end1; ++i) {
491           data.push_back(*i);
492         }
493         for (iter_t i = begin2; i != end2; ++i) {
494           data.push_back(*i);
495         }
496         return construct_TGS(data.begin(), data.end(), leaf_size, internal_size);
497       }
498     };
499
500   }
501 }