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;
115 cell_mid_x = (cell_min_x + cell_max_x)/2;
116 cell_mid_y = (cell_min_y + cell_max_y)/2;
119 cell_max_x = cell_mid_x;
123 cell_min_x = cell_mid_x;
127 cell_max_y = cell_mid_y;
131 cell_min_y = cell_mid_y;
150 get_cell_bounding_box(x, y, levels, min, max);
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;
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;
174 cell_min_x = cell_mid_x;
178 cell_max_x = cell_mid_x;
182 cell_min_y = cell_mid_y;
186 cell_max_y = cell_mid_y;
205 get_cell_bounding_box(level_index, levels, min, max);
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);
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;
235 cell_mid_x = (cell_min_x + cell_max_x)/2;
236 cell_mid_y = (cell_min_y + cell_max_y)/2;
240 cell_max_x = cell_mid_x;
244 cell_min_x = cell_mid_x;
249 cell_max_y = cell_mid_y;
253 cell_min_y = cell_mid_y;
265 return get_level_index(x, y, levels);
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;
287 cell_mid_x = (cell_min_x + cell_max_x)/2;
288 cell_mid_y = (cell_min_y + cell_max_y)/2;
292 cell_max_x = cell_mid_x;
296 cell_min_x = cell_mid_x;
301 cell_max_y = cell_mid_y;
305 cell_min_y = cell_mid_y;
326 return get_level_index(x, y, levels, min, max);
334 return level_offset[sub_level+level] + (sub_level_index << (level*2)) + get_level_index(x, y, level);
338 return level_offset[level]+get_level_index(x, y, level);
345 return get_cell_index(x, y, levels);
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)
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);
375 return cell_index - (sub_level_index << (level*2)) - level_offset[sub_level+level];
379 return cell_index - level_offset[level];
386 return get_level_index(cell_index, levels);
393 while (cell_index >= level_offset[level+1]) level++;
402 return level_index + (sub_level_index << (level*2)) + level_offset[sub_level+level];
406 return level_index + level_offset[level];
413 return get_cell_index(level_index, levels);
419 return (1<<level)*(1<<level);
425 return get_max_level_index(levels);
431 return level_offset[level+1]-1;
437 return get_max_cell_index(levels);
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);
447 if (adaptive[adaptive_pos] & adaptive_bit)
449 if (level < stop_level)
453 U32 size = 1 << (stop_level-level);
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);
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++)
469 pos = pos_y*full_size + min_x;
470 for (pos_x = 0; pos_x < size; pos_x++)
472 data[pos/32] |= (1<<(pos%32));
478 else if (does_cell_exist(cell_index))
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++)
487 pos = pos_y*full_size + min_x;
488 for (pos_x = 0; pos_x < size; pos_x++)
490 data[pos/32] |= (1<<(pos%32));
501 U32 size = (1<<level);
502 U32* data =
new U32[size*size/32];
503 for (pos = 0; pos < (size*size/32); pos++)
507 raster_occupancy(does_cell_exist, data, 0, 0, 0, 0, level);
514 return raster_occupancy(does_cell_exist, levels);
531 try { stream->
getBytes((
U8*)signature, 4); }
catch(...)
533 fprintf(stderr,
"ERROR (LASquadtree): reading signature\n");
536 if (strncmp(signature,
"LASQ", 4) != 0)
540 levels = ((
U32*)signature)[0];
547 fprintf(stderr,
"ERROR (LASquadtree): reading version\n");
552 fprintf(stderr,
"ERROR (LASquadtree): reading levels\n");
559 fprintf(stderr,
"ERROR (LASquadtree): reading level_index\n");
563 try { stream->
get32bitsLE((
U8*)&implicit_levels); }
catch(...)
565 fprintf(stderr,
"ERROR (LASquadtree): reading implicit_levels\n");
570 fprintf(stderr,
"ERROR (LASquadtree): reading min_x\n");
575 fprintf(stderr,
"ERROR (LASquadtree): reading max_x\n");
580 fprintf(stderr,
"ERROR (LASquadtree): reading min_y\n");
585 fprintf(stderr,
"ERROR (LASquadtree): reading max_y\n");
620 fprintf(stderr,
"ERROR (LASquadtree): writing levels %u\n", levels);
626 fprintf(stderr,
"ERROR (LASquadtree): writing level_index %u\n", level_index);
629 U32 implicit_levels = 0;
632 fprintf(stderr,
"ERROR (LASquadtree): writing implicit_levels %u\n", implicit_levels);
637 fprintf(stderr,
"ERROR (LASquadtree): writing min_x %g\n", min_x);
642 fprintf(stderr,
"ERROR (LASquadtree): writing max_x %g\n", max_x);
647 fprintf(stderr,
"ERROR (LASquadtree): writing min_y %g\n", min_y);
652 fprintf(stderr,
"ERROR (LASquadtree): writing max_y %g\n", max_y);
661 U32 adaptive_pos = cell_index/32;
662 U32 adaptive_bit = ((
U32)1) << (cell_index%32);
663 if (adaptive_pos >= adaptive_alloc)
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;
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;
678 adaptive[adaptive_pos] &= ~adaptive_bit;
680 U32 level = get_level(cell_index);
681 U32 level_index = get_level_index(cell_index, 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;
700 return (min_x <= x && x < max_x && min_y <= y && y < max_y);
704 return (min_x <= x && x <= max_x && min_y <= y && y <= max_y);
710 if (current_cells == 0)
719 if (r_max_x < min_x || !(r_min_x < max_x) || r_max_y < min_y || !(r_min_y < max_y))
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);
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);
738 return intersect_rectangle(r_min_x, r_min_y, r_max_x, r_max_y, levels);
743 if (current_cells == 0)
752 volatile F32 ur_x = ll_x + size;
753 volatile F32 ur_y = ll_y + size;
755 if (ur_x <= min_x || !(ll_x <= max_x) || ur_y <= min_y || !(ll_y <= max_y))
762 intersect_tile_with_cells_adaptive(ll_x, ll_y, ur_x, ur_y, min_x, max_x, min_y, max_y, 0, 0);
766 intersect_tile_with_cells(ll_x, ll_y, ur_x, ur_y, min_x, max_x, min_y, max_y, level, 0);
774 return intersect_tile(ll_x, ll_y, size, levels);
779 if (current_cells == 0)
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;
793 if (r_max_x < min_x || !(r_min_x < max_x) || r_max_y < min_y || !(r_min_y < max_y))
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);
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);
812 return intersect_circle(center_x, center_y, radius, levels);
817 volatile float cell_mid_x;
818 volatile float cell_mid_y;
824 cell_mid_x = (cell_min_x + cell_max_x)/2;
825 cell_mid_y = (cell_min_y + cell_max_y)/2;
827 if (r_max_x < cell_mid_x)
830 if (r_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);
835 else if (!(r_min_y < cell_mid_y))
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);
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);
847 else if (!(r_min_x < cell_mid_x))
851 if (r_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);
856 else if (!(r_min_y < cell_mid_y))
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);
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);
870 if (r_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);
876 else if (!(r_min_y < cell_mid_y))
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);
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);
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)
910 cell_mid_x = (cell_min_x + cell_max_x)/2;
911 cell_mid_y = (cell_min_y + cell_max_y)/2;
913 if (r_max_x < cell_mid_x)
916 if (r_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);
921 else if (!(r_min_y < cell_mid_y))
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);
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);
933 else if (!(r_min_x < cell_mid_x))
937 if (r_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);
942 else if (!(r_min_y < cell_mid_y))
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);
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);
956 if (r_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);
962 else if (!(r_min_y < cell_mid_y))
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);
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);
986 volatile float cell_mid_x;
987 volatile float cell_mid_y;
993 cell_mid_x = (cell_min_x + cell_max_x)/2;
994 cell_mid_y = (cell_min_y + cell_max_y)/2;
996 if (ur_x <= cell_mid_x)
999 if (ur_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);
1004 else if (!(ll_y < cell_mid_y))
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);
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);
1016 else if (!(ll_x < cell_mid_x))
1020 if (ur_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);
1025 else if (!(ll_y < cell_mid_y))
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);
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);
1039 if (ur_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);
1045 else if (!(ll_y < cell_mid_y))
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);
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);
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)
1079 cell_mid_x = (cell_min_x + cell_max_x)/2;
1080 cell_mid_y = (cell_min_y + cell_max_y)/2;
1082 if (ur_x <= cell_mid_x)
1085 if (ur_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);
1090 else if (!(ll_y < cell_mid_y))
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);
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);
1102 else if (!(ll_x < cell_mid_x))
1106 if (ur_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);
1111 else if (!(ll_y < cell_mid_y))
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);
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);
1125 if (ur_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);
1131 else if (!(ll_y < cell_mid_y))
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);
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);
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)
1155 volatile float cell_mid_x;
1156 volatile float cell_mid_y;
1162 cell_mid_x = (cell_min_x + cell_max_x)/2;
1163 cell_mid_y = (cell_min_y + cell_max_y)/2;
1165 if (r_max_x < cell_mid_x)
1168 if (r_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);
1173 else if (!(r_min_y < cell_mid_y))
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);
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);
1185 else if (!(r_min_x < cell_mid_x))
1189 if (r_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);
1194 else if (!(r_min_y < cell_mid_y))
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);
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);
1208 if (r_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);
1214 else if (!(r_min_y < cell_mid_y))
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);
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);
1232 if (intersect_circle_with_rectangle(center_x, center_y, radius, cell_min_x, cell_max_x, cell_min_y, cell_max_y))
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)
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)
1251 cell_mid_x = (cell_min_x + cell_max_x)/2;
1252 cell_mid_y = (cell_min_y + cell_max_y)/2;
1254 if (r_max_x < cell_mid_x)
1257 if (r_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);
1262 else if (!(r_min_y < cell_mid_y))
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);
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);
1274 else if (!(r_min_x < cell_mid_x))
1278 if (r_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);
1283 else if (!(r_min_y < cell_mid_y))
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);
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);
1297 if (r_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);
1303 else if (!(r_min_y < cell_mid_y))
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);
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);
1321 if (intersect_circle_with_rectangle(center_x, center_y, radius, cell_min_x, cell_max_x, cell_min_y, cell_max_y))
1330 F64 r_diff_x, r_diff_y;
1331 F64 radius_squared = radius * radius;
1332 if (r_max_x < center_x)
1334 r_diff_x = center_x - r_max_x;
1335 if (r_max_y < center_y)
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);
1340 else if (r_min_y > center_y)
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);
1347 return (r_diff_x < radius);
1350 else if (r_min_x > center_x)
1352 r_diff_x = -center_x + r_min_x;
1353 if (r_max_y < center_y)
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);
1358 else if (r_min_y > center_y)
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);
1365 return (r_diff_x < radius);
1370 if (r_max_y < center_y)
1372 r_diff_y = center_y - r_max_y;
1373 return (r_diff_y < radius);
1375 else if (r_min_y > center_y)
1377 r_diff_y = -center_y + r_min_y;
1378 return (r_diff_y < radius);
1389 intersect_rectangle(min_x, min_y, max_x, max_y);
1390 return get_intersected_cells();
1395 next_cell_index = 0;
1396 if (current_cells == 0)
1409 if (current_cells == 0)
1419 current_cell = ((
my_cell_vector*)current_cells)->at(next_cell_index);
1423 current_cell = level_offset[levels] + ((
my_cell_vector*)current_cells)->at(next_cell_index);
1431 this->cell_size = cell_size;
1432 this->sub_level = 0;
1433 this->sub_level_index = 0;
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));
1449 if (cells_x == 0 || cells_y == 0)
1451 fprintf(stderr,
"ERROR: cells_x %d cells_y %d\n", cells_x, cells_y);
1456 U32 c = ((cells_x > cells_y) ? cells_x - 1 : cells_y - 1);
1466 c = (1 << levels) - cells_x;
1469 min_x -= (c2 * cell_size);
1470 max_x += (c1 * cell_size);
1471 c = (1 << levels) - cells_y;
1474 min_y -= (c2 * cell_size);
1475 max_y += (c1 * cell_size);
1482 this->min_x = min_x;
1483 this->max_x = max_x;
1484 this->min_y = min_y;
1485 this->max_y = max_y;
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;
1511 level_offset[0] = 0;
1512 for (l = 0; l < 23; l++)
1514 level_offset[l+1] = level_offset[l] + ((1<<l)*(1<<l));