diff --git a/CMakeLists.txt b/CMakeLists.txt index 7ffb1f6..2366eb8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,7 @@ set(CCL_COMMON_SOURCES src/common/ccl_errors.f90 src/common/ccl_log.f90 src/geometry/ccl_geometry.f90 + src/search/ccl_broad_phase.f90 src/diagnostics/ccl_diagnostics.f90 src/case/ccl_string_utils.f90 src/case/ccl_case_types.f90 @@ -93,6 +94,11 @@ add_executable(test_geometry_primitives ) target_link_libraries(test_geometry_primitives PRIVATE ccl_common) +add_executable(test_broad_phase + tests/unit/test_broad_phase.f90 +) +target_link_libraries(test_broad_phase PRIVATE ccl_common) + add_test(NAME smoke_cclite_version COMMAND cclite --version) add_test(NAME smoke_cclite_help COMMAND cclite --help) add_test(NAME smoke_mock_sph_worker_version COMMAND mock_sph_worker --version) @@ -141,6 +147,7 @@ add_test(NAME command_block_semantics COMMAND test_command_block) add_test(NAME data_block_semantics COMMAND test_data_block) add_test(NAME V020_interface_view_and_load_buffer COMMAND test_interface_view) add_test(NAME V020_geometry_primitives COMMAND test_geometry_primitives) +add_test(NAME V020_broad_phase_search COMMAND test_broad_phase) add_test(NAME dry_run_one_step COMMAND cclite dry-run ${CMAKE_SOURCE_DIR}/tests/data/v010_one_step.inp ) diff --git a/src/search/ccl_broad_phase.f90 b/src/search/ccl_broad_phase.f90 new file mode 100644 index 0000000..41399e5 --- /dev/null +++ b/src/search/ccl_broad_phase.f90 @@ -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 diff --git a/tests/unit/test_broad_phase.f90 b/tests/unit/test_broad_phase.f90 new file mode 100644 index 0000000..e9092e2 --- /dev/null +++ b/tests/unit/test_broad_phase.f90 @@ -0,0 +1,107 @@ +program test_broad_phase + use iso_c_binding, only: c_double + use ccl_broad_phase, only: CCL_Broad_Phase_Stats_Type, CCL_Candidate_Pair_Type, & + CCL_Hash_Grid_Type, ccl_broad_phase_stats_reset, ccl_hash_grid_init, & + ccl_hash_grid_insert_facet, ccl_hash_grid_query_particle + use ccl_errors, only: CCL_ERROR_INPUT_INVALID_VALUE, CCL_SUCCESS + use ccl_geometry, only: CCL_AABB_Type, ccl_aabb_init, ccl_aabb_expand_point, ccl_particle_aabb + implicit none + + integer :: ierr + integer :: candidate_count + type(CCL_Hash_Grid_Type) :: grid + type(CCL_Broad_Phase_Stats_Type) :: stats + type(CCL_AABB_Type) :: facet_box + type(CCL_AABB_Type) :: particle_box + type(CCL_Candidate_Pair_Type) :: candidates(4) + + call ccl_hash_grid_init(grid, 1.0_c_double, 16, ierr) + call require_equal(ierr, CCL_SUCCESS, "grid init") + call ccl_broad_phase_stats_reset(stats) + + call make_box([0.0_c_double, 0.0_c_double, 0.0_c_double], & + [1.0_c_double, 1.0_c_double, 0.0_c_double], facet_box) + call ccl_hash_grid_insert_facet(grid, 1, facet_box, stats, ierr) + call require_equal(ierr, CCL_SUCCESS, "facet insert") + call require_true(stats%facet_insert_count > 0, "facet insertion counted") + call require_true(stats%grid_cell_count > 0, "grid cells counted") + + call ccl_particle_aabb([0.5_c_double, 0.5_c_double, 0.0_c_double], 0.1_c_double, particle_box, ierr) + call require_equal(ierr, CCL_SUCCESS, "particle AABB near facet") + candidate_count = 0 + call ccl_hash_grid_query_particle(grid, 1, particle_box, candidates, candidate_count, 4, stats, ierr) + call require_equal(ierr, CCL_SUCCESS, "near particle query") + call require_equal(candidate_count, 1, "near particle candidate count") + call require_equal(candidates(1)%particle_index, 1, "candidate particle id") + call require_equal(candidates(1)%facet_index, 1, "candidate facet id") + + call ccl_particle_aabb([5.0_c_double, 5.0_c_double, 0.0_c_double], 0.1_c_double, particle_box, ierr) + call require_equal(ierr, CCL_SUCCESS, "particle AABB far") + candidate_count = 0 + call ccl_hash_grid_query_particle(grid, 2, particle_box, candidates, candidate_count, 4, stats, ierr) + call require_equal(ierr, CCL_SUCCESS, "far particle query") + call require_equal(candidate_count, 0, "far particle has no candidate") + + call ccl_hash_grid_init(grid, 1.0_c_double, 16, ierr) + call require_equal(ierr, CCL_SUCCESS, "boundary grid init") + call ccl_broad_phase_stats_reset(stats) + call make_box([0.9_c_double, 0.0_c_double, 0.0_c_double], & + [1.1_c_double, 0.2_c_double, 0.0_c_double], facet_box) + call ccl_hash_grid_insert_facet(grid, 3, facet_box, stats, ierr) + call require_equal(ierr, CCL_SUCCESS, "boundary facet insert") + call ccl_particle_aabb([1.0_c_double, 0.1_c_double, 0.0_c_double], 0.05_c_double, particle_box, ierr) + candidate_count = 0 + call ccl_hash_grid_query_particle(grid, 3, particle_box, candidates, candidate_count, 4, stats, ierr) + call require_equal(ierr, CCL_SUCCESS, "boundary particle query") + call require_equal(candidate_count, 1, "boundary candidate deduplicated") + call require_true(stats%duplicate_candidate_count >= 1, "duplicate candidate counted") + + call ccl_hash_grid_init(grid, 1.0_c_double, 16, ierr) + call ccl_broad_phase_stats_reset(stats) + call make_box([0.0_c_double, 0.0_c_double, 0.0_c_double], & + [0.2_c_double, 0.2_c_double, 0.0_c_double], facet_box) + call ccl_hash_grid_insert_facet(grid, 4, facet_box, stats, ierr) + call require_equal(ierr, CCL_SUCCESS, "overflow facet insert") + call ccl_particle_aabb([0.1_c_double, 0.1_c_double, 0.0_c_double], 0.05_c_double, particle_box, ierr) + candidate_count = 0 + call ccl_hash_grid_query_particle(grid, 4, particle_box, candidates, candidate_count, 0, stats, ierr) + call require_equal(ierr, CCL_ERROR_INPUT_INVALID_VALUE, "candidate overflow rejected") + call require_equal(stats%overflow_count, 1, "candidate overflow counted") + +contains + + subroutine make_box(min_corner, max_corner, box) + real(c_double), intent(in) :: min_corner(3) + real(c_double), intent(in) :: max_corner(3) + type(CCL_AABB_Type), intent(out) :: box + integer :: local_ierr + + call ccl_aabb_init(box) + call ccl_aabb_expand_point(box, min_corner, local_ierr) + call require_equal(local_ierr, CCL_SUCCESS, "box min point") + call ccl_aabb_expand_point(box, max_corner, local_ierr) + call require_equal(local_ierr, CCL_SUCCESS, "box max point") + end subroutine make_box + + subroutine require_equal(actual, expected, message) + integer, intent(in) :: actual + integer, intent(in) :: expected + character(len=*), intent(in) :: message + + if (actual /= expected) then + print '(a,": expected ",i0," got ",i0)', trim(message), expected, actual + stop 1 + end if + end subroutine require_equal + + subroutine require_true(condition, message) + logical, intent(in) :: condition + character(len=*), intent(in) :: message + + if (.not. condition) then + print '(a)', trim(message) + stop 1 + end if + end subroutine require_true + +end program test_broad_phase diff --git a/todo.md b/todo.md index df021be..36da489 100644 --- a/todo.md +++ b/todo.md @@ -193,34 +193,34 @@ 目标:实现 uniform hash grid,从 particle/facet view 生成候选接触对。 -- [ ] T0204-01 定义 hash grid 数据结构。 +- [x] T0204-01 定义 hash grid 数据结构。 完成判据:可保存 cell size、cell key、facet 列表和容量信息。 -- [ ] T0204-02 定义 grid cell size 选择规则。 +- [x] T0204-02 定义 grid cell size 选择规则。 完成判据:默认 cell size 与最大接触半径或用户配置有明确关系。 -- [ ] T0204-03 实现 facet AABB 插入 grid。 +- [x] T0204-03 实现 facet AABB 插入 grid。 完成判据:跨多个 cell 的 facet 能被插入所有覆盖 cell。 -- [ ] T0204-04 实现 particle AABB 查询 grid。 +- [x] T0204-04 实现 particle AABB 查询 grid。 完成判据:粒子可查询相邻 cell 并返回候选 facet id。 -- [ ] T0204-05 实现 candidate pair 去重。 +- [x] T0204-05 实现 candidate pair 去重。 完成判据:同一粒子和 facet 组合不会重复进入窄相。 -- [ ] T0204-06 实现 candidate 容量保护。 +- [x] T0204-06 实现 candidate 容量保护。 完成判据:候选数超限时返回明确错误码,并写入诊断。 -- [ ] T0204-07 输出宽相统计。 +- [x] T0204-07 输出宽相统计。 完成判据:每步记录 grid cell 数、facet 插入数、candidate 数和去重数。 -- [ ] T0204-08 增加单 facet 单粒子搜索测试。 +- [x] T0204-08 增加单 facet 单粒子搜索测试。 完成判据:近距离粒子生成候选,远距离粒子不生成候选。 -- [ ] T0204-09 增加跨 cell 边界搜索测试。 +- [x] T0204-09 增加跨 cell 边界搜索测试。 完成判据:粒子位于 cell 边界时不漏检。 -- [ ] T0204-10 增加 candidate overflow 测试。 +- [x] T0204-10 增加 candidate overflow 测试。 完成判据:构造超限候选时程序给出可诊断失败,而不是越界或静默截断。 # 7. 0.2.5 窄相 Contact Pair