88 integer :: object_index = -1
116 generic ::
operator(.lt.) => less
117 generic ::
operator(.gt.) => greater
129 integer :: allocated_node_count = 0
131 integer :: node_capacity = 0
132 integer :: growth_size = 1
185 this%object_index = -1
206 integer :: object_index
208 object_index = this%object_index
214 integer :: parent_index
216 parent_index = this%parent_node_index
222 integer :: left_index
224 left_index = this%left_node_index
230 integer :: right_index
232 right_index = this%right_node_index
238 real(kind=
dp),
dimension(3),
intent(in) :: p
239 real(kind=
dp) :: distance
241 distance = 0.5_rp * this%aabb%get_diameter() &
242 - norm2(this%aabb%get_center() - p)
262 if (this%is_leaf())
then
266 & this%object_index .gt. 0
271 & this%object_index .eq. -1
285 res = this%aabb .lt. other%aabb
295 res = this%aabb .gt. other%aabb
306 integer,
intent(in) :: initial_capacity
311 this%allocated_node_count = 0
312 this%next_free_node_index = 1
313 this%node_capacity = initial_capacity
314 this%growth_size = initial_capacity
316 if (
allocated(this%nodes))
deallocate(this%nodes)
317 allocate(this%nodes(initial_capacity))
319 do i = 1, initial_capacity
320 this%nodes(i)%next_node_index = i + 1
331 class(*),
dimension(:),
intent(in) :: objects
333 integer :: i_obj, i_node, i
336 integer :: start_layer, end_layer
338 type(
aabb_t),
dimension(:),
allocatable :: box_list
339 integer,
dimension(:),
allocatable :: sorted_indices
341 call this%init(
size(objects) * 2)
347 allocate(box_list(
size(objects)))
349 do i_obj = 1,
size(objects)
350 box_list(i_obj) =
get_aabb(objects(i_obj))
352 sorted_indices =
sort(box_list)
354 do i = 1,
size(sorted_indices)
355 i_obj = sorted_indices(i)
356 i_node = this%allocate_node()
357 this%nodes(i_node)%aabb =
get_aabb(objects(i_obj))
358 this%nodes(i_node)%object_index = i_obj
363 end_layer =
size(objects)
365 do while (.not. done)
368 do i = start_layer, end_layer - 1, 2
369 i_node = this%allocate_node()
371 this%nodes(i_node)%aabb =
merge(this%nodes(i)%aabb, this%nodes(i + 1)%aabb)
373 this%nodes(i_node)%left_node_index = i
374 this%nodes(i_node)%right_node_index = i + 1
376 this%nodes(i)%parent_node_index = i_node
377 this%nodes(i + 1)%parent_node_index = i_node
382 if (mod(end_layer - start_layer, 2) .eq. 0)
then
383 i_node = this%allocate_node()
384 this%nodes(i_node)%aabb = this%nodes(end_layer)%aabb
385 this%nodes(i_node)%left_node_index = end_layer
388 this%nodes(end_layer)%parent_node_index = i_node
392 start_layer = end_layer + 1
393 end_layer = this%allocated_node_count
396 done = start_layer .eq. end_layer
400 this%root_node_index = this%allocated_node_count
402 if (this%get_size() .ne.
size(objects))
then
403 print *,
"this%get_size() = ", this%get_size()
404 print *,
"size(objects) = ",
size(objects)
410 function sort(array)
result(indices)
411 type(
aabb_t),
dimension(:),
intent(in) :: array
412 integer,
dimension(:),
allocatable :: indices
413 logical,
dimension(:),
allocatable :: visited
418 allocate(indices(
size(array)))
419 allocate(visited(
size(array)))
423 do i = 1,
size(array)
425 do imin = 1,
size(array)
426 if (.not. visited(imin) .and. minidx .eq. -1) minidx = imin
428 if (visited(imin) .and. array(imin) .lt. array(minidx)) minidx = imin
432 visited(minidx) = .true.
450 call simple_stack%init(this%allocated_node_count)
452 tmp = this%get_root_index()
454 call simple_stack%push(tmp)
457 do while (.not. simple_stack%is_empty())
458 idx = simple_stack%pop()
461 if (this%nodes(idx)%is_leaf())
then
464 tmp = this%get_left_index(idx)
465 call simple_stack%push(tmp)
466 tmp = this%get_right_index(idx)
467 call simple_stack%push(tmp)
479 integer :: root_index
481 root_index = this%root_node_index
487 integer,
intent(in) :: node_index
488 integer :: parent_index
490 parent_index = this%nodes(node_index)%parent_node_index
496 integer,
intent(in) :: node_index
497 integer :: left_index
499 left_index = this%nodes(node_index)%left_node_index
505 integer,
intent(in) :: node_index
506 integer :: right_index
508 right_index = this%nodes(node_index)%right_node_index
517 integer,
intent(in) :: node_index
520 node = this%nodes(node_index)
528 root_node = this%nodes(this%root_node_index)
534 integer,
intent(in) :: node_index
537 parent_node = this%nodes(this%nodes(node_index)%parent_node_index)
543 integer,
intent(in) :: node_index
546 left_node = this%nodes(this%nodes(node_index)%left_node_index)
552 integer,
intent(in) :: node_index
555 right_node = this%nodes(this%nodes(node_index)%right_node_index)
560 integer,
intent(in) :: node_index
563 out_box = this%nodes(node_index)%aabb
571 class(*),
intent(in) :: object
572 integer,
intent(in) :: object_index
574 integer :: node_index
576 node_index = this%allocate_node()
577 this%nodes(node_index)%aabb =
get_aabb(object)
578 this%nodes(node_index)%object_index = object_index
580 call this%insert_leaf(node_index)
589 class(*),
intent(in) :: object
590 integer,
intent(in) :: object_index
591 integer,
intent(out) :: overlaps(:)
594 type(
aabb_t) :: object_box
596 integer :: root_index, left_index, right_index
598 integer :: node_index
601 root_index = this%get_root_index()
603 call simple_stack%push(root_index)
605 do while (.not. simple_stack%is_empty())
606 node_index = simple_stack%pop()
610 if (this%nodes(node_index)%aabb%overlaps(object_box))
then
611 if (this%nodes(node_index)%is_leaf())
then
612 if (.not. this%nodes(node_index)%object_index == object_index)
then
613 overlaps = [this%nodes(node_index)%object_index, overlaps]
616 left_index = this%get_left_index(node_index)
617 call simple_stack%push(left_index)
618 right_index = this%get_right_index(node_index)
619 call simple_stack%push(right_index)
631 integer :: node_index
634 call this%resize_node_pool(this%node_capacity + this%growth_size)
637 node_index = this%next_free_node_index
639 associate(new_node => this%nodes(node_index))
640 this%next_free_node_index = new_node%next_node_index
646 this%next_free_node_index = new_node%next_node_index
647 this%allocated_node_count = this%allocated_node_count + 1
655 integer,
intent(in) :: node_index
657 this%nodes(node_index)%next_node_index = this%next_free_node_index
658 this%next_free_node_index = node_index
659 this%allocated_node_count = this%allocated_node_count - 1
665 integer,
intent(in) :: leaf_node_index
667 integer :: tree_node_index
669 real(kind=rp) :: cost_left
670 real(kind=rp) :: cost_right
677 type(aabb_t) :: combined_aabb
678 real(kind=rp) :: new_parent_node_cost
679 real(kind=rp) :: minimum_push_down_cost
680 type(aabb_t) :: new_left_aabb
681 type(aabb_t) :: new_right_aabb
682 integer :: leaf_sibling_index
684 integer :: old_parent_index
685 integer :: new_parent_index
690 leaf_node = this%nodes(leaf_node_index)
694 this%root_node_index = leaf_node_index
704 tree_node_index = this%root_node_index
705 tree_node = this%get_node(tree_node_index)
706 do while (.not. tree_node%is_leaf())
710 left_node = this%get_left_node(tree_node_index)
711 right_node = this%get_right_node(tree_node_index)
715 combined_aabb = merge(tree_node%aabb, leaf_node%get_aabb())
717 new_parent_node_cost = 2.0_rp * combined_aabb%get_surface_area()
718 minimum_push_down_cost = 2.0_rp * ( &
719 & combined_aabb%get_surface_area() &
720 & - tree_node%aabb%get_surface_area()&
725 if (left_node%is_leaf())
then
726 new_left_aabb = merge(leaf_node%aabb, left_node%get_aabb())
727 cost_left = new_left_aabb%get_surface_area() + minimum_push_down_cost
729 new_left_aabb = merge(leaf_node%aabb, left_node%get_aabb())
731 & new_left_aabb%get_surface_area() &
732 & - left_node%aabb%get_surface_area()&
733 & ) + minimum_push_down_cost
736 if (right_node%is_leaf())
then
737 new_right_aabb = merge(leaf_node%aabb, right_node%aabb)
738 cost_right = new_right_aabb%get_surface_area() + minimum_push_down_cost
740 new_right_aabb = merge(leaf_node%aabb, right_node%aabb)
742 & new_right_aabb%get_surface_area() &
743 & - right_node%aabb%get_surface_area() &
744 & ) + minimum_push_down_cost
750 if (new_parent_node_cost < cost_left .and. new_parent_node_cost < cost_right)
then
755 if (cost_left .lt. cost_right)
then
756 tree_node_index = tree_node%get_left_index()
758 tree_node_index = tree_node%get_right_index()
763 tree_node = this%get_node(tree_node_index)
768 leaf_sibling_index = tree_node_index
769 leaf_sibling = this%nodes(leaf_sibling_index)
770 old_parent_index = this%get_parent_index(leaf_sibling_index)
771 new_parent_index = this%allocate_node()
772 new_parent = this%nodes(new_parent_index)
773 new_parent%parent_node_index = old_parent_index
774 new_parent%aabb = merge(leaf_node%aabb, leaf_sibling%aabb)
776 if (leaf_node .lt. leaf_sibling)
then
777 new_parent%left_node_index = leaf_node_index
778 new_parent%right_node_index = leaf_sibling_index
780 new_parent%left_node_index = leaf_sibling_index
781 new_parent%right_node_index = leaf_node_index
784 leaf_node%parent_node_index = new_parent_index
785 leaf_sibling%parent_node_index = new_parent_index
789 this%root_node_index = new_parent_index
793 old_parent = this%nodes(old_parent_index)
794 if (old_parent%left_node_index .eq. leaf_sibling_index)
then
795 old_parent%left_node_index = new_parent_index
797 old_parent%right_node_index = new_parent_index
799 this%nodes(old_parent_index) = old_parent
802 this%nodes(leaf_node_index) = leaf_node
803 this%nodes(leaf_sibling_index) = leaf_sibling
804 this%nodes(new_parent_index) = new_parent
807 tree_node_index = leaf_node%parent_node_index
809 call this%fix_upwards_tree(tree_node_index)
822 integer :: current_index
823 integer :: root_index, left_index, right_index
830 root_index = this%get_root_index()
832 call simple_stack%init(this%node_capacity)
833 call simple_stack%push(root_index)
835 do while (.not. simple_stack%is_empty())
836 current_index = simple_stack%pop()
839 valid = valid .and. this%nodes(current_index)%is_valid()
841 if (.not. this%nodes(current_index)%is_leaf())
then
842 left_index = this%get_left_index(current_index)
843 right_index = this%get_right_index(current_index)
845 call simple_stack%push(left_index)
846 call simple_stack%push(right_index)
856 integer,
intent(in) :: tree_start_index
860 integer :: tree_node_index
862 tree_node_index = tree_start_index
864 left_node = this%get_left_node(tree_node_index)
865 right_node = this%get_right_node(tree_node_index)
867 this%nodes(tree_node_index)%aabb = merge(left_node%aabb, right_node%aabb)
869 tree_node_index = this%get_parent_index(tree_node_index)
879 integer :: current_index
880 integer :: root_index, left_index, right_index
882 root_index = this%get_root_index()
883 call simple_stack%init(this%node_capacity)
884 call simple_stack%push(root_index)
886 do while (.not. simple_stack%is_empty())
887 current_index = simple_stack%pop()
890 left_index = this%get_left_index(current_index)
891 right_index = this%get_right_index(current_index)
893 call simple_stack%push(this%nodes(current_index)%left_node_index)
894 call simple_stack%push(this%nodes(current_index)%right_node_index)
896 write(*, *)
"i = ", current_index
897 write(*, *)
" Parent : ", this%get_parent_index(current_index)
898 write(*, *)
" Children: ", this%get_left_index(current_index), this%get_right_index(current_index)
900 write(*, *)
" object_index = ", this%nodes(current_index)%object_index
908 integer,
intent(in) :: new_capacity
910 type(
aabb_node_t),
dimension(:),
allocatable :: temp
913 allocate(temp(new_capacity))
914 temp(:this%node_capacity) = this%nodes(:this%node_capacity)
916 do i = this%allocated_node_count, new_capacity
917 temp(i)%next_node_index = i + 1
921 call move_alloc(temp, this%nodes)
923 this%node_capacity = new_capacity
924 this%next_free_node_index = this%allocated_node_count + 1
Axis Aligned Bounding Box (aabb) Tree data structure.
pure type(aabb_t) function aabb_tree_get_aabb(this, node_index)
subroutine aabb_tree_fix_upwards_tree(this, tree_start_index)
Fixes the tree upwards.
pure logical function aabb_node_is_valid(this)
Returns true if the node is a valid node.
subroutine aabb_node_init(this)
Initializes the AABB node.
pure type(aabb_node_t) function aabb_tree_get_right_node(this, node_index)
Returns the right node of the node at the given index.
pure type(aabb_node_t) function aabb_tree_get_parent_node(this, node_index)
Returns the parent node of the node at the given index.
pure type(aabb_node_t) function aabb_tree_get_root_node(this)
Returns the root node of the tree.
subroutine aabb_tree_insert_object(this, object, object_index)
Inserts an object into the tree.
pure type(aabb_node_t) function aabb_tree_get_node(this, node_index)
Returns the node at the given index.
subroutine aabb_tree_print(this)
Prints the tree.
pure logical function aabb_node_greater(this, other)
Returns true if the node is greater than the other node.
integer function aabb_tree_allocate_node(this)
Allocates a new node in the tree.
pure type(aabb_t) function aabb_node_get_aabb(this)
Returns the Axis Aligned Bounding Box (aabb) of the node.
pure integer function aabb_tree_get_root_index(this)
Returns the index of the root node.
pure integer function aabb_node_get_right_index(this)
Returns the right index of the node.
subroutine aabb_tree_deallocate_node(this, node_index)
Deallocates a node in the tree.
pure integer function aabb_node_get_left_index(this)
Returns the left index of the node.
pure logical function aabb_node_is_leaf(this)
Returns true if the node is a leaf node.
real(kind=dp) function aabb_node_min_distance(this, p)
Get the minimum possible distance from the aabb to a point.
subroutine aabb_tree_init(this, initial_capacity)
Initializes the AABB tree.
subroutine aabb_tree_insert_leaf(this, leaf_node_index)
Inserts a leaf into the tree.
subroutine aabb_tree_query_overlaps(this, object, object_index, overlaps)
Queries the tree for overlapping objects.
subroutine aabb_tree_build_tree(this, objects)
Builds the tree.
integer function, dimension(:), allocatable sort(array)
pure logical function aabb_node_less(this, other)
Returns true if the node is less than the other node.
pure integer function aabb_tree_get_parent_index(this, node_index)
Returns the index of the parent node of the node at the given index.
pure integer function aabb_node_get_parent_index(this)
Returns the parent index of the node.
pure integer function aabb_tree_get_left_index(this, node_index)
Returns the index of the left node of the node at the given index.
integer, parameter, public aabb_null_node
pure integer function aabb_node_get_object_index(this)
Returns the object index of the node.
logical function aabb_tree_valid_tree(this)
Validates the tree.
pure type(aabb_node_t) function aabb_tree_get_left_node(this, node_index)
Returns the left node of the node at the given index.
integer function aabb_tree_get_size(this)
Returns the size of the tree, in number of leaves.
subroutine aabb_tree_resize_node_pool(this, new_capacity)
Resizes the node pool.
pure integer function aabb_tree_get_right_index(this, node_index)
Returns the index of the right node of the node at the given index.
Axis Aligned Bounding Box (aabb) implementation in Fortran.
type(aabb_t) function, public get_aabb(object, padding)
Construct the aabb of a predefined object.
integer, parameter, public dp
integer, parameter, public rp
Global precision used in computations.
Implements a dynamic stack ADT.
Defines a triangular element.
Axis Aligned Bounding Box (aabb) data structure.
Node type for the Axis Aligned Bounding Box (aabb) Tree.
Axis Aligned Bounding Box (aabb) Tree.