GetFEM  5.4.2
bgeot_rtree.cc
1 /*===========================================================================
2 
3  Copyright (C) 2000-2020 Julien Pommier
4 
5  This file is a part of GetFEM
6 
7  GetFEM is free software; you can redistribute it and/or modify it
8  under the terms of the GNU Lesser General Public License as published
9  by the Free Software Foundation; either version 3 of the License, or
10  (at your option) any later version along with the GCC Runtime Library
11  Exception either version 3.1 or (at your option) any later version.
12  This program is distributed in the hope that it will be useful, but
13  WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
14  or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
15  License and GCC Runtime Library Exception for more details.
16  You should have received a copy of the GNU Lesser General Public License
17  along with this program; if not, write to the Free Software Foundation,
18  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA.
19 
20 ===========================================================================*/
21 
22 #include "getfem/bgeot_rtree.h"
23 
24 namespace bgeot {
25 
26  struct rtree_node : public rtree_elt_base {
27  std::unique_ptr<rtree_elt_base> left, right;
28  rtree_node(const base_node& bmin, const base_node& bmax,
29  std::unique_ptr<rtree_elt_base> &&left_,
30  std::unique_ptr<rtree_elt_base> &&right_)
31  : rtree_elt_base(false, bmin, bmax), left(std::move(left_)),
32  right( std::move(right_)) { }
33  };
34 
35  struct rtree_leaf : public rtree_elt_base {
36  rtree::pbox_cont lst;
37  rtree_leaf(const base_node& bmin, const base_node& bmax,
38  rtree::pbox_cont& lst_)
39  : rtree_elt_base(true, bmin, bmax) { lst.swap(lst_); }
40  };
41 
42  /* enlarge box to hold [a..b] */
43  static void update_box(base_node& bmin, base_node& bmax,
44  const base_node& a, const base_node& b) {
45  base_node::iterator itmin=bmin.begin(), itmax=bmax.begin();
46  for (size_type i=0; i < a.size(); ++i) {
47  itmin[i] = std::min(itmin[i], a.at(i));
48  itmax[i] = std::max(itmax[i], b.at(i));
49  }
50  }
51 
52  inline static bool r1_ge_r2(const base_node& min1, const base_node& max1,
53  const base_node& min2, const base_node& max2,
54  scalar_type EPS) {
55  for (size_type i=0; i < min1.size(); ++i)
56  if ((min1[i] > min2[i]+EPS) || (max1[i] < max2[i]-EPS)) return false;
57  return true;
58  }
59 
60  inline static bool r1_inter_r2(const base_node& min1, const base_node& max1,
61  const base_node& min2, const base_node& max2,
62  scalar_type EPS) {
63  for (size_type i=0; i < min1.size(); ++i)
64  if ((max1[i] < min2[i]-EPS) || (min1[i] > max2[i]+EPS)) return false;
65  return true;
66  }
67 
68  /* some predicates for searches */
69  struct intersection_p {
70  const base_node &min, &max;
71  const scalar_type EPS;
72  intersection_p(const base_node& min_, const base_node& max_, scalar_type EPS_)
73  : min(min_), max(max_), EPS(EPS_) {}
74  bool operator()(const base_node& min2, const base_node& max2) const
75  { return r1_inter_r2(min,max,min2,max2,EPS); }
76  bool accept(const base_node& min2, const base_node& max2) const
77  { return operator()(min2,max2); }
78  };
79 
80  /* match boxes containing [min..max] */
81  struct contains_p {
82  const base_node &min, &max;
83  const scalar_type EPS;
84  contains_p(const base_node& min_, const base_node& max_, scalar_type EPS_)
85  : min(min_), max(max_), EPS(EPS_) {}
86  bool operator()(const base_node& min2, const base_node& max2) const
87  { return r1_ge_r2(min2,max2,min,max,EPS); }
88  bool accept(const base_node& min2, const base_node& max2) const
89  { return r1_inter_r2(min,max,min2,max2,EPS); }
90  };
91 
92  /* match boxes contained in [min..max] */
93  struct contained_p {
94  const base_node &min, &max;
95  const scalar_type EPS;
96  contained_p(const base_node& min_, const base_node& max_, scalar_type EPS_)
97  : min(min_), max(max_), EPS(EPS_) {}
98  bool accept(const base_node& min2, const base_node& max2) const
99  { return r1_inter_r2(min,max,min2,max2,EPS); }
100  bool operator()(const base_node& min2, const base_node& max2) const
101  { return r1_ge_r2(min,max,min2,max2,EPS); }
102  };
103 
104  /* match boxes containing P */
105  struct has_point_p {
106  const base_node &P;
107  const scalar_type EPS;
108  has_point_p(const base_node& P_, scalar_type EPS_) : P(P_), EPS(EPS_) {}
109  bool operator()(const base_node& min2, const base_node& max2) const {
110  for (size_type i = 0; i < P.size(); ++i) {
111  if (P[i] < min2[i]-EPS) return false;
112  if (P[i] > max2[i]+EPS) return false;
113  }
114  return true;
115  }
116  bool accept(const base_node& min2, const base_node& max2) const
117  { return operator()(min2,max2); }
118  };
119 
120  /* match boxes intersecting the line passing through org and of
121  direction vector dirv.*/
122  struct intersect_line {
123  const base_node org;
124  const base_small_vector dirv;
125  intersect_line(const base_node& org_, const base_small_vector &dirv_)
126  : org(org_), dirv(dirv_) {}
127  bool operator()(const base_node& min2, const base_node& max2) const {
128  size_type N = org.size();
129  GMM_ASSERT1(N == min2.size(), "Dimensions mismatch");
130  for (size_type i = 0; i < N; ++i)
131  if (dirv[i] != scalar_type(0)) {
132  scalar_type a1=(min2[i]-org[i])/dirv[i], a2=(max2[i]-org[i])/dirv[i];
133  bool interf1 = true, interf2 = true;
134  for (size_type j = 0; j < N; ++j)
135  if (j != i) {
136  scalar_type y1 = org[j] + a1*dirv[j], y2 = org[j] + a2*dirv[j];
137  if (y1 < min2[j] || y1 > max2[j]) interf1 = false;
138  if (y2 < min2[j] || y2 > max2[j]) interf2 = false;
139  }
140  if (interf1 || interf2) return true;
141  }
142  return false;
143  }
144  bool accept(const base_node& min2, const base_node& max2) const
145  { return operator()(min2,max2); }
146  };
147 
148  /* match boxes intersecting the line passing through org and of
149  direction vector dirv.*/
150  struct intersect_line_and_box {
151  const base_node org;
152  const base_small_vector dirv;
153  const base_node min,max;
154  const scalar_type EPS;
155  intersect_line_and_box(const base_node& org_,
156  const base_small_vector &dirv_,
157  const base_node& min_, const base_node& max_,
158  scalar_type EPS_)
159  : org(org_), dirv(dirv_), min(min_), max(max_), EPS(EPS_) {}
160  bool operator()(const base_node& min2, const base_node& max2) const {
161  size_type N = org.size();
162  GMM_ASSERT1(N == min2.size(), "Dimensions mismatch");
163  if (!(r1_inter_r2(min,max,min2,max2,EPS))) return false;
164  for (size_type i = 0; i < N; ++i)
165  if (dirv[i] != scalar_type(0)) {
166  scalar_type a1=(min2[i]-org[i])/dirv[i], a2=(max2[i]-org[i])/dirv[i];
167  bool interf1 = true, interf2 = true;
168  for (size_type j = 0; j < N; ++j)
169  if (j != i) {
170  scalar_type y1 = org[j] + a1*dirv[j], y2 = org[j] + a2*dirv[j];
171  if (y1 < min2[j] || y1 > max2[j]) interf1 = false;
172  if (y2 < min2[j] || y2 > max2[j]) interf2 = false;
173  }
174  if (interf1 || interf2) return true;
175  }
176  return false;
177  }
178  bool accept(const base_node& min2, const base_node& max2) const
179  { return operator()(min2,max2); }
180  };
181 
182  size_type rtree::add_box(const base_node &min, const base_node &max,
183  size_type id) {
184  box_index bi;
185  if (tree_built) {
186  GMM_WARNING3("Add a box when the tree is already built cancel the tree. "
187  "Unefficient operation.");
188  tree_built = false; root = std::unique_ptr<rtree_elt_base>();
189  }
190  bi.min = &nodes[nodes.add_node(min, EPS)];
191  bi.max = &nodes[nodes.add_node(max, EPS)];
192  bi.id = (id + 1) ? id : boxes.size();
193  return boxes.emplace(std::move(bi)).first->id;
194  }
195 
196  rtree::rtree(scalar_type EPS_)
197  : EPS(EPS_), boxes(box_index_topology_compare(EPS_)), tree_built(false)
198  {}
199 
200  void rtree::clear() {
201  root = std::unique_ptr<rtree_elt_base>();
202  boxes.clear();
203  nodes.clear();
204  tree_built = false;
205  }
206 
207  template <typename Predicate>
208  static void find_matching_boxes_(rtree_elt_base *n, rtree::pbox_set& boxlst,
209  const Predicate &p) {
210  if (n->isleaf()) {
211  const rtree_leaf *rl = static_cast<rtree_leaf*>(n);
212  for (rtree::pbox_cont::const_iterator it = rl->lst.begin();
213  it != rl->lst.end(); ++it) {
214  if (p(*(*it)->min, *(*it)->max)) { boxlst.insert(*it); }
215  }
216  } else {
217  const rtree_node *rn = static_cast<rtree_node*>(n);
218  if (p.accept(rn->left->rmin,rn->left->rmax))
219  bgeot::find_matching_boxes_(rn->left.get(), boxlst, p);
220  if (p.accept(rn->right->rmin,rn->right->rmax))
221  bgeot::find_matching_boxes_(rn->right.get(), boxlst, p);
222  }
223  }
224 
225  void rtree::find_intersecting_boxes(const base_node& bmin,
226  const base_node& bmax,
227  pbox_set& boxlst) const {
228 
229  boxlst.clear();
230  GMM_ASSERT2(tree_built, "Boxtree not initialised.");
231  if (root)
232  find_matching_boxes_(root.get(),boxlst,intersection_p(bmin,bmax, EPS));
233  }
234 
235  void rtree::find_containing_boxes(const base_node& bmin,
236  const base_node& bmax,
237  pbox_set& boxlst) const {
238  boxlst.clear();
239  GMM_ASSERT2( tree_built, "Boxtree not initialised.");
240  if (root)
241  find_matching_boxes_(root.get(), boxlst, contains_p(bmin,bmax, EPS));
242  }
243 
244  void rtree::find_contained_boxes(const base_node& bmin,
245  const base_node& bmax,
246  pbox_set& boxlst) const {
247  boxlst.clear();
248  GMM_ASSERT2(tree_built, "Boxtree not initialised.");
249  if (root)
250  find_matching_boxes_(root.get(), boxlst, contained_p(bmin,bmax, EPS));
251  }
252 
253  void rtree::find_boxes_at_point(const base_node& P, pbox_set& boxlst) const {
254  boxlst.clear();
255  GMM_ASSERT2(tree_built, "Boxtree not initialised.");
256  if (root)
257  find_matching_boxes_(root.get(), boxlst, has_point_p(P, EPS));
258  }
259 
260  void rtree::find_line_intersecting_boxes(const base_node& org,
261  const base_small_vector& dirv,
262  pbox_set& boxlst) const {
263  boxlst.clear();
264  GMM_ASSERT2(tree_built, "Boxtree not initialised.");
265  if (root)
266  find_matching_boxes_(root.get(),boxlst,intersect_line(org, dirv));
267  }
268 
269  void rtree::find_line_intersecting_boxes(const base_node& org,
270  const base_small_vector& dirv,
271  const base_node& bmin,
272  const base_node& bmax,
273  pbox_set& boxlst) const {
274  boxlst.clear();
275  GMM_ASSERT2(tree_built, "Boxtree not initialised.");
276  if (root)
277  find_matching_boxes_(root.get(), boxlst,
278  intersect_line_and_box(org, dirv, bmin, bmax, EPS));
279  }
280 
281  /*
282  try to split at the approximate center of the box. Could be much more
283  sophisticated
284  */
285  static bool split_test(const rtree::pbox_cont& b,
286  const base_node& bmin, const base_node& bmax,
287  unsigned dir, scalar_type& split_v) {
288  scalar_type v = bmin[dir] + (bmax[dir] - bmin[dir])/2; split_v = v;
289  size_type cnt = 0;
290  for (rtree::pbox_cont::const_iterator it = b.begin(); it!=b.end(); ++it) {
291  if ((*it)->max->at(dir) < v) {
292  if (cnt == 0) split_v = (*it)->max->at(dir);
293  else split_v = std::max((*it)->max->at(dir),split_v);
294  cnt++;
295  }
296  }
297  return (cnt > 0 && cnt < b.size());
298  }
299 
300  /*
301  there are many flavors of rtree ... this one is more or less a quadtree
302  where splitting does not occurs at predefined positions (hence the
303  split_test function above).
304  Regions of the tree do not overlap (box are splitted).
305  */
306  static std::unique_ptr<rtree_elt_base> build_tree_(rtree::pbox_cont b,
307  const base_node& bmin,
308  const base_node& bmax,
309  unsigned last_dir) {
310  size_type N=bmin.size();
311  scalar_type split_v(0);
312  unsigned split_dir = unsigned((last_dir+1)%N);
313  bool split_ok = false;
314  if (b.size() > rtree_elt_base::RECTS_PER_LEAF) {
315  for (size_type itry=0; itry < N; ++itry) {
316  if (split_test(b, bmin, bmax, split_dir, split_v))
317  { split_ok = true; break; }
318  split_dir = unsigned((split_dir+1)%N);
319  }
320  }
321  if (split_ok) {
322  size_type cnt1=0,cnt2=0;
323  for (rtree::pbox_cont::const_iterator it = b.begin();
324  it != b.end(); ++it) {
325  if ((*it)->min->at(split_dir) < split_v) cnt1++;
326  if ((*it)->max->at(split_dir) > split_v) cnt2++;
327  }
328  assert(cnt1); assert(cnt2);
329  GMM_ASSERT1(cnt1+cnt2 >= b.size(), "internal error");
330  rtree::pbox_cont v1(cnt1), v2(cnt2);
331  base_node bmin1(bmax), bmax1(bmin);
332  base_node bmin2(bmax), bmax2(bmin);
333  cnt1 = cnt2 = 0;
334  for (rtree::pbox_cont::const_iterator it = b.begin();
335  it != b.end(); ++it) {
336  if ((*it)->min->at(split_dir) < split_v) {
337  v1[cnt1++] = *it;
338  update_box(bmin1,bmax1,*(*it)->min,*(*it)->max);
339  }
340  if ((*it)->max->at(split_dir) > split_v) {
341  v2[cnt2++] = *it;
342  update_box(bmin2,bmax2,*(*it)->min,*(*it)->max);
343  }
344  }
345  for (size_type k=0; k < N; ++k) {
346  bmin1[k] = std::max(bmin1[k],bmin[k]);
347  bmax1[k] = std::min(bmax1[k],bmax[k]);
348  bmin2[k] = std::max(bmin2[k],bmin[k]);
349  bmax2[k] = std::min(bmax2[k],bmax[k]);
350  }
351  bmax1[split_dir] = std::min(bmax1[split_dir], split_v);
352  bmin2[split_dir] = std::max(bmin2[split_dir], split_v);
353  assert(cnt1 == v1.size()); assert(cnt2 == v2.size());
354  return std::make_unique<rtree_node>
355  (bmin, bmax,
356  build_tree_(v1, bmin1, bmax1, split_dir),
357  build_tree_(v2, bmin2, bmax2, split_dir));
358  } else {
359  return std::make_unique<rtree_leaf>(bmin, bmax, b);
360  }
361  }
362 
363  void rtree::build_tree() {
364  if (!tree_built) {
365  if (boxes.size() == 0) { tree_built = true; return; }
366  getfem::local_guard lock = locks_.get_lock();
367  assert(root == 0);
368  pbox_cont b(boxes.size());
369  pbox_cont::iterator b_it = b.begin();
370  base_node bmin(*boxes.begin()->min), bmax(*boxes.begin()->max);
371  for (box_cont::const_iterator it=boxes.begin(); it != boxes.end(); ++it) {
372  update_box(bmin,bmax,*(*it).min,*(*it).max);
373  *b_it++ = &(*it);
374  }
375  root = build_tree_(b, bmin, bmax, 0);
376  tree_built = true;
377  }
378  }
379 
380  static void dump_tree_(rtree_elt_base *p, int level, size_type& count) {
381  if (!p) return;
382  for (int i=0; i < level; ++i) cout << " ";
383  cout << "span=" << p->rmin << ".." << p->rmax << " ";
384  if (p->isleaf()) {
385  rtree_leaf *rl = static_cast<rtree_leaf*>(p);
386  cout << "Leaf [" << rl->lst.size() << " elts] = ";
387  for (size_type i=0; i < rl->lst.size(); ++i)
388  cout << " " << rl->lst[i]->id;
389  cout << "\n";
390  count += rl->lst.size();
391  } else {
392  cout << "Node\n";
393  const rtree_node *rn = static_cast<rtree_node*>(p);
394  if (rn->left) { dump_tree_(rn->left.get(), level+1, count); }
395  if (rn->right) { dump_tree_(rn->right.get(), level+1, count); }
396  }
397  }
398 
399  void rtree::dump() {
400  cout << "tree dump follows\n";
401  if (!root) build_tree();
402  size_type count = 0;
403  dump_tree_(root.get(), 0, count);
404  cout << " --- end of tree dump, nb of rectangles: " << boxes.size()
405  << ", rectangle ref in tree: " << count << "\n";
406  }
407 }
bgeot::size_type
size_t size_type
used as the common size type in the library
Definition: bgeot_poly.h:49
bgeot_rtree.h
region-tree for window/point search on a set of rectangles.
bgeot::node_tab::add_node
size_type add_node(const base_node &pt, const scalar_type radius=0, bool remove_duplicated_nodes=true)
Add a point to the array or use an existing point, located within a distance smaller than radius.
Definition: bgeot_node_tab.cc:96
bgeot
Basic Geometric Tools.
Definition: bgeot_convex_ref.cc:27