lasquadtree.cpp
Go to the documentation of this file.
1 /*
2 ===============================================================================
3 
4  FILE: lasquadtree.cpp
5 
6  CONTENTS:
7 
8  see corresponding header file
9 
10  PROGRAMMERS:
11 
12  martin.isenburg@gmail.com
13 
14  COPYRIGHT:
15 
16  (c) 2011, Martin Isenburg, LASSO - tools to catch reality
17 
18  This software is distributed WITHOUT ANY WARRANTY and without even the
19  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
20 
21  CHANGE HISTORY:
22 
23  see corresponding header file
24 
25 ===============================================================================
26 */
27 #include "lasquadtree.hpp"
28 
29 #include "bytestreamin.hpp"
30 #include "bytestreamout.hpp"
31 
32 #include <stdio.h>
33 #include <stdlib.h>
34 #include <string.h>
35 
36 #include <vector>
37 using namespace std;
38 
39 typedef vector<I32> my_cell_vector;
40 
41 /*
42 
43 class LAScell
44 {
45 public:
46  LAScell* parent;
47  LAScell* child[4];
48  U32 num_children : 3;
49  U32 level : 5;
50  U32 idx : 24;
51  F32 mid_x;
52  F32 mid_y;
53  F32 half_size;
54  F32 get_min_x() const { return mid_x - half_size; };
55  F32 get_min_y() const { return mid_y - half_size; };
56  F32 get_max_x() const { return mid_x + half_size; };
57  F32 get_max_y() const { return mid_y + half_size; };
58  LAScell();
59 };
60 
61 LAScell::LAScell()
62 {
63  memset(this, 0, sizeof(LAScell));
64 }
65 
66  LAScell* get_cell(const F64 x, const F64 y);
67 LAScell* LASquadtree::get_cell(const F64 x, const F64 y)
68 {
69  LAScell* cell = &root;
70  while (cell && cell->num_children)
71  {
72  int child = 0;
73  if (x < cell->mid_x) // only if strictly less
74  {
75  child |= 1;
76  }
77  if (!(y < cell->mid_y)) // only if not strictly less
78  {
79  child |= 2;
80  }
81  cell = cell->child[child];
82  }
83  return cell;
84 }
85 
86 static bool intersect_point(LAScell* cell, const float* p_pos)
87 {
88  if (cell->num_children) // check if cell has children
89  {
90  int idx = 0;
91  if (p_pos[0] < cell->r_mid[0]) idx |= 1;
92  if (!(p_pos[1] < cell->r_mid[1])) idx |= 2;
93  if (cell->child[idx] && intersect_point(cell->child[idx], p_pos)) return true;
94  return false;
95  }
96  return true;
97 }
98 */
99 
100 // returns the bounding box of the cell that x & y fall into at the specified level
101 void LASquadtree::get_cell_bounding_box(const F64 x, const F64 y, U32 level, F32* min, F32* max) const
102 {
103  volatile float cell_mid_x;
104  volatile float cell_mid_y;
105  float cell_min_x, cell_max_x;
106  float cell_min_y, cell_max_y;
107 
108  cell_min_x = min_x;
109  cell_max_x = max_x;
110  cell_min_y = min_y;
111  cell_max_y = max_y;
112 
113  while (level)
114  {
115  cell_mid_x = (cell_min_x + cell_max_x)/2;
116  cell_mid_y = (cell_min_y + cell_max_y)/2;
117  if (x < cell_mid_x)
118  {
119  cell_max_x = cell_mid_x;
120  }
121  else
122  {
123  cell_min_x = cell_mid_x;
124  }
125  if (y < cell_mid_y)
126  {
127  cell_max_y = cell_mid_y;
128  }
129  else
130  {
131  cell_min_y = cell_mid_y;
132  }
133  level--;
134  }
135  if (min)
136  {
137  min[0] = cell_min_x;
138  min[1] = cell_min_y;
139  }
140  if (max)
141  {
142  max[0] = cell_max_x;
143  max[1] = cell_max_y;
144  }
145 }
146 
147 // returns the bounding box of the cell that x & y fall into
148 void LASquadtree::get_cell_bounding_box(const F64 x, const F64 y, F32* min, F32* max) const
149 {
150  get_cell_bounding_box(x, y, levels, min, max);
151 }
152 
153 // returns the bounding box of the cell with the spedified level_index at the specified level
154 void LASquadtree::get_cell_bounding_box(U32 level_index, U32 level, F32* min, F32* max) const
155 {
156  volatile float cell_mid_x;
157  volatile float cell_mid_y;
158  float cell_min_x, cell_max_x;
159  float cell_min_y, cell_max_y;
160 
161  cell_min_x = min_x;
162  cell_max_x = max_x;
163  cell_min_y = min_y;
164  cell_max_y = max_y;
165 
166  U32 index;
167  while (level)
168  {
169  index = (level_index >>(2*(level-1)))&3;
170  cell_mid_x = (cell_min_x + cell_max_x)/2;
171  cell_mid_y = (cell_min_y + cell_max_y)/2;
172  if (index & 1)
173  {
174  cell_min_x = cell_mid_x;
175  }
176  else
177  {
178  cell_max_x = cell_mid_x;
179  }
180  if (index & 2)
181  {
182  cell_min_y = cell_mid_y;
183  }
184  else
185  {
186  cell_max_y = cell_mid_y;
187  }
188  level--;
189  }
190  if (min)
191  {
192  min[0] = cell_min_x;
193  min[1] = cell_min_y;
194  }
195  if (max)
196  {
197  max[0] = cell_max_x;
198  max[1] = cell_max_y;
199  }
200 }
201 
202 // returns the bounding box of the cell with the spedified level_index
203 void LASquadtree::get_cell_bounding_box(U32 level_index, F32* min, F32* max) const
204 {
205  get_cell_bounding_box(level_index, levels, min, max);
206 }
207 
208 // returns the bounding box of the cell that x & y fall into
209 void LASquadtree::get_cell_bounding_box(const I32 cell_index, F32* min, F32* max) const
210 {
211  U32 level = get_level((U32)cell_index);
212  U32 level_index = get_level_index((U32)cell_index, level);
213  get_cell_bounding_box(level_index, level, min, max);
214 }
215 
216 // returns the (sub-)level index of the cell that x & y fall into at the specified level
217 U32 LASquadtree::get_level_index(const F64 x, const F64 y, U32 level) const
218 {
219  volatile float cell_mid_x;
220  volatile float cell_mid_y;
221  float cell_min_x, cell_max_x;
222  float cell_min_y, cell_max_y;
223 
224  cell_min_x = min_x;
225  cell_max_x = max_x;
226  cell_min_y = min_y;
227  cell_max_y = max_y;
228 
229  U32 level_index = 0;
230 
231  while (level)
232  {
233  level_index <<= 2;
234 
235  cell_mid_x = (cell_min_x + cell_max_x)/2;
236  cell_mid_y = (cell_min_y + cell_max_y)/2;
237 
238  if (x < cell_mid_x)
239  {
240  cell_max_x = cell_mid_x;
241  }
242  else
243  {
244  cell_min_x = cell_mid_x;
245  level_index |= 1;
246  }
247  if (y < cell_mid_y)
248  {
249  cell_max_y = cell_mid_y;
250  }
251  else
252  {
253  cell_min_y = cell_mid_y;
254  level_index |= 2;
255  }
256  level--;
257  }
258 
259  return level_index;
260 }
261 
262 // returns the (sub-)level index of the cell that x & y fall into
263 U32 LASquadtree::get_level_index(const F64 x, const F64 y) const
264 {
265  return get_level_index(x, y, levels);
266 }
267 
268 // returns the (sub-)level index and the bounding box of the cell that x & y fall into at the specified level
269 U32 LASquadtree::get_level_index(const F64 x, const F64 y, U32 level, F32* min, F32* max) const
270 {
271  volatile float cell_mid_x;
272  volatile float cell_mid_y;
273  float cell_min_x, cell_max_x;
274  float cell_min_y, cell_max_y;
275 
276  cell_min_x = min_x;
277  cell_max_x = max_x;
278  cell_min_y = min_y;
279  cell_max_y = max_y;
280 
281  U32 level_index = 0;
282 
283  while (level)
284  {
285  level_index <<= 2;
286 
287  cell_mid_x = (cell_min_x + cell_max_x)/2;
288  cell_mid_y = (cell_min_y + cell_max_y)/2;
289 
290  if (x < cell_mid_x)
291  {
292  cell_max_x = cell_mid_x;
293  }
294  else
295  {
296  cell_min_x = cell_mid_x;
297  level_index |= 1;
298  }
299  if (y < cell_mid_y)
300  {
301  cell_max_y = cell_mid_y;
302  }
303  else
304  {
305  cell_min_y = cell_mid_y;
306  level_index |= 2;
307  }
308  level--;
309  }
310  if (min)
311  {
312  min[0] = cell_min_x;
313  min[1] = cell_min_y;
314  }
315  if (max)
316  {
317  max[0] = cell_max_x;
318  max[1] = cell_max_y;
319  }
320  return level_index;
321 }
322 
323 // returns the (sub-)level index and the bounding box of the cell that x & y fall into
324 U32 LASquadtree::get_level_index(const F64 x, const F64 y, F32* min, F32* max) const
325 {
326  return get_level_index(x, y, levels, min, max);
327 }
328 
329 // returns the index of the cell that x & y fall into at the specified level
330 U32 LASquadtree::get_cell_index(const F64 x, const F64 y, U32 level) const
331 {
332  if (sub_level)
333  {
334  return level_offset[sub_level+level] + (sub_level_index << (level*2)) + get_level_index(x, y, level);
335  }
336  else
337  {
338  return level_offset[level]+get_level_index(x, y, level);
339  }
340 }
341 
342 // returns the index of the cell that x & y fall into
343 U32 LASquadtree::get_cell_index(const F64 x, const F64 y) const
344 {
345  return get_cell_index(x, y, levels);
346 }
347 
348 // returns the indices of parent and siblings for the specified cell index
349 BOOL LASquadtree::coarsen(const I32 cell_index, I32* coarser_cell_index, U32* num_cell_indices, I32** cell_indices) const
350 {
351  if (cell_index < 0) return FALSE;
352  U32 level = get_level((U32)cell_index);
353  if (level == 0) return FALSE;
354  U32 level_index = get_level_index((U32)cell_index, level);
355  level_index = level_index >> 2;
356  if (coarser_cell_index) (*coarser_cell_index) = get_cell_index(level_index, level-1);
357  if (num_cell_indices && cell_indices)
358  {
359  (*num_cell_indices) = 4;
360  (*cell_indices) = (I32*)coarser_indices;
361  level_index = level_index << 2;
362  (*cell_indices)[0] = get_cell_index(level_index + 0, level);
363  (*cell_indices)[1] = get_cell_index(level_index + 1, level);
364  (*cell_indices)[2] = get_cell_index(level_index + 2, level);
365  (*cell_indices)[3] = get_cell_index(level_index + 3, level);
366  }
367  return TRUE;
368 }
369 
370 // returns the level index of the cell index at the specified level
371 U32 LASquadtree::get_level_index(U32 cell_index, U32 level) const
372 {
373  if (sub_level)
374  {
375  return cell_index - (sub_level_index << (level*2)) - level_offset[sub_level+level];
376  }
377  else
378  {
379  return cell_index - level_offset[level];
380  }
381 }
382 
383 // returns the level index of the cell index
385 {
386  return get_level_index(cell_index, levels);
387 }
388 
389 // returns the level the cell index
390 U32 LASquadtree::get_level(U32 cell_index) const
391 {
392  int level = 0;
393  while (cell_index >= level_offset[level+1]) level++;
394  return level;
395 }
396 
397 // returns the cell index of the level index at the specified level
398 U32 LASquadtree::get_cell_index(U32 level_index, U32 level) const
399 {
400  if (sub_level)
401  {
402  return level_index + (sub_level_index << (level*2)) + level_offset[sub_level+level];
403  }
404  else
405  {
406  return level_index + level_offset[level];
407  }
408 }
409 
410 // returns the cell index of the level index
412 {
413  return get_cell_index(level_index, levels);
414 }
415 
416 // returns the maximal level index at the specified level
418 {
419  return (1<<level)*(1<<level);
420 }
421 
422 // returns the maximal level index
424 {
425  return get_max_level_index(levels);
426 }
427 
428 // returns the maximal cell index at the specified level
430 {
431  return level_offset[level+1]-1;
432 }
433 
434 // returns the maximal cell index
436 {
437  return get_max_cell_index(levels);
438 }
439 
440 // recursively does the actual rastering of the occupancy
441 void LASquadtree::raster_occupancy(BOOL(*does_cell_exist)(I32), U32* data, U32 min_x, U32 min_y, U32 level_index, U32 level, U32 stop_level) const
442 {
443  U32 cell_index = get_cell_index(level_index, level);
444  U32 adaptive_pos = cell_index/32;
445  U32 adaptive_bit = ((U32)1) << (cell_index%32);
446  // have we reached a leaf
447  if (adaptive[adaptive_pos] & adaptive_bit) // interior node
448  {
449  if (level < stop_level) // do we need to continue
450  {
451  level_index <<= 2;
452  level += 1;
453  U32 size = 1 << (stop_level-level);
454  // recurse into the four children
455  raster_occupancy(does_cell_exist, data, min_x, min_y, level_index, level, stop_level);
456  raster_occupancy(does_cell_exist, data, min_x+size, min_y, level_index + 1, level, stop_level);
457  raster_occupancy(does_cell_exist, data, min_x, min_y+size, level_index + 2, level, stop_level);
458  raster_occupancy(does_cell_exist, data, min_x+size, min_y+size, level_index + 3, level, stop_level);
459  return;
460  }
461  else // no ... raster remaining subtree
462  {
463  U32 full_size = (1 << stop_level);
464  U32 size = 1 << (stop_level-level);
465  U32 max_y = min_y + size;
466  U32 pos, pos_x, pos_y;
467  for (pos_y = min_y; pos_y < max_y; pos_y++)
468  {
469  pos = pos_y*full_size + min_x;
470  for (pos_x = 0; pos_x < size; pos_x++)
471  {
472  data[pos/32] |= (1<<(pos%32));
473  pos++;
474  }
475  }
476  }
477  }
478  else if (does_cell_exist(cell_index))
479  {
480  // raster actual cell
481  U32 full_size = (1 << stop_level);
482  U32 size = 1 << (stop_level-level);
483  U32 max_y = min_y + size;
484  U32 pos, pos_x, pos_y;
485  for (pos_y = min_y; pos_y < max_y; pos_y++)
486  {
487  pos = pos_y*full_size + min_x;
488  for (pos_x = 0; pos_x < size; pos_x++)
489  {
490  data[pos/32] |= (1<<(pos%32));
491  pos++;
492  }
493  }
494  }
495 }
496 
497 // rasters the occupancy to a simple binary raster at depth level
498 U32* LASquadtree::raster_occupancy(BOOL(*does_cell_exist)(I32), U32 level) const
499 {
500  U32 pos;
501  U32 size = (1<<level);
502  U32* data = new U32[size*size/32];
503  for (pos = 0; pos < (size*size/32); pos++)
504  {
505  data[pos] = 0;
506  }
507  raster_occupancy(does_cell_exist, data, 0, 0, 0, 0, level);
508  return data;
509 }
510 
511 // rasters the occupancy to a simple binary raster at depth levels
512 U32* LASquadtree::raster_occupancy(BOOL(*does_cell_exist)(I32)) const
513 {
514  return raster_occupancy(does_cell_exist, levels);
515 }
516 
517 // read from file
519 {
520  // read data in the following order
521  // U32 levels 4 bytes
522  // U32 level_index 4 bytes (default 0)
523  // U32 implicit_levels 4 bytes (only used when level_index != 0))
524  // F32 min_x 4 bytes
525  // F32 max_x 4 bytes
526  // F32 min_y 4 bytes
527  // F32 max_y 4 bytes
528  // which totals 28 bytes
529 
530  char signature[4];
531  try { stream->getBytes((U8*)signature, 4); } catch(...)
532  {
533  fprintf(stderr,"ERROR (LASquadtree): reading signature\n");
534  return FALSE;
535  }
536  if (strncmp(signature, "LASQ", 4) != 0)
537  {
538 // fprintf(stderr,"ERROR (LASquadtree): wrong signature %4s instead of 'LASV'\n", signature);
539 // return FALSE;
540  levels = ((U32*)signature)[0];
541  }
542  else
543  {
544  U32 version;
545  try { stream->get32bitsLE((U8*)&version); } catch(...)
546  {
547  fprintf(stderr,"ERROR (LASquadtree): reading version\n");
548  return FALSE;
549  }
550  try { stream->get32bitsLE((U8*)&levels); } catch(...)
551  {
552  fprintf(stderr,"ERROR (LASquadtree): reading levels\n");
553  return FALSE;
554  }
555  }
556  U32 level_index;
557  try { stream->get32bitsLE((U8*)&level_index); } catch(...)
558  {
559  fprintf(stderr,"ERROR (LASquadtree): reading level_index\n");
560  return FALSE;
561  }
562  U32 implicit_levels;
563  try { stream->get32bitsLE((U8*)&implicit_levels); } catch(...)
564  {
565  fprintf(stderr,"ERROR (LASquadtree): reading implicit_levels\n");
566  return FALSE;
567  }
568  try { stream->get32bitsLE((U8*)&min_x); } catch(...)
569  {
570  fprintf(stderr,"ERROR (LASquadtree): reading min_x\n");
571  return FALSE;
572  }
573  try { stream->get32bitsLE((U8*)&max_x); } catch(...)
574  {
575  fprintf(stderr,"ERROR (LASquadtree): reading max_x\n");
576  return FALSE;
577  }
578  try { stream->get32bitsLE((U8*)&min_y); } catch(...)
579  {
580  fprintf(stderr,"ERROR (LASquadtree): reading min_y\n");
581  return FALSE;
582  }
583  try { stream->get32bitsLE((U8*)&max_y); } catch(...)
584  {
585  fprintf(stderr,"ERROR (LASquadtree): reading max_y\n");
586  return FALSE;
587  }
588  return TRUE;
589 }
590 
592 {
593  // which totals 28 bytes
594  // U32 levels 4 bytes
595  // U32 level_index 4 bytes (default 0)
596  // U32 implicit_levels 4 bytes (only used when level_index != 0))
597  // F32 min_x 4 bytes
598  // F32 max_x 4 bytes
599  // F32 min_y 4 bytes
600  // F32 max_y 4 bytes
601  // which totals 28 bytes
602 
603 /*
604  if (!stream->putBytes((U8*)"LASQ", 4))
605  {
606  fprintf(stderr,"ERROR (LASquadtree): writing signature\n");
607  return FALSE;
608  }
609 
610  U32 version = 0;
611  if (!stream->put32bitsLE((U8*)&version))
612  {
613  fprintf(stderr,"ERROR (LASquadtree): writing version\n");
614  return FALSE;
615  }
616 */
617 
618  if (!stream->put32bitsLE((U8*)&levels))
619  {
620  fprintf(stderr,"ERROR (LASquadtree): writing levels %u\n", levels);
621  return FALSE;
622  }
623  U32 level_index = 0;
624  if (!stream->put32bitsLE((U8*)&level_index))
625  {
626  fprintf(stderr,"ERROR (LASquadtree): writing level_index %u\n", level_index);
627  return FALSE;
628  }
629  U32 implicit_levels = 0;
630  if (!stream->put32bitsLE((U8*)&implicit_levels))
631  {
632  fprintf(stderr,"ERROR (LASquadtree): writing implicit_levels %u\n", implicit_levels);
633  return FALSE;
634  }
635  if (!stream->put32bitsLE((U8*)&min_x))
636  {
637  fprintf(stderr,"ERROR (LASquadtree): writing min_x %g\n", min_x);
638  return FALSE;
639  }
640  if (!stream->put32bitsLE((U8*)&max_x))
641  {
642  fprintf(stderr,"ERROR (LASquadtree): writing max_x %g\n", max_x);
643  return FALSE;
644  }
645  if (!stream->put32bitsLE((U8*)&min_y))
646  {
647  fprintf(stderr,"ERROR (LASquadtree): writing min_y %g\n", min_y);
648  return FALSE;
649  }
650  if (!stream->put32bitsLE((U8*)&max_y))
651  {
652  fprintf(stderr,"ERROR (LASquadtree): writing max_y %g\n", max_y);
653  return FALSE;
654  }
655  return TRUE;
656 }
657 
658 // create or finalize the cell (in the spatial hierarchy)
659 BOOL LASquadtree::manage_cell(const U32 cell_index, const BOOL finalize)
660 {
661  U32 adaptive_pos = cell_index/32;
662  U32 adaptive_bit = ((U32)1) << (cell_index%32);
663  if (adaptive_pos >= adaptive_alloc)
664  {
665  if (adaptive)
666  {
667  adaptive = (U32*)realloc(adaptive, adaptive_pos*2*sizeof(U32));
668  for (U32 i = adaptive_alloc; i < adaptive_pos*2; i++) adaptive[i] = 0;
669  adaptive_alloc = adaptive_pos*2;
670  }
671  else
672  {
673  adaptive = (U32*)malloc((adaptive_pos+1)*sizeof(U32));
674  for (U32 i = adaptive_alloc; i <= adaptive_pos; i++) adaptive[i] = 0;
675  adaptive_alloc = adaptive_pos+1;
676  }
677  }
678  adaptive[adaptive_pos] &= ~adaptive_bit;
679  U32 index;
680  U32 level = get_level(cell_index);
681  U32 level_index = get_level_index(cell_index, level);
682  while (level)
683  {
684  level--;
685  level_index = level_index >> 2;
686  index = get_cell_index(level_index, level);
687  adaptive_pos = index/32;
688  adaptive_bit = ((U32)1) << (index%32);
689  if (adaptive[adaptive_pos] & adaptive_bit) break;
690  adaptive[adaptive_pos] |= adaptive_bit;
691  }
692  return TRUE;
693 }
694 
695 // check whether the x & y coordinates fall into the tiling
696 BOOL LASquadtree::inside(const F64 x, const F64 y) const
697 {
698  if (sub_level)
699  {
700  return (min_x <= x && x < max_x && min_y <= y && y < max_y);
701  }
702  else
703  {
704  return (min_x <= x && x <= max_x && min_y <= y && y <= max_y);
705  }
706 }
707 
708 U32 LASquadtree::intersect_rectangle(const F64 r_min_x, const F64 r_min_y, const F64 r_max_x, const F64 r_max_y, U32 level)
709 {
710  if (current_cells == 0)
711  {
712  current_cells = (void*) new my_cell_vector;
713  }
714  else
715  {
716  ((my_cell_vector*)current_cells)->clear();
717  }
718 
719  if (r_max_x < min_x || !(r_min_x < max_x) || r_max_y < min_y || !(r_min_y < max_y))
720  {
721  return 0;
722  }
723 
724  if (adaptive)
725  {
726  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, min_x, max_x, min_y, max_y, 0, 0);
727  }
728  else
729  {
730  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, min_x, max_x, min_y, max_y, level, 0);
731  }
732 
733  return (U32)(((my_cell_vector*)current_cells)->size());
734 }
735 
736 U32 LASquadtree::intersect_rectangle(const F64 r_min_x, const F64 r_min_y, const F64 r_max_x, const F64 r_max_y)
737 {
738  return intersect_rectangle(r_min_x, r_min_y, r_max_x, r_max_y, levels);
739 }
740 
741 U32 LASquadtree::intersect_tile(const F32 ll_x, const F32 ll_y, const F32 size, U32 level)
742 {
743  if (current_cells == 0)
744  {
745  current_cells = (void*) new my_cell_vector;
746  }
747  else
748  {
749  ((my_cell_vector*)current_cells)->clear();
750  }
751 
752  volatile F32 ur_x = ll_x + size;
753  volatile F32 ur_y = ll_y + size;
754 
755  if (ur_x <= min_x || !(ll_x <= max_x) || ur_y <= min_y || !(ll_y <= max_y))
756  {
757  return 0;
758  }
759 
760  if (adaptive)
761  {
762  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, min_x, max_x, min_y, max_y, 0, 0);
763  }
764  else
765  {
766  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, min_x, max_x, min_y, max_y, level, 0);
767  }
768 
769  return (U32)(((my_cell_vector*)current_cells)->size());
770 }
771 
772 U32 LASquadtree::intersect_tile(const F32 ll_x, const F32 ll_y, const F32 size)
773 {
774  return intersect_tile(ll_x, ll_y, size, levels);
775 }
776 
777 U32 LASquadtree::intersect_circle(const F64 center_x, const F64 center_y, const F64 radius, U32 level)
778 {
779  if (current_cells == 0)
780  {
781  current_cells = (void*) new my_cell_vector;
782  }
783  else
784  {
785  ((my_cell_vector*)current_cells)->clear();
786  }
787 
788  F64 r_min_x = center_x - radius;
789  F64 r_min_y = center_y - radius;
790  F64 r_max_x = center_x + radius;
791  F64 r_max_y = center_y + radius;
792 
793  if (r_max_x < min_x || !(r_min_x < max_x) || r_max_y < min_y || !(r_min_y < max_y))
794  {
795  return 0;
796  }
797 
798  if (adaptive)
799  {
800  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, min_x, max_x, min_y, max_y, 0, 0);
801  }
802  else
803  {
804  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, min_x, max_x, min_y, max_y, level, 0);
805  }
806 
807  return (U32)(((my_cell_vector*)current_cells)->size());
808 }
809 
810 U32 LASquadtree::intersect_circle(const F64 center_x, const F64 center_y, const F64 radius)
811 {
812  return intersect_circle(center_x, center_y, radius, levels);
813 }
814 
815 void LASquadtree::intersect_rectangle_with_cells(const F64 r_min_x, const F64 r_min_y, const F64 r_max_x, const F64 r_max_y, const F32 cell_min_x, const F32 cell_max_x, const F32 cell_min_y, const F32 cell_max_y, U32 level, U32 level_index)
816 {
817  volatile float cell_mid_x;
818  volatile float cell_mid_y;
819  if (level)
820  {
821  level--;
822  level_index <<= 2;
823 
824  cell_mid_x = (cell_min_x + cell_max_x)/2;
825  cell_mid_y = (cell_min_y + cell_max_y)/2;
826 
827  if (r_max_x < cell_mid_x)
828  {
829  // cell_max_x = cell_mid_x;
830  if (r_max_y < cell_mid_y)
831  {
832  // cell_max_y = cell_mid_y;
833  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
834  }
835  else if (!(r_min_y < cell_mid_y))
836  {
837  // cell_min_y = cell_mid_y;
838  // level_index |= 1;
839  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
840  }
841  else
842  {
843  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
844  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
845  }
846  }
847  else if (!(r_min_x < cell_mid_x))
848  {
849  // cell_min_x = cell_mid_x;
850  // level_index |= 1;
851  if (r_max_y < cell_mid_y)
852  {
853  // cell_max_y = cell_mid_y;
854  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
855  }
856  else if (!(r_min_y < cell_mid_y))
857  {
858  // cell_min_y = cell_mid_y;
859  // level_index |= 1;
860  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
861  }
862  else
863  {
864  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
865  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
866  }
867  }
868  else
869  {
870  if (r_max_y < cell_mid_y)
871  {
872  // cell_max_y = cell_mid_y;
873  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
874  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
875  }
876  else if (!(r_min_y < cell_mid_y))
877  {
878  // cell_min_y = cell_mid_y;
879  // level_index |= 1;
880  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
881  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
882  }
883  else
884  {
885  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
886  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
887  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
888  intersect_rectangle_with_cells(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
889  }
890  }
891  }
892  else
893  {
894  ((my_cell_vector*)current_cells)->push_back(level_index);
895  }
896 }
897 
898 void LASquadtree::intersect_rectangle_with_cells_adaptive(const F64 r_min_x, const F64 r_min_y, const F64 r_max_x, const F64 r_max_y, const F32 cell_min_x, const F32 cell_max_x, const F32 cell_min_y, const F32 cell_max_y, U32 level, U32 level_index)
899 {
900  volatile float cell_mid_x;
901  volatile float cell_mid_y;
902  U32 cell_index = get_cell_index(level_index, level);
903  U32 adaptive_pos = cell_index/32;
904  U32 adaptive_bit = ((U32)1) << (cell_index%32);
905  if (adaptive[adaptive_pos] & adaptive_bit)
906  {
907  level++;
908  level_index <<= 2;
909 
910  cell_mid_x = (cell_min_x + cell_max_x)/2;
911  cell_mid_y = (cell_min_y + cell_max_y)/2;
912 
913  if (r_max_x < cell_mid_x)
914  {
915  // cell_max_x = cell_mid_x;
916  if (r_max_y < cell_mid_y)
917  {
918  // cell_max_y = cell_mid_y;
919  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
920  }
921  else if (!(r_min_y < cell_mid_y))
922  {
923  // cell_min_y = cell_mid_y;
924  // level_index |= 1;
925  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
926  }
927  else
928  {
929  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
930  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
931  }
932  }
933  else if (!(r_min_x < cell_mid_x))
934  {
935  // cell_min_x = cell_mid_x;
936  // level_index |= 1;
937  if (r_max_y < cell_mid_y)
938  {
939  // cell_max_y = cell_mid_y;
940  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
941  }
942  else if (!(r_min_y < cell_mid_y))
943  {
944  // cell_min_y = cell_mid_y;
945  // level_index |= 1;
946  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
947  }
948  else
949  {
950  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
951  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
952  }
953  }
954  else
955  {
956  if (r_max_y < cell_mid_y)
957  {
958  // cell_max_y = cell_mid_y;
959  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
960  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
961  }
962  else if (!(r_min_y < cell_mid_y))
963  {
964  // cell_min_y = cell_mid_y;
965  // level_index |= 1;
966  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
967  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
968  }
969  else
970  {
971  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
972  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
973  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
974  intersect_rectangle_with_cells_adaptive(r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
975  }
976  }
977  }
978  else
979  {
980  ((my_cell_vector*)current_cells)->push_back(cell_index);
981  }
982 }
983 
984 void LASquadtree::intersect_tile_with_cells(const F32 ll_x, const F32 ll_y, const F32 ur_x, const F32 ur_y, const F32 cell_min_x, const F32 cell_max_x, const F32 cell_min_y, const F32 cell_max_y, U32 level, U32 level_index)
985 {
986  volatile float cell_mid_x;
987  volatile float cell_mid_y;
988  if (level)
989  {
990  level--;
991  level_index <<= 2;
992 
993  cell_mid_x = (cell_min_x + cell_max_x)/2;
994  cell_mid_y = (cell_min_y + cell_max_y)/2;
995 
996  if (ur_x <= cell_mid_x)
997  {
998  // cell_max_x = cell_mid_x;
999  if (ur_y <= cell_mid_y)
1000  {
1001  // cell_max_y = cell_mid_y;
1002  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1003  }
1004  else if (!(ll_y < cell_mid_y))
1005  {
1006  // cell_min_y = cell_mid_y;
1007  // level_index |= 1;
1008  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1009  }
1010  else
1011  {
1012  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1013  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1014  }
1015  }
1016  else if (!(ll_x < cell_mid_x))
1017  {
1018  // cell_min_x = cell_mid_x;
1019  // level_index |= 1;
1020  if (ur_y <= cell_mid_y)
1021  {
1022  // cell_max_y = cell_mid_y;
1023  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1024  }
1025  else if (!(ll_y < cell_mid_y))
1026  {
1027  // cell_min_y = cell_mid_y;
1028  // level_index |= 1;
1029  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1030  }
1031  else
1032  {
1033  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1034  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1035  }
1036  }
1037  else
1038  {
1039  if (ur_y <= cell_mid_y)
1040  {
1041  // cell_max_y = cell_mid_y;
1042  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1043  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1044  }
1045  else if (!(ll_y < cell_mid_y))
1046  {
1047  // cell_min_y = cell_mid_y;
1048  // level_index |= 1;
1049  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1050  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1051  }
1052  else
1053  {
1054  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1055  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1056  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1057  intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1058  }
1059  }
1060  }
1061  else
1062  {
1063  ((my_cell_vector*)current_cells)->push_back(level_index);
1064  }
1065 }
1066 
1067 void LASquadtree::intersect_tile_with_cells_adaptive(const F32 ll_x, const F32 ll_y, const F32 ur_x, const F32 ur_y, const F32 cell_min_x, const F32 cell_max_x, const F32 cell_min_y, const F32 cell_max_y, U32 level, U32 level_index)
1068 {
1069  volatile float cell_mid_x;
1070  volatile float cell_mid_y;
1071  U32 cell_index = get_cell_index(level_index, level);
1072  U32 adaptive_pos = cell_index/32;
1073  U32 adaptive_bit = ((U32)1) << (cell_index%32);
1074  if (adaptive[adaptive_pos] & adaptive_bit)
1075  {
1076  level++;
1077  level_index <<= 2;
1078 
1079  cell_mid_x = (cell_min_x + cell_max_x)/2;
1080  cell_mid_y = (cell_min_y + cell_max_y)/2;
1081 
1082  if (ur_x <= cell_mid_x)
1083  {
1084  // cell_max_x = cell_mid_x;
1085  if (ur_y <= cell_mid_y)
1086  {
1087  // cell_max_y = cell_mid_y;
1088  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1089  }
1090  else if (!(ll_y < cell_mid_y))
1091  {
1092  // cell_min_y = cell_mid_y;
1093  // level_index |= 1;
1094  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1095  }
1096  else
1097  {
1098  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1099  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1100  }
1101  }
1102  else if (!(ll_x < cell_mid_x))
1103  {
1104  // cell_min_x = cell_mid_x;
1105  // level_index |= 1;
1106  if (ur_y <= cell_mid_y)
1107  {
1108  // cell_max_y = cell_mid_y;
1109  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1110  }
1111  else if (!(ll_y < cell_mid_y))
1112  {
1113  // cell_min_y = cell_mid_y;
1114  // level_index |= 1;
1115  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1116  }
1117  else
1118  {
1119  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1120  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1121  }
1122  }
1123  else
1124  {
1125  if (ur_y <= cell_mid_y)
1126  {
1127  // cell_max_y = cell_mid_y;
1128  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1129  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1130  }
1131  else if (!(ll_y < cell_mid_y))
1132  {
1133  // cell_min_y = cell_mid_y;
1134  // level_index |= 1;
1135  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1136  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1137  }
1138  else
1139  {
1140  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1141  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1142  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1143  intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1144  }
1145  }
1146  }
1147  else
1148  {
1149  ((my_cell_vector*)current_cells)->push_back(cell_index);
1150  }
1151 }
1152 
1153 void LASquadtree::intersect_circle_with_cells(const F64 center_x, const F64 center_y, const F64 radius, const F64 r_min_x, const F64 r_min_y, const F64 r_max_x, const F64 r_max_y, const F32 cell_min_x, const F32 cell_max_x, const F32 cell_min_y, const F32 cell_max_y, U32 level, U32 level_index)
1154 {
1155  volatile float cell_mid_x;
1156  volatile float cell_mid_y;
1157  if (level)
1158  {
1159  level--;
1160  level_index <<= 2;
1161 
1162  cell_mid_x = (cell_min_x + cell_max_x)/2;
1163  cell_mid_y = (cell_min_y + cell_max_y)/2;
1164 
1165  if (r_max_x < cell_mid_x)
1166  {
1167  // cell_max_x = cell_mid_x;
1168  if (r_max_y < cell_mid_y)
1169  {
1170  // cell_max_y = cell_mid_y;
1171  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1172  }
1173  else if (!(r_min_y < cell_mid_y))
1174  {
1175  // cell_min_y = cell_mid_y;
1176  // level_index |= 1;
1177  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1178  }
1179  else
1180  {
1181  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1182  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1183  }
1184  }
1185  else if (!(r_min_x < cell_mid_x))
1186  {
1187  // cell_min_x = cell_mid_x;
1188  // level_index |= 1;
1189  if (r_max_y < cell_mid_y)
1190  {
1191  // cell_max_y = cell_mid_y;
1192  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1193  }
1194  else if (!(r_min_y < cell_mid_y))
1195  {
1196  // cell_min_y = cell_mid_y;
1197  // level_index |= 1;
1198  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1199  }
1200  else
1201  {
1202  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1203  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1204  }
1205  }
1206  else
1207  {
1208  if (r_max_y < cell_mid_y)
1209  {
1210  // cell_max_y = cell_mid_y;
1211  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1212  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1213  }
1214  else if (!(r_min_y < cell_mid_y))
1215  {
1216  // cell_min_y = cell_mid_y;
1217  // level_index |= 1;
1218  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1219  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1220  }
1221  else
1222  {
1223  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1224  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1225  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1226  intersect_circle_with_cells(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1227  }
1228  }
1229  }
1230  else
1231  {
1232  if (intersect_circle_with_rectangle(center_x, center_y, radius, cell_min_x, cell_max_x, cell_min_y, cell_max_y))
1233  {
1234  ((my_cell_vector*)current_cells)->push_back(level_index);
1235  }
1236  }
1237 }
1238 
1239 void LASquadtree::intersect_circle_with_cells_adaptive(const F64 center_x, const F64 center_y, const F64 radius, const F64 r_min_x, const F64 r_min_y, const F64 r_max_x, const F64 r_max_y, const F32 cell_min_x, const F32 cell_max_x, const F32 cell_min_y, const F32 cell_max_y, U32 level, U32 level_index)
1240 {
1241  volatile float cell_mid_x;
1242  volatile float cell_mid_y;
1243  U32 cell_index = get_cell_index(level_index, level);
1244  U32 adaptive_pos = cell_index/32;
1245  U32 adaptive_bit = ((U32)1) << (cell_index%32);
1246  if (adaptive[adaptive_pos] & adaptive_bit)
1247  {
1248  level++;
1249  level_index <<= 2;
1250 
1251  cell_mid_x = (cell_min_x + cell_max_x)/2;
1252  cell_mid_y = (cell_min_y + cell_max_y)/2;
1253 
1254  if (r_max_x < cell_mid_x)
1255  {
1256  // cell_max_x = cell_mid_x;
1257  if (r_max_y < cell_mid_y)
1258  {
1259  // cell_max_y = cell_mid_y;
1260  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1261  }
1262  else if (!(r_min_y < cell_mid_y))
1263  {
1264  // cell_min_y = cell_mid_y;
1265  // level_index |= 1;
1266  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1267  }
1268  else
1269  {
1270  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1271  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1272  }
1273  }
1274  else if (!(r_min_x < cell_mid_x))
1275  {
1276  // cell_min_x = cell_mid_x;
1277  // level_index |= 1;
1278  if (r_max_y < cell_mid_y)
1279  {
1280  // cell_max_y = cell_mid_y;
1281  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1282  }
1283  else if (!(r_min_y < cell_mid_y))
1284  {
1285  // cell_min_y = cell_mid_y;
1286  // level_index |= 1;
1287  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1288  }
1289  else
1290  {
1291  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1292  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1293  }
1294  }
1295  else
1296  {
1297  if (r_max_y < cell_mid_y)
1298  {
1299  // cell_max_y = cell_mid_y;
1300  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1301  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1302  }
1303  else if (!(r_min_y < cell_mid_y))
1304  {
1305  // cell_min_y = cell_mid_y;
1306  // level_index |= 1;
1307  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1308  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1309  }
1310  else
1311  {
1312  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_min_y, cell_mid_y, level, level_index);
1313  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_min_y, cell_mid_y, level, level_index | 1);
1314  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_min_x, cell_mid_x, cell_mid_y, cell_max_y, level, level_index | 2);
1315  intersect_circle_with_cells_adaptive(center_x, center_y, radius, r_min_x, r_min_y, r_max_x, r_max_y, cell_mid_x, cell_max_x, cell_mid_y, cell_max_y, level, level_index | 3);
1316  }
1317  }
1318  }
1319  else
1320  {
1321  if (intersect_circle_with_rectangle(center_x, center_y, radius, cell_min_x, cell_max_x, cell_min_y, cell_max_y))
1322  {
1323  ((my_cell_vector*)current_cells)->push_back(cell_index);
1324  }
1325  }
1326 }
1327 
1328 BOOL LASquadtree::intersect_circle_with_rectangle(const F64 center_x, const F64 center_y, const F64 radius, const F32 r_min_x, const F32 r_max_x, const F32 r_min_y, const F32 r_max_y)
1329 {
1330  F64 r_diff_x, r_diff_y;
1331  F64 radius_squared = radius * radius;
1332  if (r_max_x < center_x) // R to left of circle center
1333  {
1334  r_diff_x = center_x - r_max_x;
1335  if (r_max_y < center_y) // R in lower left corner
1336  {
1337  r_diff_y = center_y - r_max_y;
1338  return ((r_diff_x * r_diff_x + r_diff_y * r_diff_y) < radius_squared);
1339  }
1340  else if (r_min_y > center_y) // R in upper left corner
1341  {
1342  r_diff_y = -center_y + r_min_y;
1343  return ((r_diff_x * r_diff_x + r_diff_y * r_diff_y) < radius_squared);
1344  }
1345  else // R due West of circle
1346  {
1347  return (r_diff_x < radius);
1348  }
1349  }
1350  else if (r_min_x > center_x) // R to right of circle center
1351  {
1352  r_diff_x = -center_x + r_min_x;
1353  if (r_max_y < center_y) // R in lower right corner
1354  {
1355  r_diff_y = center_y - r_max_y;
1356  return ((r_diff_x * r_diff_x + r_diff_y * r_diff_y) < radius_squared);
1357  }
1358  else if (r_min_y > center_y) // R in upper right corner
1359  {
1360  r_diff_y = -center_y + r_min_y;
1361  return ((r_diff_x * r_diff_x + r_diff_y * r_diff_y) < radius_squared);
1362  }
1363  else // R due East of circle
1364  {
1365  return (r_diff_x < radius);
1366  }
1367  }
1368  else // R on circle vertical centerline
1369  {
1370  if (r_max_y < center_y) // R due South of circle
1371  {
1372  r_diff_y = center_y - r_max_y;
1373  return (r_diff_y < radius);
1374  }
1375  else if (r_min_y > center_y) // R due North of circle
1376  {
1377  r_diff_y = -center_y + r_min_y;
1378  return (r_diff_y < radius);
1379  }
1380  else // R contains circle centerpoint
1381  {
1382  return TRUE;
1383  }
1384  }
1385 }
1386 
1388 {
1389  intersect_rectangle(min_x, min_y, max_x, max_y);
1390  return get_intersected_cells();
1391 }
1392 
1394 {
1395  next_cell_index = 0;
1396  if (current_cells == 0)
1397  {
1398  return FALSE;
1399  }
1400  if (((my_cell_vector*)current_cells)->size() == 0)
1401  {
1402  return FALSE;
1403  }
1404  return TRUE;
1405 }
1406 
1408 {
1409  if (current_cells == 0)
1410  {
1411  return FALSE;
1412  }
1413  if (next_cell_index >= ((my_cell_vector*)current_cells)->size())
1414  {
1415  return FALSE;
1416  }
1417  if (adaptive)
1418  {
1419  current_cell = ((my_cell_vector*)current_cells)->at(next_cell_index);
1420  }
1421  else
1422  {
1423  current_cell = level_offset[levels] + ((my_cell_vector*)current_cells)->at(next_cell_index);
1424  }
1425  next_cell_index++;
1426  return TRUE;
1427 }
1428 
1429 BOOL LASquadtree::setup(F64 bb_min_x, F64 bb_max_x, F64 bb_min_y, F64 bb_max_y, F32 cell_size)
1430 {
1431  this->cell_size = cell_size;
1432  this->sub_level = 0;
1433  this->sub_level_index = 0;
1434 
1435  // enlarge bounding box to units of cells
1436  if (bb_min_x >= 0) min_x = cell_size*((I32)(bb_min_x/cell_size));
1437  else min_x = cell_size*((I32)(bb_min_x/cell_size)-1);
1438  if (bb_max_x >= 0) max_x = cell_size*((I32)(bb_max_x/cell_size)+1);
1439  else max_x = cell_size*((I32)(bb_max_x/cell_size));
1440  if (bb_min_y >= 0) min_y = cell_size*((I32)(bb_min_y/cell_size));
1441  else min_y = cell_size*((I32)(bb_min_y/cell_size)-1);
1442  if (bb_max_y >= 0) max_y = cell_size*((I32)(bb_max_y/cell_size)+1);
1443  else max_y = cell_size*((I32)(bb_max_y/cell_size));
1444 
1445  // how many cells minimally in each direction
1446  cells_x = U32_QUANTIZE((max_x - min_x)/cell_size);
1447  cells_y = U32_QUANTIZE((max_y - min_y)/cell_size);
1448 
1449  if (cells_x == 0 || cells_y == 0)
1450  {
1451  fprintf(stderr, "ERROR: cells_x %d cells_y %d\n", cells_x, cells_y);
1452  return FALSE;
1453  }
1454 
1455  // how many quad tree levels to get to that many cells
1456  U32 c = ((cells_x > cells_y) ? cells_x - 1 : cells_y - 1);
1457  levels = 0;
1458  while (c)
1459  {
1460  c = c >> 1;
1461  levels++;
1462  }
1463 
1464  // enlarge bounding box to quad tree size
1465  U32 c1, c2;
1466  c = (1 << levels) - cells_x;
1467  c1 = c/2;
1468  c2 = c - c1;
1469  min_x -= (c2 * cell_size);
1470  max_x += (c1 * cell_size);
1471  c = (1 << levels) - cells_y;
1472  c1 = c/2;
1473  c2 = c - c1;
1474  min_y -= (c2 * cell_size);
1475  max_y += (c1 * cell_size);
1476 
1477  return TRUE;
1478 }
1479 
1480 BOOL LASquadtree::subtiling_setup(F32 min_x, F32 max_x, F32 min_y, F32 max_y, U32 sub_level, U32 sub_level_index, U32 levels)
1481 {
1482  this->min_x = min_x;
1483  this->max_x = max_x;
1484  this->min_y = min_y;
1485  this->max_y = max_y;
1486  F32 min[2];
1487  F32 max[2];
1488  get_cell_bounding_box(sub_level_index, sub_level, min, max);
1489  this->min_x = min[0];
1490  this->max_x = max[0];
1491  this->min_y = min[1];
1492  this->max_y = max[1];
1493  this->sub_level = sub_level;
1494  this->sub_level_index = sub_level_index;
1495  this->levels = levels;
1496  return TRUE;
1497 }
1498 
1500 {
1501  U32 l;
1502  levels = 0;
1503  cell_size = 0;
1504  min_x = 0;
1505  max_x = 0;
1506  min_y = 0;
1507  max_y = 0;
1508  cells_x = 0;
1509  cells_y = 0;
1510  sub_level = 0;
1511  level_offset[0] = 0;
1512  for (l = 0; l < 23; l++)
1513  {
1514  level_offset[l+1] = level_offset[l] + ((1<<l)*(1<<l));
1515  }
1516  current_cells = 0;
1517  adaptive_alloc = 0;
1518  adaptive = 0;
1519 }
1520 
1522 {
1523  if (current_cells) delete ((my_cell_vector*)current_cells);
1524 }
LASquadtree::intersect_rectangle_with_cells
void intersect_rectangle_with_cells(const F64 r_min_x, const F64 r_min_y, const F64 r_max_x, const F64 r_max_y, const F32 cell_min_x, const F32 cell_max_x, const F32 cell_min_y, const F32 cell_max_y, U32 level, U32 level_index)
Definition: lasquadtree.cpp:815
LASquadtree::manage_cell
BOOL manage_cell(const U32 cell_index, const BOOL finalize=FALSE)
Definition: lasquadtree.cpp:659
LASquadtree::intersect_tile_with_cells_adaptive
void intersect_tile_with_cells_adaptive(const F32 ll_x, const F32 ll_y, const F32 ur_x, const F32 ur_y, const F32 cell_min_x, const F32 cell_max_x, const F32 cell_min_y, const F32 cell_max_y, U32 level, U32 level_index)
Definition: lasquadtree.cpp:1067
U32_QUANTIZE
#define U32_QUANTIZE(n)
Definition: mydefs.hpp:112
LASquadtree::intersect_circle_with_cells
void intersect_circle_with_cells(const F64 center_x, const F64 center_y, const F64 radius, const F64 r_min_x, const F64 r_min_y, const F64 r_max_x, const F64 r_max_y, const F32 cell_min_x, const F32 cell_max_x, const F32 cell_min_y, const F32 cell_max_y, U32 level, U32 level_index)
Definition: lasquadtree.cpp:1153
LASquadtree::read
BOOL read(ByteStreamIn *stream)
Definition: lasquadtree.cpp:518
LASquadtree::intersect_circle_with_cells_adaptive
void intersect_circle_with_cells_adaptive(const F64 center_x, const F64 center_y, const F64 radius, const F64 r_min_x, const F64 r_min_y, const F64 r_max_x, const F64 r_max_y, const F32 cell_min_x, const F32 cell_max_x, const F32 cell_min_y, const F32 cell_max_y, U32 level, U32 level_index)
Definition: lasquadtree.cpp:1239
LASquadtree::has_more_cells
BOOL has_more_cells()
Definition: lasquadtree.cpp:1407
F64
double F64
Definition: mydefs.hpp:52
LASquadtree::get_level_index
U32 get_level_index(const F64 x, const F64 y, U32 level) const
Definition: lasquadtree.cpp:217
LASquadtree::get_all_cells
BOOL get_all_cells()
Definition: lasquadtree.cpp:1387
I32
int I32
Definition: mydefs.hpp:35
LASquadtree::setup
BOOL setup(F64 bb_min_x, F64 bb_max_x, F64 bb_min_y, F64 bb_max_y, F32 cell_size=1000.0f)
Definition: lasquadtree.cpp:1429
TRUE
#define TRUE
Definition: mydefs.hpp:137
LASquadtree::intersect_tile
U32 intersect_tile(const F32 ll_x, const F32 ll_y, const F32 size)
Definition: lasquadtree.cpp:772
LASquadtree::write
BOOL write(ByteStreamOut *stream) const
Definition: lasquadtree.cpp:591
LASquadtree::intersect_rectangle
U32 intersect_rectangle(const F64 r_min_x, const F64 r_min_y, const F64 r_max_x, const F64 r_max_y)
Definition: lasquadtree.cpp:736
LASquadtree::get_level
U32 get_level(U32 cell_index) const
Definition: lasquadtree.cpp:390
ByteStreamOut::put32bitsLE
virtual BOOL put32bitsLE(const U8 *bytes)=0
LASquadtree::coarsen
BOOL coarsen(const I32 cell_index, I32 *coarser_cell_index, U32 *num_cell_indices, I32 **cell_indices) const
Definition: lasquadtree.cpp:349
LASquadtree::LASquadtree
LASquadtree()
Definition: lasquadtree.cpp:1499
LASquadtree::subtiling_setup
BOOL subtiling_setup(F32 min_x, F32 max_x, F32 min_y, F32 max_y, U32 sub_level, U32 sub_level_index, U32 levels)
Definition: lasquadtree.cpp:1480
ByteStreamOut
Definition: bytestreamout.hpp:36
bytestreamin.hpp
LASquadtree::intersect_circle
U32 intersect_circle(const F64 center_x, const F64 center_y, const F64 radius)
Definition: lasquadtree.cpp:810
my_cell_vector
vector< I32 > my_cell_vector
Definition: lasquadtree.cpp:39
ByteStreamIn::get32bitsLE
virtual void get32bitsLE(U8 *bytes)=0
LASquadtree::inside
BOOL inside(const F64 x, const F64 y) const
Definition: lasquadtree.cpp:696
bytestreamout.hpp
ByteStreamIn
Definition: bytestreamin.hpp:36
LASquadtree::intersect_tile_with_cells
void intersect_tile_with_cells(const F32 ll_x, const F32 ll_y, const F32 ur_x, const F32 ur_y, const F32 cell_min_x, const F32 cell_max_x, const F32 cell_min_y, const F32 cell_max_y, U32 level, U32 level_index)
Definition: lasquadtree.cpp:984
lasquadtree.hpp
LASquadtree::get_max_cell_index
U32 get_max_cell_index() const
Definition: lasquadtree.cpp:435
LASquadtree::raster_occupancy
U32 * raster_occupancy(BOOL(*does_cell_exist)(I32), U32 level) const
Definition: lasquadtree.cpp:498
U8
unsigned char U8
Definition: mydefs.hpp:41
BOOL
int BOOL
Definition: mydefs.hpp:57
FALSE
#define FALSE
Definition: mydefs.hpp:133
F32
float F32
Definition: mydefs.hpp:51
LASquadtree::get_cell_index
U32 get_cell_index(const F64 x, const F64 y) const
Definition: lasquadtree.cpp:343
std
Definition: HalfEdge.hpp:124
LASquadtree::get_cell_bounding_box
void get_cell_bounding_box(const I32 cell_index, F32 *min, F32 *max) const
Definition: lasquadtree.cpp:209
LASquadtree::get_intersected_cells
BOOL get_intersected_cells()
Definition: lasquadtree.cpp:1393
U32
unsigned int U32
Definition: mydefs.hpp:39
LASquadtree::get_max_level_index
U32 get_max_level_index() const
Definition: lasquadtree.cpp:423
LASquadtree::intersect_circle_with_rectangle
BOOL intersect_circle_with_rectangle(const F64 center_x, const F64 center_y, const F64 radius, const F32 r_min_x, const F32 r_max_x, const F32 r_min_y, const F32 r_max_y)
Definition: lasquadtree.cpp:1328
LASquadtree::intersect_rectangle_with_cells_adaptive
void intersect_rectangle_with_cells_adaptive(const F64 r_min_x, const F64 r_min_y, const F64 r_max_x, const F64 r_max_y, const F32 cell_min_x, const F32 cell_max_x, const F32 cell_min_y, const F32 cell_max_y, U32 level, U32 level_index)
Definition: lasquadtree.cpp:898
ByteStreamIn::getBytes
virtual void getBytes(U8 *bytes, const U32 num_bytes)=0
LASquadtree::~LASquadtree
~LASquadtree()
Definition: lasquadtree.cpp:1521


lvr2
Author(s): Thomas Wiemann , Sebastian Pütz , Alexander Mock , Lars Kiesow , Lukas Kalbertodt , Tristan Igelbrink , Johan M. von Behren , Dominik Feldschnieders , Alexander Löhr
autogenerated on Wed Mar 2 2022 00:37:23