Bundle latest version of Carve library which shall resolve compilation issues with...
[blender.git] / extern / carve / include / carve / kd_node.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 #include <carve/geom.hpp>
22 #include <carve/aabb.hpp>
23
24 #include <queue>
25 #include <list>
26 #include <limits>
27
28 namespace carve {
29   namespace geom {
30     template<unsigned ndim,
31              typename data_t,
32              typename inserter_t,
33              typename aabb_calc_t>
34     class kd_node {
35       kd_node(const kd_node &);
36       kd_node &operator=(const kd_node &);
37
38     public:
39       kd_node *c_neg;
40       kd_node *c_pos;
41       kd_node *parent;
42       axis_pos splitpos;
43
44       typedef vector<ndim> vector_t;
45       typedef std::list<data_t> container_t;
46
47       container_t data;
48
49       kd_node(kd_node *_parent = NULL) : c_neg(NULL), c_pos(NULL), parent(_parent), splitpos(0, 0.0) {
50       }
51
52       ~kd_node() {
53         if (c_neg) delete c_neg;
54         if (c_pos) delete c_pos;
55       }
56
57       template<typename visitor_t>
58       void closeNodes(const vector_t &p, double d, visitor_t &visit) const {
59         if (c_neg) {
60           double delta = splitpos.pos - p[splitpos.axis];
61           if (delta <= d) c_neg->closeNodes(p, d, visit);
62           if (delta >= -d) c_pos->closeNodes(p, d, visit);
63         } else {
64           visit(this);
65         }
66       }
67
68       void removeData(const data_t &d) {
69         typename container_t::iterator i = std::find(data.begin(), data.end(), d);
70
71         if (i != data.end()) {
72           data.erase(i);
73         }
74       }
75
76       void addData(const data_t &d) {
77         data.push_back(d);
78       }
79
80       void insert(const data_t &data, inserter_t &inserter) {
81         inserter.insert(this, data);
82       }
83
84       void insert(const data_t &data) {
85         inserter_t inserter;
86         insert(data, inserter);
87       }
88
89       void remove(const data_t &data, inserter_t &inserter) {
90         inserter.remove(this, data);
91       }
92
93       void remove(const data_t &data) {
94         inserter_t inserter;
95         remove(data, inserter);
96       }
97
98       carve::geom::aabb<ndim> nodeAABB() const {
99         carve::geom::aabb<ndim> aabb;
100         if (c_neg) {
101           aabb = c_neg->nodeAABB();
102           aabb.unionAABB(c_pos->nodeAABB());
103         } else {
104           if (data.size()) {
105             typename container_t::const_iterator i = data.begin();
106             aabb = aabb_calc_t()(*i);
107             while (i != data.end()) {
108               aabb.unionAABB(aabb_calc_t()(*i));
109               ++i;
110             }
111           }
112         }
113         return aabb;
114       }
115
116       bool split(axis_pos split_at, inserter_t &inserter) {
117         if (c_neg) {
118           // already split
119           return false;
120         }
121
122         c_neg = new kd_node(this);
123         c_pos = new kd_node(this);
124
125         // choose an axis and split point.
126         splitpos = split_at;
127
128         carve::geom::aabb<ndim> aabb;
129
130         if (splitpos.axis < 0 ||
131             splitpos.axis >= ndim ||
132             splitpos.pos == std::numeric_limits<double>::max()) {
133           // need an aabb
134           if (data.size()) {
135             typename container_t::const_iterator i = data.begin();
136             aabb = aabb_calc_t()(*i);
137             while (i != data.end()) {
138               aabb.unionAABB(aabb_calc_t()(*i));
139               ++i;
140             }
141           }
142         }
143
144         if (splitpos.axis < 0 || splitpos.axis >= ndim) {
145
146           // choose an axis;
147
148           // if no axis was specified, force calculation of the split position.
149           splitpos.pos = std::numeric_limits<double>::max();
150
151           // choose the axis of the AABB with the biggest extent.
152           splitpos.axis = largestAxis(aabb.extent);
153
154           if (parent && splitpos.axis == parent->splitpos.axis) {
155             // but don't choose the same axis as the parent node;
156             // choose the axis with the second greatest AABB extent.
157             double e = -1.0;
158             int a = -1;
159             for (unsigned i = 0; i < ndim; ++i) {
160               if (i == splitpos.axis) continue;
161               if (e < aabb.extent[i]) { a = i; e = aabb.extent[i]; }
162             }
163             if (a != -1) {
164               splitpos.axis = a;
165             }
166           }
167         }
168
169         if (splitpos.pos == std::numeric_limits<double>::max()) {
170           carve::geom::vector<ndim> min = aabb.min();
171           carve::geom::vector<ndim> max = aabb.max();
172           splitpos.pos = aabb.pos.v[splitpos.axis];
173         }
174
175         inserter.propagate(this);
176
177         return true;
178       }
179
180       bool split(axis_pos split_at = axis_pos(-1, std::numeric_limits<double>::max())) {
181         inserter_t inserter;
182         return split(split_at, inserter);
183       }
184
185       void splitn(int num, inserter_t &inserter) {
186         if (num <= 0) return;
187         if (!c_neg) {
188           split(inserter);
189         }
190         if (c_pos) c_pos->splitn(num-1, inserter);
191         if (c_neg) c_neg->splitn(num-1, inserter);
192       }
193
194       void splitn(int num) {
195         inserter_t inserter;
196         splitn(num, inserter);
197       }
198
199       template<typename split_t>
200       void splitn(int num, split_t splitter, inserter_t &inserter) {
201         if (num <= 0) return;
202         if (!c_neg) {
203           split(inserter, splitter(this));
204         }
205         if (c_pos) c_pos->splitn(num-1, inserter, splitter);
206         if (c_neg) c_neg->splitn(num-1, inserter, splitter);
207       }
208
209       template<typename split_t>
210       void splitn(int num, split_t splitter) {
211         inserter_t inserter;
212         splitn(num, splitter, inserter);
213       }
214
215       template<typename pred_t>
216       void splitpred(pred_t pred, inserter_t &inserter, int depth = 0) {
217         if (!c_neg) {
218           axis_pos splitpos(-1, std::numeric_limits<double>::max());
219           if (!pred(this, depth, splitpos)) return;
220           split(splitpos, inserter);
221         }
222         if (c_pos) c_pos->splitpred(pred, inserter, depth + 1);
223         if (c_neg) c_neg->splitpred(pred, inserter, depth + 1);
224       }
225
226       template<typename pred_t>
227       void splitpred(pred_t pred, int depth = 0) {
228         inserter_t inserter;
229         splitpred(pred, inserter, depth);
230       }
231
232       // distance_t must provide:
233       // double operator()(data_t, vector<ndim>);
234       // double operator()(axis_pos, vector<ndim>);
235       template<typename distance_t>
236       struct near_point_query {
237
238         // q_t - the priority queue value type.
239         // q_t.first:  distance from object to query point.
240         // q_t.second: pointer to object
241         typedef std::pair<double, const data_t *> q_t;
242
243         // the queue priority should sort from smallest distance to largest, and on equal distance, by object pointer.
244         struct pcmp {
245           bool operator()(const q_t &a, const q_t &b) {
246             return (a.first > b.first) || ((a.first == b.first) && (a.second < b.second));
247           }
248         };
249
250         vector<ndim> point;
251         const kd_node *node;
252         std::priority_queue<q_t, std::vector<q_t>, pcmp> pq;
253
254         distance_t dist;
255         double dist_to_parent_split;
256
257         void addToPQ(kd_node *node) {
258           if (node->c_neg) {
259             addToPQ(node->c_neg);
260             addToPQ(node->c_pos);
261           } else {
262             for (typename kd_node::container_t::const_iterator i = node->data.begin(); i != node->data.end(); ++i) {
263               double d = dist((*i), point);
264               pq.push(std::make_pair(d, &(*i)));
265             }
266           }
267         }
268
269         const data_t *next() {
270           while (1) {
271             if (pq.size()) {
272               q_t t = pq.top();
273               if (!node->parent || t.first < dist_to_parent_split) {
274                 pq.pop();
275                 return t.second;
276               }
277             }
278
279             if (!node->parent) return NULL;
280
281             if (node->parent->c_neg == node) {
282               addToPQ(node->parent->c_pos);
283             } else {
284               addToPQ(node->parent->c_neg);
285             }
286
287             node = node->parent;
288             dist_to_parent_split = dist(node->splitpos, point);
289           }
290         }
291
292         near_point_query(const vector<ndim> _point, const kd_node *_node) : point(_point), node(_node), pq(), dist() {
293           while (node->c_neg) {
294             node = (point[node->axis] < node->pos) ? node->c_neg : node->c_pos;
295           }
296           if (node->parent) {
297             dist_to_parent_split = dist(node->parent->splitpos, point);
298           } else {
299             dist_to_parent_split = HUGE_VAL;
300           }
301           addToPQ(node);
302         }
303       };
304
305     };
306
307   }
308 }