Add CCLite 0.2.4 broad-phase search
This commit is contained in:
@@ -0,0 +1,246 @@
|
||||
module ccl_broad_phase
|
||||
use, intrinsic :: ieee_arithmetic, only: ieee_is_finite
|
||||
use iso_c_binding, only: c_double
|
||||
use ccl_errors, only: CCL_ERROR_INPUT_INVALID_VALUE, CCL_SUCCESS
|
||||
use ccl_geometry, only: CCL_AABB_Type
|
||||
implicit none
|
||||
|
||||
type :: CCL_Grid_Facet_Entry_Type
|
||||
integer :: ix = 0
|
||||
integer :: iy = 0
|
||||
integer :: iz = 0
|
||||
integer :: facet_index = 0
|
||||
end type CCL_Grid_Facet_Entry_Type
|
||||
|
||||
type :: CCL_Hash_Grid_Type
|
||||
real(c_double) :: cell_size = 0.0_c_double
|
||||
integer :: entry_count = 0
|
||||
integer :: entry_capacity = 0
|
||||
type(CCL_Grid_Facet_Entry_Type), allocatable :: entries(:)
|
||||
end type CCL_Hash_Grid_Type
|
||||
|
||||
type :: CCL_Candidate_Pair_Type
|
||||
integer :: particle_index = 0
|
||||
integer :: facet_index = 0
|
||||
end type CCL_Candidate_Pair_Type
|
||||
|
||||
type :: CCL_Broad_Phase_Stats_Type
|
||||
integer :: grid_cell_count = 0
|
||||
integer :: facet_insert_count = 0
|
||||
integer :: candidate_count = 0
|
||||
integer :: duplicate_candidate_count = 0
|
||||
integer :: overflow_count = 0
|
||||
end type CCL_Broad_Phase_Stats_Type
|
||||
|
||||
contains
|
||||
|
||||
subroutine ccl_hash_grid_init(grid, cell_size, entry_capacity, ierr)
|
||||
type(CCL_Hash_Grid_Type), intent(out) :: grid
|
||||
real(c_double), intent(in) :: cell_size
|
||||
integer, intent(in) :: entry_capacity
|
||||
integer, intent(out) :: ierr
|
||||
|
||||
ierr = CCL_SUCCESS
|
||||
grid%cell_size = 0.0_c_double
|
||||
grid%entry_count = 0
|
||||
grid%entry_capacity = 0
|
||||
if (.not. ieee_is_finite(cell_size) .or. cell_size <= 0.0_c_double) then
|
||||
ierr = CCL_ERROR_INPUT_INVALID_VALUE
|
||||
return
|
||||
end if
|
||||
if (entry_capacity < 0) then
|
||||
ierr = CCL_ERROR_INPUT_INVALID_VALUE
|
||||
return
|
||||
end if
|
||||
grid%cell_size = cell_size
|
||||
grid%entry_capacity = entry_capacity
|
||||
allocate(grid%entries(entry_capacity))
|
||||
end subroutine ccl_hash_grid_init
|
||||
|
||||
subroutine ccl_broad_phase_stats_reset(stats)
|
||||
type(CCL_Broad_Phase_Stats_Type), intent(out) :: stats
|
||||
|
||||
stats%grid_cell_count = 0
|
||||
stats%facet_insert_count = 0
|
||||
stats%candidate_count = 0
|
||||
stats%duplicate_candidate_count = 0
|
||||
stats%overflow_count = 0
|
||||
end subroutine ccl_broad_phase_stats_reset
|
||||
|
||||
subroutine ccl_hash_grid_insert_facet(grid, facet_index, aabb, stats, ierr)
|
||||
type(CCL_Hash_Grid_Type), intent(inout) :: grid
|
||||
integer, intent(in) :: facet_index
|
||||
type(CCL_AABB_Type), intent(in) :: aabb
|
||||
type(CCL_Broad_Phase_Stats_Type), intent(inout) :: stats
|
||||
integer, intent(out) :: ierr
|
||||
integer :: min_cell(3)
|
||||
integer :: max_cell(3)
|
||||
integer :: ix
|
||||
integer :: iy
|
||||
integer :: iz
|
||||
|
||||
ierr = CCL_SUCCESS
|
||||
if (facet_index <= 0 .or. .not. aabb%is_valid) then
|
||||
ierr = CCL_ERROR_INPUT_INVALID_VALUE
|
||||
return
|
||||
end if
|
||||
call aabb_cell_range(grid, aabb, min_cell, max_cell, ierr)
|
||||
if (ierr /= CCL_SUCCESS) return
|
||||
|
||||
do iz = min_cell(3), max_cell(3)
|
||||
do iy = min_cell(2), max_cell(2)
|
||||
do ix = min_cell(1), max_cell(1)
|
||||
if (grid%entry_count >= grid%entry_capacity) then
|
||||
stats%overflow_count = stats%overflow_count + 1
|
||||
ierr = CCL_ERROR_INPUT_INVALID_VALUE
|
||||
return
|
||||
end if
|
||||
if (.not. grid_cell_exists(grid, ix, iy, iz)) then
|
||||
stats%grid_cell_count = stats%grid_cell_count + 1
|
||||
end if
|
||||
grid%entry_count = grid%entry_count + 1
|
||||
grid%entries(grid%entry_count)%ix = ix
|
||||
grid%entries(grid%entry_count)%iy = iy
|
||||
grid%entries(grid%entry_count)%iz = iz
|
||||
grid%entries(grid%entry_count)%facet_index = facet_index
|
||||
stats%facet_insert_count = stats%facet_insert_count + 1
|
||||
end do
|
||||
end do
|
||||
end do
|
||||
end subroutine ccl_hash_grid_insert_facet
|
||||
|
||||
subroutine ccl_hash_grid_query_particle(grid, particle_index, aabb, candidates, candidate_count, &
|
||||
max_candidates, stats, ierr)
|
||||
type(CCL_Hash_Grid_Type), intent(in) :: grid
|
||||
integer, intent(in) :: particle_index
|
||||
type(CCL_AABB_Type), intent(in) :: aabb
|
||||
type(CCL_Candidate_Pair_Type), intent(inout) :: candidates(:)
|
||||
integer, intent(inout) :: candidate_count
|
||||
integer, intent(in) :: max_candidates
|
||||
type(CCL_Broad_Phase_Stats_Type), intent(inout) :: stats
|
||||
integer, intent(out) :: ierr
|
||||
integer :: min_cell(3)
|
||||
integer :: max_cell(3)
|
||||
integer :: ix
|
||||
integer :: iy
|
||||
integer :: iz
|
||||
integer :: entry_id
|
||||
|
||||
ierr = CCL_SUCCESS
|
||||
if (particle_index <= 0 .or. .not. aabb%is_valid) then
|
||||
ierr = CCL_ERROR_INPUT_INVALID_VALUE
|
||||
return
|
||||
end if
|
||||
if (candidate_count < 0 .or. max_candidates < 0 .or. max_candidates > size(candidates)) then
|
||||
ierr = CCL_ERROR_INPUT_INVALID_VALUE
|
||||
return
|
||||
end if
|
||||
call aabb_cell_range(grid, aabb, min_cell, max_cell, ierr)
|
||||
if (ierr /= CCL_SUCCESS) return
|
||||
|
||||
do iz = min_cell(3), max_cell(3)
|
||||
do iy = min_cell(2), max_cell(2)
|
||||
do ix = min_cell(1), max_cell(1)
|
||||
do entry_id = 1, grid%entry_count
|
||||
if (grid%entries(entry_id)%ix == ix .and. grid%entries(entry_id)%iy == iy .and. &
|
||||
grid%entries(entry_id)%iz == iz) then
|
||||
call append_candidate(candidates, candidate_count, max_candidates, particle_index, &
|
||||
grid%entries(entry_id)%facet_index, stats, ierr)
|
||||
if (ierr /= CCL_SUCCESS) return
|
||||
end if
|
||||
end do
|
||||
end do
|
||||
end do
|
||||
end do
|
||||
end subroutine ccl_hash_grid_query_particle
|
||||
|
||||
subroutine append_candidate(candidates, candidate_count, max_candidates, particle_index, facet_index, &
|
||||
stats, ierr)
|
||||
type(CCL_Candidate_Pair_Type), intent(inout) :: candidates(:)
|
||||
integer, intent(inout) :: candidate_count
|
||||
integer, intent(in) :: max_candidates
|
||||
integer, intent(in) :: particle_index
|
||||
integer, intent(in) :: facet_index
|
||||
type(CCL_Broad_Phase_Stats_Type), intent(inout) :: stats
|
||||
integer, intent(out) :: ierr
|
||||
|
||||
ierr = CCL_SUCCESS
|
||||
if (candidate_exists(candidates, candidate_count, particle_index, facet_index)) then
|
||||
stats%duplicate_candidate_count = stats%duplicate_candidate_count + 1
|
||||
return
|
||||
end if
|
||||
if (candidate_count >= max_candidates) then
|
||||
stats%overflow_count = stats%overflow_count + 1
|
||||
ierr = CCL_ERROR_INPUT_INVALID_VALUE
|
||||
return
|
||||
end if
|
||||
candidate_count = candidate_count + 1
|
||||
candidates(candidate_count)%particle_index = particle_index
|
||||
candidates(candidate_count)%facet_index = facet_index
|
||||
stats%candidate_count = candidate_count
|
||||
end subroutine append_candidate
|
||||
|
||||
logical function candidate_exists(candidates, candidate_count, particle_index, facet_index) result(found)
|
||||
type(CCL_Candidate_Pair_Type), intent(in) :: candidates(:)
|
||||
integer, intent(in) :: candidate_count
|
||||
integer, intent(in) :: particle_index
|
||||
integer, intent(in) :: facet_index
|
||||
integer :: i
|
||||
|
||||
found = .false.
|
||||
do i = 1, candidate_count
|
||||
if (candidates(i)%particle_index == particle_index .and. candidates(i)%facet_index == facet_index) then
|
||||
found = .true.
|
||||
return
|
||||
end if
|
||||
end do
|
||||
end function candidate_exists
|
||||
|
||||
logical function grid_cell_exists(grid, ix, iy, iz) result(found)
|
||||
type(CCL_Hash_Grid_Type), intent(in) :: grid
|
||||
integer, intent(in) :: ix
|
||||
integer, intent(in) :: iy
|
||||
integer, intent(in) :: iz
|
||||
integer :: i
|
||||
|
||||
found = .false.
|
||||
do i = 1, grid%entry_count
|
||||
if (grid%entries(i)%ix == ix .and. grid%entries(i)%iy == iy .and. grid%entries(i)%iz == iz) then
|
||||
found = .true.
|
||||
return
|
||||
end if
|
||||
end do
|
||||
end function grid_cell_exists
|
||||
|
||||
subroutine aabb_cell_range(grid, aabb, min_cell, max_cell, ierr)
|
||||
type(CCL_Hash_Grid_Type), intent(in) :: grid
|
||||
type(CCL_AABB_Type), intent(in) :: aabb
|
||||
integer, intent(out) :: min_cell(3)
|
||||
integer, intent(out) :: max_cell(3)
|
||||
integer, intent(out) :: ierr
|
||||
integer :: i
|
||||
|
||||
ierr = CCL_SUCCESS
|
||||
min_cell = 0
|
||||
max_cell = 0
|
||||
if (grid%cell_size <= 0.0_c_double .or. .not. aabb%is_valid) then
|
||||
ierr = CCL_ERROR_INPUT_INVALID_VALUE
|
||||
return
|
||||
end if
|
||||
if (.not. finite_vector3(aabb%min_corner) .or. .not. finite_vector3(aabb%max_corner)) then
|
||||
ierr = CCL_ERROR_INPUT_INVALID_VALUE
|
||||
return
|
||||
end if
|
||||
do i = 1, 3
|
||||
min_cell(i) = int(floor(aabb%min_corner(i) / grid%cell_size))
|
||||
max_cell(i) = int(floor(aabb%max_corner(i) / grid%cell_size))
|
||||
end do
|
||||
end subroutine aabb_cell_range
|
||||
|
||||
logical function finite_vector3(value) result(is_finite)
|
||||
real(c_double), intent(in) :: value(3)
|
||||
|
||||
is_finite = ieee_is_finite(value(1)) .and. ieee_is_finite(value(2)) .and. ieee_is_finite(value(3))
|
||||
end function finite_vector3
|
||||
|
||||
end module ccl_broad_phase
|
||||
Reference in New Issue
Block a user