88 integer :: object_index = -1
103 procedure, pass(this),
public :: get_object_index => &
105 procedure, pass(this),
public :: get_parent_index => &
107 procedure, pass(this),
public :: get_left_index => &
109 procedure, pass(this),
public :: get_right_index => &
120 generic ::
operator(.lt.) => less
121 generic ::
operator(.gt.) => greater
133 integer :: allocated_node_count = 0
135 integer :: node_capacity = 0
136 integer :: growth_size = 1
143 procedure, pass(this),
public :: insert_object => &
149 procedure, pass(this),
public :: get_root_index => &
151 procedure, pass(this),
public :: get_parent_index => &
153 procedure, pass(this),
public :: get_left_index => &
155 procedure, pass(this),
public :: get_right_index => &
159 procedure, pass(this),
public :: get_root_node => &
161 procedure, pass(this),
public :: get_parent_node => &
163 procedure, pass(this),
public :: get_left_node => &
165 procedure, pass(this),
public :: get_right_node => &
170 procedure, pass(this),
public :: query_overlaps => &
199 this%object_index = -1
220 integer :: object_index
222 object_index = this%object_index
228 integer :: parent_index
230 parent_index = this%parent_node_index
236 integer :: left_index
238 left_index = this%left_node_index
244 integer :: right_index
246 right_index = this%right_node_index
252 real(kind=
dp),
dimension(3),
intent(in) :: p
253 real(kind=
dp) :: distance
255 distance = 0.5_rp * this%aabb%get_diameter() &
256 - norm2(this%aabb%get_center() - p)
276 if (this%is_leaf())
then
280 & this%object_index .gt. 0
285 & this%object_index .eq. -1
299 res = this%aabb .lt. other%aabb
309 res = this%aabb .gt. other%aabb
320 integer,
intent(in) :: initial_capacity
325 this%allocated_node_count = 0
326 this%next_free_node_index = 1
327 this%node_capacity = initial_capacity
328 this%growth_size = initial_capacity
330 if (
allocated(this%nodes))
deallocate(this%nodes)
331 allocate(this%nodes(initial_capacity))
333 do i = 1, initial_capacity
334 this%nodes(i)%next_node_index = i + 1
345 class(*),
dimension(:),
intent(in) :: objects
347 integer :: i_obj, i_node, i
350 integer :: start_layer, end_layer
352 type(
aabb_t),
dimension(:),
allocatable :: box_list
353 integer,
dimension(:),
allocatable :: sorted_indices
355 call this%init(
size(objects) * 2)
361 allocate(box_list(
size(objects)))
363 do i_obj = 1,
size(objects)
364 box_list(i_obj) =
get_aabb(objects(i_obj))
366 sorted_indices =
sort(box_list)
368 do i = 1,
size(sorted_indices)
369 i_obj = sorted_indices(i)
370 i_node = this%allocate_node()
371 this%nodes(i_node)%aabb =
get_aabb(objects(i_obj))
372 this%nodes(i_node)%object_index = i_obj
377 end_layer =
size(objects)
379 do while (.not. done)
382 do i = start_layer, end_layer - 1, 2
383 i_node = this%allocate_node()
385 this%nodes(i_node)%aabb =
merge(this%nodes(i)%aabb, &
386 this%nodes(i + 1)%aabb)
388 this%nodes(i_node)%left_node_index = i
389 this%nodes(i_node)%right_node_index = i + 1
391 this%nodes(i)%parent_node_index = i_node
392 this%nodes(i + 1)%parent_node_index = i_node
397 if (mod(end_layer - start_layer, 2) .eq. 0)
then
398 i_node = this%allocate_node()
399 this%nodes(i_node)%aabb = this%nodes(end_layer)%aabb
400 this%nodes(i_node)%left_node_index = end_layer
403 this%nodes(end_layer)%parent_node_index = i_node
407 start_layer = end_layer + 1
408 end_layer = this%allocated_node_count
411 done = start_layer .eq. end_layer
415 this%root_node_index = this%allocated_node_count
417 if (this%get_size() .ne.
size(objects))
then
418 print *,
"this%get_size() = ", this%get_size()
419 print *,
"size(objects) = ",
size(objects)
425 function sort(array)
result(indices)
426 type(
aabb_t),
dimension(:),
intent(in) :: array
427 integer,
dimension(:),
allocatable :: indices
428 logical,
dimension(:),
allocatable :: visited
433 allocate(indices(
size(array)))
434 allocate(visited(
size(array)))
438 do i = 1,
size(array)
440 do imin = 1,
size(array)
441 if (.not. visited(imin) .and. minidx .eq. -1) minidx = imin
443 if (visited(imin) .and. array(imin) .lt. array(minidx)) minidx = imin
447 visited(minidx) = .true.
465 call simple_stack%init(this%allocated_node_count)
467 tmp = this%get_root_index()
469 call simple_stack%push(tmp)
472 do while (.not. simple_stack%is_empty())
473 idx = simple_stack%pop()
476 if (this%nodes(idx)%is_leaf())
then
479 tmp = this%get_left_index(idx)
480 call simple_stack%push(tmp)
481 tmp = this%get_right_index(idx)
482 call simple_stack%push(tmp)
494 integer :: root_index
496 root_index = this%root_node_index
503 integer,
intent(in) :: node_index
504 integer :: parent_index
506 parent_index = this%nodes(node_index)%parent_node_index
513 integer,
intent(in) :: node_index
514 integer :: left_index
516 left_index = this%nodes(node_index)%left_node_index
523 integer,
intent(in) :: node_index
524 integer :: right_index
526 right_index = this%nodes(node_index)%right_node_index
535 integer,
intent(in) :: node_index
538 node = this%nodes(node_index)
546 root_node = this%nodes(this%root_node_index)
553 integer,
intent(in) :: node_index
556 parent_node = this%nodes(this%nodes(node_index)%parent_node_index)
562 integer,
intent(in) :: node_index
565 left_node = this%nodes(this%nodes(node_index)%left_node_index)
572 integer,
intent(in) :: node_index
575 right_node = this%nodes(this%nodes(node_index)%right_node_index)
580 integer,
intent(in) :: node_index
583 out_box = this%nodes(node_index)%aabb
591 class(*),
intent(in) :: object
592 integer,
intent(in) :: object_index
594 integer :: node_index
596 node_index = this%allocate_node()
597 this%nodes(node_index)%aabb =
get_aabb(object)
598 this%nodes(node_index)%object_index = object_index
600 call this%insert_leaf(node_index)
609 class(*),
intent(in) :: object
610 integer,
intent(in) :: object_index
611 integer,
intent(out) :: overlaps(:)
614 type(
aabb_t) :: object_box
616 integer :: root_index, left_index, right_index
618 integer :: node_index
621 root_index = this%get_root_index()
623 call simple_stack%init()
624 call simple_stack%push(root_index)
626 do while (.not. simple_stack%is_empty())
627 node_index = simple_stack%pop()
631 if (this%nodes(node_index)%aabb%overlaps(object_box))
then
632 if (this%nodes(node_index)%is_leaf())
then
633 if (this%nodes(node_index)%object_index .ne. object_index)
then
634 overlaps = [this%nodes(node_index)%object_index, overlaps]
637 left_index = this%get_left_index(node_index)
638 call simple_stack%push(left_index)
639 right_index = this%get_right_index(node_index)
640 call simple_stack%push(right_index)
652 integer :: node_index
655 call this%resize_node_pool(this%node_capacity + this%growth_size)
658 node_index = this%next_free_node_index
660 associate(new_node => this%nodes(node_index))
661 this%next_free_node_index = new_node%next_node_index
667 this%next_free_node_index = new_node%next_node_index
668 this%allocated_node_count = this%allocated_node_count + 1
676 integer,
intent(in) :: node_index
678 this%nodes(node_index)%next_node_index = this%next_free_node_index
679 this%next_free_node_index = node_index
680 this%allocated_node_count = this%allocated_node_count - 1
686 integer,
intent(in) :: leaf_node_index
688 integer :: tree_node_index
690 real(kind=rp) :: cost_left
691 real(kind=rp) :: cost_right
698 type(aabb_t) :: combined_aabb
699 real(kind=rp) :: new_parent_node_cost
700 real(kind=rp) :: minimum_push_down_cost
701 type(aabb_t) :: new_left_aabb
702 type(aabb_t) :: new_right_aabb
703 integer :: leaf_sibling_index
705 integer :: old_parent_index
706 integer :: new_parent_index
711 leaf_node = this%nodes(leaf_node_index)
715 this%root_node_index = leaf_node_index
725 tree_node_index = this%root_node_index
726 tree_node = this%get_node(tree_node_index)
727 do while (.not. tree_node%is_leaf())
731 left_node = this%get_left_node(tree_node_index)
732 right_node = this%get_right_node(tree_node_index)
736 combined_aabb = merge(tree_node%aabb, leaf_node%get_aabb())
738 new_parent_node_cost = 2.0_rp * combined_aabb%get_surface_area()
739 minimum_push_down_cost = 2.0_rp * ( &
740 & combined_aabb%get_surface_area() &
741 & - tree_node%aabb%get_surface_area()&
746 if (left_node%is_leaf())
then
747 new_left_aabb = merge(leaf_node%aabb, left_node%get_aabb())
748 cost_left = new_left_aabb%get_surface_area() + minimum_push_down_cost
750 new_left_aabb = merge(leaf_node%aabb, left_node%get_aabb())
752 & new_left_aabb%get_surface_area() &
753 & - left_node%aabb%get_surface_area()&
754 & ) + minimum_push_down_cost
757 if (right_node%is_leaf())
then
758 new_right_aabb = merge(leaf_node%aabb, right_node%aabb)
759 cost_right = new_right_aabb%get_surface_area() + &
760 minimum_push_down_cost
762 new_right_aabb = merge(leaf_node%aabb, right_node%aabb)
764 & new_right_aabb%get_surface_area() &
765 & - right_node%aabb%get_surface_area() &
766 & ) + minimum_push_down_cost
772 if (new_parent_node_cost < cost_left .and. &
773 new_parent_node_cost < cost_right)
then
778 if (cost_left .lt. cost_right)
then
779 tree_node_index = tree_node%get_left_index()
781 tree_node_index = tree_node%get_right_index()
786 tree_node = this%get_node(tree_node_index)
791 leaf_sibling_index = tree_node_index
792 leaf_sibling = this%nodes(leaf_sibling_index)
793 old_parent_index = this%get_parent_index(leaf_sibling_index)
794 new_parent_index = this%allocate_node()
795 new_parent = this%nodes(new_parent_index)
796 new_parent%parent_node_index = old_parent_index
797 new_parent%aabb = merge(leaf_node%aabb, leaf_sibling%aabb)
799 if (leaf_node .lt. leaf_sibling)
then
800 new_parent%left_node_index = leaf_node_index
801 new_parent%right_node_index = leaf_sibling_index
803 new_parent%left_node_index = leaf_sibling_index
804 new_parent%right_node_index = leaf_node_index
807 leaf_node%parent_node_index = new_parent_index
808 leaf_sibling%parent_node_index = new_parent_index
812 this%root_node_index = new_parent_index
816 old_parent = this%nodes(old_parent_index)
817 if (old_parent%left_node_index .eq. leaf_sibling_index)
then
818 old_parent%left_node_index = new_parent_index
820 old_parent%right_node_index = new_parent_index
822 this%nodes(old_parent_index) = old_parent
825 this%nodes(leaf_node_index) = leaf_node
826 this%nodes(leaf_sibling_index) = leaf_sibling
827 this%nodes(new_parent_index) = new_parent
830 tree_node_index = leaf_node%parent_node_index
832 call this%fix_upwards_tree(tree_node_index)
845 integer :: current_index
846 integer :: root_index, left_index, right_index
853 root_index = this%get_root_index()
855 call simple_stack%init(this%node_capacity)
856 call simple_stack%push(root_index)
858 do while (.not. simple_stack%is_empty())
859 current_index = simple_stack%pop()
862 valid = valid .and. this%nodes(current_index)%is_valid()
864 if (.not. this%nodes(current_index)%is_leaf())
then
865 left_index = this%get_left_index(current_index)
866 right_index = this%get_right_index(current_index)
868 call simple_stack%push(left_index)
869 call simple_stack%push(right_index)
879 integer,
intent(in) :: tree_start_index
883 integer :: tree_node_index
885 tree_node_index = tree_start_index
887 left_node = this%get_left_node(tree_node_index)
888 right_node = this%get_right_node(tree_node_index)
890 this%nodes(tree_node_index)%aabb = merge(left_node%aabb, right_node%aabb)
892 tree_node_index = this%get_parent_index(tree_node_index)
902 integer :: current_index
903 integer :: root_index, left_index, right_index
905 root_index = this%get_root_index()
906 call simple_stack%init(this%node_capacity)
907 call simple_stack%push(root_index)
909 do while (.not. simple_stack%is_empty())
910 current_index = simple_stack%pop()
913 left_index = this%get_left_index(current_index)
914 right_index = this%get_right_index(current_index)
916 call simple_stack%push(this%nodes(current_index)%left_node_index)
917 call simple_stack%push(this%nodes(current_index)%right_node_index)
919 write(*, *)
"i = ", current_index
920 write(*, *)
" Parent : ", this%get_parent_index(current_index)
921 write(*, *)
" Children: ", this%get_left_index(current_index), &
922 this%get_right_index(current_index)
924 write(*, *)
" object_index = ", this%nodes(current_index)%object_index
932 integer,
intent(in) :: new_capacity
934 type(
aabb_node_t),
dimension(:),
allocatable :: temp
937 allocate(temp(new_capacity))
938 temp(:this%node_capacity) = this%nodes(:this%node_capacity)
940 do i = this%allocated_node_count, new_capacity
941 temp(i)%next_node_index = i + 1
945 call move_alloc(temp, this%nodes)
947 this%node_capacity = new_capacity
948 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.