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