fix for carve memory leak, update carve to hg bf36d92ff093
[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       struct data_aabb_t {
53         aabb_t bbox;
54         data_t data;
55
56         data_aabb_t() { }
57         data_aabb_t(const data_t &_data) : bbox(aabb_calc_t()(_data)), data(_data) {
58         }
59
60         aabb_t getAABB() const { return bbox; }
61
62         struct cmp {
63           size_t dim;
64           cmp(size_t _dim) : dim(_dim) { }
65           bool operator()(const data_aabb_t &a, const data_aabb_t &b) {
66             return a.bbox.pos.v[dim] < b.bbox.pos.v[dim];
67           }
68         };
69       };
70
71       // Fill an rtree node with a set of (data, aabb) pairs.
72       template<typename iter_t>
73       void _fill(iter_t begin, iter_t end, data_aabb_t) {
74         data.reserve(std::distance(begin, end));
75         for (iter_t i = begin; i != end; ++i) {
76           data.push_back((*i).data);
77         }
78         bbox.fit(begin, end);
79       }
80
81       // Fill an rtree node with a set of data.
82       template<typename iter_t>
83       void _fill(iter_t begin, iter_t end, data_t) {
84         data.reserve(std::distance(begin, end));
85         std::copy(begin, end, std::back_inserter(data));
86         bbox.fit(begin, end, aabb_calc_t());
87       }
88
89       // Fill an rtree node with a set of child nodes.
90       template<typename iter_t>
91       void _fill(iter_t begin, iter_t end, node_t *) {
92         iter_t i = begin;
93         node_t *curr = child = *i;
94         while (++i != end) {
95           curr->sibling = *i;
96           curr = curr->sibling;
97         }
98         bbox.fit(begin, end);
99       }
100
101       // Search the rtree for objects that intersect obj (generally an aabb).
102       // The aabb class must provide a method intersects(obj_t).
103       template<typename obj_t, typename out_iter_t>
104       void search(const obj_t &obj, out_iter_t out) const {
105         if (!bbox.intersects(obj)) return;
106         if (child) {
107           for (node_t *node = child; node; node = node->sibling) {
108             node->search(obj, out);
109           }
110         } else {
111           std::copy(data.begin(), data.end(), out);
112         }
113       }
114
115       // update the bounding box extents of nodes that intersect obj (generally an aabb).
116       // The aabb class must provide a method intersects(obj_t).
117       template<typename obj_t>
118       void updateExtents(const obj_t &obj) {
119         if (!bbox.intersects(obj)) return;
120
121         if (child) {
122           node_t *node = child;
123           node->updateExtents(obj);
124           bbox = node->bbox;
125           for (node = node->sibling; node; node = node->sibling) {
126             node->updateExtents(obj);
127             bbox.unionAABB(node->bbox);
128           }
129         } else {
130           bbox.fit(data.begin(), data.end());
131         }
132       }
133
134       // update the bounding box extents of nodes that intersect obj (generally an aabb).
135       // The aabb class must provide a method intersects(obj_t).
136       bool remove(const data_t &val, const aabb_t &val_aabb) {
137         if (!bbox.intersects(val_aabb)) return false;
138
139         if (child) {
140           node_t *node = child;
141           node->remove(val, val_aabb);
142           bbox = node->bbox;
143           bool removed = false;
144           for (node = node->sibling; node; node = node->sibling) {
145             if (!removed) removed = node->remove(val, val_aabb);
146             bbox.unionAABB(node->bbox);
147           }
148           return removed;
149         } else {
150           typename std::vector<data_t>::iterator i = std::remove(data.begin(), data.end(), val);
151           if (i == data.end()) {
152             return false;
153           }
154           data.erase(i, data.end());
155           bbox.fit(data.begin(), data.end());
156           return true;
157         }
158       }
159
160       template<typename iter_t>
161       RTreeNode(iter_t begin, iter_t end) : bbox(), child(NULL), sibling(NULL), data() {
162         _fill(begin, end, typename std::iterator_traits<iter_t>::value_type());
163       }
164
165       ~RTreeNode() {
166         if (child) {
167           RTreeNode *next = child;
168           while (next) {
169             RTreeNode *curr = next;
170             next = next->sibling;
171             delete curr;
172           }
173         }
174       }
175
176
177
178       // functor for ordering nodes by increasing aabb midpoint, along a specified axis.
179       struct aabb_cmp_mid {
180         size_t dim;
181         aabb_cmp_mid(size_t _dim) : dim(_dim) { }
182
183         bool operator()(const node_t *a, const node_t *b) {
184           return a->bbox.mid(dim) < b->bbox.mid(dim);
185         }
186         bool operator()(const data_aabb_t &a, const data_aabb_t &b) {
187           return a.bbox.mid(dim) < b.bbox.mid(dim);
188         }
189       };
190
191       // functor for ordering nodes by increasing aabb minimum, along a specified axis.
192       struct aabb_cmp_min {
193         size_t dim;
194         aabb_cmp_min(size_t _dim) : dim(_dim) { }
195
196         bool operator()(const node_t *a, const node_t *b) {
197           return a->bbox.min(dim) < b->bbox.min(dim);
198         }
199         bool operator()(const data_aabb_t &a, const data_aabb_t &b) {
200           return a.bbox.min(dim) < b.bbox.min(dim);
201         }
202       };
203
204       // functor for ordering nodes by increasing aabb maximum, along a specified axis.
205       struct aabb_cmp_max {
206         size_t dim;
207         aabb_cmp_max(size_t _dim) : dim(_dim) { }
208
209         bool operator()(const node_t *a, const node_t *b) {
210           return a->bbox.max(dim) < b->bbox.max(dim);
211         }
212         bool operator()(const data_aabb_t &a, const data_aabb_t &b) {
213           return a.bbox.max(dim) < b.bbox.max(dim);
214         }
215       };
216
217       // facade for projecting node bounding box onto an axis.
218       struct aabb_extent {
219         size_t dim;
220         aabb_extent(size_t _dim) : dim(_dim) { }
221
222         double min(const node_t *a) { return a->bbox.pos.v[dim] - a->bbox.extent.v[dim]; }
223         double max(const node_t *a) { return a->bbox.pos.v[dim] + a->bbox.extent.v[dim]; }
224         double len(const node_t *a) { return 2.0 * a->bbox.extent.v[dim]; }
225         double min(const data_aabb_t &a) { return a.bbox.pos.v[dim] - a.bbox.extent.v[dim]; }
226         double max(const data_aabb_t &a) { return a.bbox.pos.v[dim] + a.bbox.extent.v[dim]; }
227         double len(const data_aabb_t &a) { return 2.0 * a.bbox.extent.v[dim]; }
228       };
229
230       template<typename iter_t>
231       static void makeNodes(const iter_t begin,
232                             const iter_t end,
233                             size_t dim_num,
234                             uint32_t dim_mask,
235                             size_t child_size,
236                             std::vector<node_t *> &out) {
237         const size_t N = std::distance(begin, end);
238
239         size_t dim = ndim;
240         double r_best = N+1;
241
242         // find the sparsest remaining dimension to partition by.
243         for (size_t i = 0; i < ndim; ++i) {
244           if (dim_mask & (1U << i)) continue;
245           aabb_extent extent(i);
246           double dmin, dmax, dsum;
247
248           dmin = extent.min(*begin);
249           dmax = extent.max(*begin);
250           dsum = 0.0;
251           for (iter_t j = begin; j != end; ++j) {
252             dmin = std::min(dmin, extent.min(*j));
253             dmax = std::max(dmax, extent.max(*j));
254             dsum += extent.len(*j);
255           }
256           double r = dsum ? dsum / (dmax - dmin) : 0.0;
257           if (r_best > r) {
258             dim = i;
259             r_best = r;
260           }
261         }
262
263         CARVE_ASSERT(dim < ndim);
264
265         // dim = dim_num;
266
267         const size_t P = (N + child_size - 1) / child_size;
268         const size_t n_parts = (size_t)std::ceil(std::pow((double)P, 1.0 / (ndim - dim_num)));
269
270         std::sort(begin, end, aabb_cmp_mid(dim));
271
272         if (dim_num == ndim - 1 || n_parts == 1) {
273           for (size_t i = 0, s = 0, e = 0; i < P; ++i, s = e) {
274             e = N * (i+1) / P;
275             CARVE_ASSERT(e - s <= child_size);
276             out.push_back(new node_t(begin + s, begin + e));
277           }
278         } else {
279           for (size_t i = 0, s = 0, e = 0; i < n_parts; ++i, s = e) {
280             e = N * (i+1) / n_parts;
281             makeNodes(begin + s, begin + e, dim_num + 1, dim_mask | (1U << dim), child_size, out);
282           }
283         }
284       }
285
286       static node_t *construct_STR(std::vector<data_aabb_t> &data, size_t leaf_size, size_t internal_size) {
287         std::vector<node_t *> out;
288         makeNodes(data.begin(), data.end(), 0, 0, leaf_size, out);
289
290         while (out.size() > 1) {
291           std::vector<node_t *> next;
292           makeNodes(out.begin(), out.end(), 0, 0, internal_size, next);
293           std::swap(out, next);
294         }
295
296         CARVE_ASSERT(out.size() == 1);
297         return out[0];
298       }
299
300       template<typename iter_t>
301       static node_t *construct_STR(const iter_t &begin,
302                                    const iter_t &end,
303                                    size_t leaf_size,
304                                    size_t internal_size) {
305         std::vector<data_aabb_t> data;
306         data.reserve(std::distance(begin, end));
307         for (iter_t i = begin; i != end; ++i) {
308           data.push_back(*i);
309         }
310         return construct_STR(data, leaf_size, internal_size);
311       }
312
313
314       template<typename iter_t>
315       static node_t *construct_STR(const iter_t &begin1,
316                                    const iter_t &end1,
317                                    const iter_t &begin2,
318                                    const iter_t &end2,
319                                    size_t leaf_size,
320                                    size_t internal_size) {
321         std::vector<data_aabb_t> data;
322         data.reserve(std::distance(begin1, end1) + std::distance(begin2, end2));
323         for (iter_t i = begin1; i != end1; ++i) {
324           data.push_back(*i);
325         }
326         for (iter_t i = begin2; i != end2; ++i) {
327           data.push_back(*i);
328         }
329         return construct_STR(data, leaf_size, internal_size);
330       }
331
332
333       struct partition_info {
334         double score;
335         size_t partition_pos;
336
337         partition_info() : score(std::numeric_limits<double>::max()), partition_pos(0) {
338         }
339         partition_info(double _score, size_t _partition_pos) :
340           score(_score),
341           partition_pos(_partition_pos) {
342         }
343       };
344
345       static partition_info findPartition(typename std::vector<data_aabb_t>::iterator base,
346                                           std::vector<size_t>::iterator begin,
347                                           std::vector<size_t>::iterator end,
348                                           size_t part_size) {
349         partition_info best(std::numeric_limits<double>::max(), 0);
350         const size_t N = std::distance(begin, end);
351
352         std::vector<double> rhs_vol(N, 0.0);
353
354         aabb_t rhs = base[begin[N-1]].aabb;
355         rhs_vol[N-1] = rhs.volume();
356         for (size_t i = N - 1; i > 0; ) {
357           rhs.unionAABB(base[begin[--i]].aabb);
358           rhs_vol[i] = rhs.volume();
359         }
360
361         aabb_t lhs = base[begin[0]].aabb;
362         for (size_t i = 1; i < N; ++i) {
363           lhs.unionAABB(base[begin[i]].aabb);
364           if (i % part_size == 0 || (N - i) % part_size == 0) {
365             partition_info curr(lhs.volume() + rhs_vol[i], i);
366             if (best.score > curr.score) best = curr;
367           }
368         }
369         return best;
370       }
371
372       static void partition(typename std::vector<data_aabb_t>::iterator base,
373                             std::vector<size_t>::iterator begin,
374                             std::vector<size_t>::iterator end,
375                             size_t part_size,
376                             std::vector<size_t> &part_num,
377                             size_t &part_next) {
378         const size_t N = std::distance(begin, end);
379
380         partition_info best;
381         partition_info curr;
382         size_t part_curr = part_num[*begin];
383
384         std::vector<size_t> tmp(begin, end);
385
386         for (size_t dim = 0; dim < ndim; ++dim) {
387           std::sort(tmp.begin(), tmp.end(), make_index_sort(base, aabb_cmp_min(dim)));
388           curr = findPartition(base, tmp.begin(), tmp.end(), part_size);
389           if (best.score > curr.score) {
390             best = curr;
391             std::copy(tmp.begin(), tmp.end(), begin);
392           }
393
394           std::sort(tmp.begin(), tmp.end(), make_index_sort(base, aabb_cmp_mid(dim)));
395           curr = findPartition(base, tmp.begin(), tmp.end(), part_size);
396           if (best.score > curr.score) {
397             best = curr;
398             std::copy(tmp.begin(), tmp.end(), begin);
399           }
400
401           std::sort(tmp.begin(), tmp.end(), make_index_sort(base, aabb_cmp_max(dim)));
402           curr = findPartition(base, tmp.begin(), tmp.end(), part_size);
403           if (best.score > curr.score) {
404             best = curr;
405             std::copy(tmp.begin(), tmp.end(), begin);
406           }
407         }
408
409         for (size_t j = 0; j < best.partition_pos; ++j) part_num[begin[j]] = part_curr;
410         for (size_t j = best.partition_pos; j < N; ++j) part_num[begin[j]] = part_next;
411         ++part_next;
412
413         if (best.partition_pos > part_size) {
414           partition(base, begin, begin + best.partition_pos, part_size, part_num, part_next);
415         }
416         if (N - best.partition_pos > part_size) {
417           partition(base, begin + best.partition_pos, end, part_size, part_num, part_next);
418         }
419       }
420
421       static size_t makePartitions(typename std::vector<data_aabb_t>::iterator begin,
422                                    typename std::vector<data_aabb_t>::iterator end,
423                                    size_t part_size,
424                                    std::vector<size_t> &part_num) {
425         const size_t N = std::distance(begin, end);
426         std::vector<size_t> idx;
427         idx.reserve(N);
428         for (size_t i = 0; i < N; ++i) { idx.push_back(i); }
429         size_t part_next = 1;
430
431         partition(begin, idx.begin(), idx.end(), part_size, part_num, part_next);
432         return part_next;
433       }
434
435       static node_t *construct_TGS(typename std::vector<data_aabb_t>::iterator begin,
436                                    typename std::vector<data_aabb_t>::iterator end,
437                                    size_t leaf_size,
438                                    size_t internal_size) {
439         size_t N = std::distance(begin, end);
440
441         if (N <= leaf_size) {
442           return new node_t(begin, end);
443         } else {
444           size_t P = (N + internal_size - 1) / internal_size;
445           std::vector<size_t> part_num(N, 0);
446           P = makePartitions(begin, end, P, part_num);
447
448           size_t S = 0, E = 0;
449           std::vector<node_t *> children;
450           for (size_t i = 0; i < P; ++i) {
451             size_t j = S, k = N;
452             while (true) {
453               while (true) {
454                 if (j == k) goto done;
455                 else if (part_num[j] == i) ++j;
456                 else break;
457               }
458               --k;
459               while (true) {
460                 if (j == k) goto done;
461                 else if (part_num[k] != i) --k;
462                 else break;
463               }
464               std::swap(*(begin+j), *(begin+k));
465               std::swap(part_num[j], part_num[k]);
466               ++j;
467             }
468           done:
469             E = j;
470             children.push_back(construct_TGS(begin + S, begin + E, leaf_size, internal_size));
471             S = E;
472           }
473           return new node_t(children.begin(), children.end());
474         }
475       }
476
477       template<typename iter_t>
478       static node_t *construct_TGS(const iter_t &begin,
479                                    const iter_t &end,
480                                    size_t leaf_size,
481                                    size_t internal_size) {
482         std::vector<data_aabb_t> data;
483         data.reserve(std::distance(begin, end));
484         for (iter_t i = begin; i != end; ++i) {
485           data.push_back(*i);
486         }
487         return construct_TGS(data.begin(), data.end(), leaf_size, internal_size);
488       }
489
490       template<typename iter_t>
491       static node_t *construct_TGS(const iter_t &begin1,
492                                    const iter_t &end1,
493                                    const iter_t &begin2,
494                                    const iter_t &end2,
495                                    size_t leaf_size,
496                                    size_t internal_size) {
497         std::vector<data_aabb_t> data;
498         data.reserve(std::distance(begin1, end1) + std::distance(begin2, end2));
499         for (iter_t i = begin1; i != end1; ++i) {
500           data.push_back(*i);
501         }
502         for (iter_t i = begin2; i != end2; ++i) {
503           data.push_back(*i);
504         }
505         return construct_TGS(data.begin(), data.end(), leaf_size, internal_size);
506       }
507     };
508
509   }
510 }