bem_collision.f90 Source File

!> 粒子軌道セグメントと三角形要素の交差判定を提供する衝突検出モジュール。
module bem_collision
  use, intrinsic :: ieee_arithmetic, only: ieee_is_finite
  use, intrinsic :: iso_fortran_env, only: error_unit
  use bem_kinds, only: dp, i32, i64
  use bem_types, only: mesh_type, hit_info, sim_config, bc_periodic
  use bem_string_utils, only: lower_ascii
  implicit none
  integer(i32), parameter, public :: collision_query_ok = 0_i32
  integer(i32), parameter, public :: collision_query_image_limit = 1_i32
  integer(i32), parameter, public :: collision_query_index_range = 2_i32
  integer(i32), parameter, public :: collision_query_invalid_segment = 3_i32
  integer(i32), parameter, public :: collision_query_grid_stalled = 4_i32
  integer(i64), parameter :: max_periodic2_collision_images = 4096_i64
  private :: finalize_collision_query, report_collision_grid_stall
contains

  !> 線分 `[p0,p1]` に対して最初に衝突する三角形要素を探索し、命中情報を返す。
  !! @param[in] mesh 三角形要素とAABB情報を保持した衝突判定対象メッシュ。
  !! @param[in] p0 線分始点（粒子の移動前位置） [m]。
  !! @param[in] p1 線分終点（粒子の移動後候補位置） [m]。
  !! @param[out] hit 最初に命中した要素インデックス・命中位置・線分パラメータを格納。
  !! @param[out] status 照会完了状態。未指定時に照会が不完全なら停止する。
  subroutine find_first_hit(mesh, p0, p1, hit, sim, box_min, box_max, require_elem_inside, status)
    type(mesh_type), intent(in) :: mesh
    real(dp), intent(in) :: p0(3), p1(3)
    type(hit_info), intent(out) :: hit
    type(sim_config), intent(in), optional :: sim
    real(dp), intent(in), optional :: box_min(3), box_max(3)
    logical, intent(in), optional :: require_elem_inside
    integer(i32), intent(out), optional :: status

    logical :: use_periodic2
    integer(i32) :: periodic_axes(2), query_status
    real(dp) :: periodic_len(2)

    use_periodic2 = .false.
    query_status = collision_query_ok
    periodic_axes = 0_i32
    periodic_len = 0.0d0
    if (any(.not. ieee_is_finite(p0)) .or. any(.not. ieee_is_finite(p1))) then
      call initialize_hit(hit)
      call finalize_collision_query(collision_query_invalid_segment, status)
      return
    end if
    if (present(sim)) then
      call resolve_periodic2_collision_config(sim, use_periodic2, periodic_axes, periodic_len)
    end if

    if (use_periodic2) then
      call find_first_hit_periodic2( &
        mesh, p0, p1, hit, sim, box_min, box_max, require_elem_inside, status=query_status &
        )
    else
      call find_first_hit_base(mesh, p0, p1, hit, box_min, box_max, require_elem_inside, status=query_status)
    end if
    call finalize_collision_query(query_status, status)
  end subroutine find_first_hit

  !> 通常メッシュに対する最初の命中要素探索を行う。
  subroutine find_first_hit_base(mesh, p0, p1, hit, box_min, box_max, require_elem_inside, status)
    type(mesh_type), intent(in) :: mesh
    real(dp), intent(in) :: p0(3), p1(3)
    type(hit_info), intent(out) :: hit
    real(dp), intent(in), optional :: box_min(3), box_max(3)
    logical, intent(in), optional :: require_elem_inside
    integer(i32), intent(out), optional :: status

    real(dp) :: d(3), seg_min(3), seg_max(3), best_t
    real(dp) :: box_min_local(3), box_max_local(3), box_tol
    logical :: use_box_filter, require_inside_elem
    integer(i32) :: query_status

    call initialize_hit(hit)
    query_status = collision_query_ok

    d = p1 - p0
    seg_min = min(p0, p1)
    seg_max = max(p0, p1)
    best_t = huge(1.0d0)

    call resolve_box_filter_args( &
      box_min, box_max, require_elem_inside, use_box_filter, box_min_local, box_max_local, box_tol, require_inside_elem &
      )

    if (mesh%use_collision_grid) then
      call find_first_hit_base_grid( &
        mesh, p0, p1, d, seg_min, seg_max, hit, best_t, &
        use_box_filter, box_min_local, box_max_local, box_tol, require_inside_elem, query_status &
        )
    else
      call find_first_hit_base_linear( &
        mesh, p0, p1, seg_min, seg_max, hit, best_t, &
        use_box_filter, box_min_local, box_max_local, box_tol, require_inside_elem &
        )
    end if
    call finalize_collision_query(query_status, status)
  end subroutine find_first_hit_base

  !> periodic2 用に image shift を列挙し、base collision の結果を統合する。
  subroutine find_first_hit_periodic2(mesh, p0, p1, hit, sim, box_min, box_max, require_elem_inside, status)
    type(mesh_type), intent(in) :: mesh
    real(dp), intent(in) :: p0(3), p1(3)
    type(hit_info), intent(out) :: hit
    type(sim_config), intent(in) :: sim
    real(dp), intent(in), optional :: box_min(3), box_max(3)
    logical, intent(in), optional :: require_elem_inside
    integer(i32), intent(out), optional :: status

    integer(i32) :: periodic_axes(2), image_shift(2), nmin(2), nmax(2), iaxis, bound_status, base_status
    integer(i64) :: image_count(2), total_image_count, n1, n2
    real(dp) :: periodic_len(2), shift_vec(3), candidate_pos(3), candidate_wrapped(3)
    real(dp) :: box_min_local(3), box_max_local(3), box_tol
    real(dp) :: shifted_p0(3), shifted_p1(3)
    type(hit_info) :: candidate
    logical :: use_periodic2, use_box_filter, require_inside_elem

    call initialize_hit(hit)
    if (present(status)) status = collision_query_ok
    if (.not. mesh%periodic2_collision_ready) then
      error stop 'periodic2 collision requires prepare_periodic2_collision_mesh before ray queries.'
    end if

    call resolve_periodic2_collision_config(sim, use_periodic2, periodic_axes, periodic_len)
    if (.not. use_periodic2) then
      call find_first_hit_base(mesh, p0, p1, hit, box_min, box_max, require_elem_inside, status=base_status)
      call finalize_collision_query(base_status, status)
      return
    end if

    call resolve_box_filter_args( &
      box_min, box_max, require_elem_inside, use_box_filter, box_min_local, box_max_local, box_tol, require_inside_elem &
      )

    do iaxis = 1, 2
      call compute_periodic_shift_bounds( &
        mesh, p0, p1, periodic_axes(iaxis), periodic_len(iaxis), nmin(iaxis), nmax(iaxis), bound_status &
        )
      if (bound_status /= collision_query_ok) then
        call finalize_collision_query(bound_status, status)
        return
      end if
      if (nmin(iaxis) > nmax(iaxis)) return
    end do
    image_count = int(nmax, kind=i64) - int(nmin, kind=i64) + 1_i64
    if (any(image_count <= 0_i64)) return
    if (any(image_count > max_periodic2_collision_images)) then
      call finalize_collision_query(collision_query_image_limit, status)
      return
    end if
    total_image_count = image_count(1)*image_count(2)
    if (total_image_count > max_periodic2_collision_images) then
      call finalize_collision_query(collision_query_image_limit, status)
      return
    end if

    do n1 = int(nmin(1), i64), int(nmax(1), i64)
      do n2 = int(nmin(2), i64), int(nmax(2), i64)
        image_shift = [int(n1, i32), int(n2, i32)]
        shift_vec = 0.0d0
        shift_vec(periodic_axes(1)) = real(image_shift(1), dp)*periodic_len(1)
        shift_vec(periodic_axes(2)) = real(image_shift(2), dp)*periodic_len(2)
        shifted_p0 = p0 - shift_vec
        shifted_p1 = p1 - shift_vec

        call find_first_hit_base(mesh, shifted_p0, shifted_p1, candidate, status=base_status)
        if (base_status /= collision_query_ok) then
          call finalize_collision_query(base_status, status)
          return
        end if
        if (.not. candidate%has_hit) cycle

        candidate_pos = candidate%pos + shift_vec
        candidate_wrapped = candidate_pos
        call wrap_periodic2_point(candidate_wrapped, sim%box_min, periodic_axes, periodic_len)
        if (use_box_filter) then
          if (.not. point_inside_box_periodic2( &
              candidate_wrapped, box_min_local, box_max_local, box_tol, periodic_axes, require_inside_elem)) cycle
        end if

        if (prefer_periodic_candidate(candidate%t, candidate%elem_idx, image_shift, hit)) then
          hit%has_hit = .true.
          hit%elem_idx = candidate%elem_idx
          hit%t = candidate%t
          hit%pos = candidate_pos
          hit%pos_wrapped = candidate_wrapped
          hit%image_shift = image_shift
        end if
      end do
    end do
  end subroutine find_first_hit_periodic2

  !> 線分のAABBと要素AABBの重なりを先に判定し、詳細交差計算を枝刈りする。
  !! @param[in] p0 線分始点（粒子の移動前位置） [m]。
  !! @param[in] p1 線分終点（粒子の移動後候補位置） [m]。
  !! @param[in] bb_min 要素AABBの最小座標。
  !! @param[in] bb_max 要素AABBの最大座標。
  !! @return segment_bbox_overlap 関数の戻り値。
  pure logical function segment_bbox_overlap(p0, p1, bb_min, bb_max)
    real(dp), intent(in) :: p0(3), p1(3), bb_min(3), bb_max(3)
    real(dp) :: seg_min(3), seg_max(3)
    seg_min = min(p0, p1)
    seg_max = max(p0, p1)
    segment_bbox_overlap = segment_bbox_overlap_precomputed(seg_min, seg_max, bb_min, bb_max)
  end function segment_bbox_overlap

  !> 事前計算済みの線分AABBと要素AABBの重なりを判定する。
  pure logical function segment_bbox_overlap_precomputed(seg_min, seg_max, bb_min, bb_max)
    real(dp), intent(in) :: seg_min(3), seg_max(3), bb_min(3), bb_max(3)
    segment_bbox_overlap_precomputed = all(bb_max >= seg_min) .and. all(bb_min <= seg_max)
  end function segment_bbox_overlap_precomputed

  !> 旧実装と同じ線形探索で最初の命中要素を探索する。
  subroutine find_first_hit_base_linear( &
    mesh, p0, p1, seg_min, seg_max, hit, best_t, use_box_filter, box_min, box_max, box_tol, require_elem_inside &
    )
    type(mesh_type), intent(in) :: mesh
    real(dp), intent(in) :: p0(3), p1(3), seg_min(3), seg_max(3)
    type(hit_info), intent(inout) :: hit
    real(dp), intent(inout) :: best_t
    logical, intent(in) :: use_box_filter
    real(dp), intent(in) :: box_min(3), box_max(3), box_tol
    logical, intent(in) :: require_elem_inside

    integer(i32) :: i
    logical :: ok
    real(dp) :: t, h(3)

    do i = 1, mesh%nelem
      if (use_box_filter) then
        if (.not. segment_bbox_overlap_precomputed(mesh%bb_min(:, i), mesh%bb_max(:, i), box_min, box_max)) cycle
        if (require_elem_inside) then
          if (.not. bbox_inside_box(mesh%bb_min(:, i), mesh%bb_max(:, i), box_min, box_max, box_tol)) cycle
        end if
      end if
      if (.not. segment_bbox_overlap_precomputed(seg_min, seg_max, mesh%bb_min(:, i), mesh%bb_max(:, i))) cycle
      call segment_triangle_intersect(p0, p1, mesh%v0(:, i), mesh%v1(:, i), mesh%v2(:, i), ok, t, h)
      if (.not. ok) cycle
      if (use_box_filter) then
        if (.not. point_inside_box(h, box_min, box_max, box_tol)) cycle
      end if
      if (t < best_t) then
        best_t = t
        hit%has_hit = .true.
        hit%elem_idx = i
        hit%t = t
        hit%pos = h
        hit%pos_wrapped = h
        hit%image_shift = 0_i32
      end if
    end do
  end subroutine find_first_hit_base_linear

  !> 一様グリッド + 3D-DDA で候補セルのみ探索し、最初の命中要素を返す。
  subroutine find_first_hit_base_grid( &
    mesh, p0, p1, d, seg_min, seg_max, hit, best_t, use_box_filter, box_min, box_max, box_tol, require_elem_inside, status &
    )
    type(mesh_type), intent(in) :: mesh
    real(dp), intent(in) :: p0(3), p1(3), d(3), seg_min(3), seg_max(3)
    type(hit_info), intent(inout) :: hit
    real(dp), intent(inout) :: best_t
    logical, intent(in) :: use_box_filter
    real(dp), intent(in) :: box_min(3), box_max(3), box_tol
    logical, intent(in) :: require_elem_inside
    integer(i32), intent(out) :: status

    real(dp), parameter :: t_eps = 1.0d-12
    real(dp), parameter :: axis_rel_eps = 64.0d0*epsilon(1.0d0)
    real(dp) :: t_entry, t_exit, t_cur, t_next
    real(dp) :: t_max(3), t_delta(3), cell_size
    real(dp) :: t, h(3), p_entry(3)
    real(dp) :: axis_eps
    integer(i32) :: axis, nx, ny, cid
    integer(i32) :: cell(3), step(3)
    integer(i32) :: k, start_idx, end_idx, elem_idx
    integer(i64) :: align_iterations, traversal_iterations, max_traversal_iterations
    logical :: ok, hit_grid, advanced_cell

    status = collision_query_ok
    if (any(mesh%grid_ncell <= 0_i32) .or. &
        any(.not. ieee_is_finite(mesh%grid_inv_cell)) .or. any(mesh%grid_inv_cell <= 0.0d0) .or. &
        any(.not. ieee_is_finite(mesh%grid_bb_min)) .or. any(.not. ieee_is_finite(mesh%grid_bb_max))) then
      call report_collision_grid_stall('invalid_grid_geometry', mesh, p0, p1)
      status = collision_query_grid_stalled
      return
    end if

    call segment_aabb_intersection_t(p0, d, mesh%grid_bb_min, mesh%grid_bb_max, hit_grid, t_entry, t_exit)
    if (.not. hit_grid) return
    if (.not. ieee_is_finite(t_entry) .or. .not. ieee_is_finite(t_exit)) then
      call report_collision_grid_stall( &
        'nonfinite_aabb_interval', mesh, p0, p1, t_entry=t_entry, t_exit=t_exit &
        )
      status = collision_query_grid_stalled
      return
    end if
    if (t_exit < 0.0d0 .or. t_entry > 1.0d0) return

    t_cur = max(0.0d0, t_entry)
    if (t_cur > t_exit) return

    p_entry = p0 + t_cur*d
    axis_eps = axis_rel_eps*max( &
               maxval(abs(d)), &
               maxval(abs(mesh%grid_bb_max - mesh%grid_bb_min)), &
               tiny(1.0d0) &
               )
    cell = 0_i32
    step = 0_i32
    t_max = huge(1.0d0)
    t_delta = huge(1.0d0)
    do axis = 1, 3
      cell(axis) = coord_to_cell(mesh, p_entry(axis), int(axis, kind=i32))
      if (abs(d(axis)) <= axis_eps) then
        step(axis) = 0_i32
        t_max(axis) = huge(1.0d0)
        t_delta(axis) = huge(1.0d0)
      else
        cell_size = 1.0d0/mesh%grid_inv_cell(axis)
        if (.not. ieee_is_finite(cell_size) .or. cell_size <= 0.0d0) then
          call report_collision_grid_stall( &
            'invalid_cell_size', mesh, p0, p1, cell=cell, step=step, axis=axis, &
            t_entry=t_entry, t_exit=t_exit, t_cur=t_cur &
            )
          status = collision_query_grid_stalled
          return
        end if
        if (d(axis) > 0.0d0) then
          step(axis) = 1_i32
          t_max(axis) = (mesh%grid_bb_min(axis) + real(cell(axis), dp)*cell_size - p0(axis))/d(axis)
          t_delta(axis) = cell_size/d(axis)
        else
          step(axis) = -1_i32
          t_max(axis) = (mesh%grid_bb_min(axis) + real(cell(axis) - 1_i32, dp)*cell_size - p0(axis))/d(axis)
          t_delta(axis) = -cell_size/d(axis)
        end if
        if (.not. ieee_is_finite(t_max(axis)) .or. .not. ieee_is_finite(t_delta(axis)) .or. &
            t_delta(axis) <= 0.0d0) then
          call report_collision_grid_stall( &
            'invalid_axis_progress', mesh, p0, p1, cell=cell, step=step, axis=axis, &
            t_entry=t_entry, t_exit=t_exit, t_cur=t_cur, t_max=t_max, t_delta=t_delta &
            )
          status = collision_query_grid_stalled
          return
        end if
        align_iterations = 0_i64
        do while (t_max(axis) < t_cur - t_eps)
          align_iterations = align_iterations + 1_i64
          if (align_iterations > int(mesh%grid_ncell(axis), i64) + 1_i64) then
            call report_collision_grid_stall( &
              'align_iteration_limit', mesh, p0, p1, cell=cell, step=step, axis=axis, &
              iterations=align_iterations, max_iterations=int(mesh%grid_ncell(axis), i64) + 1_i64, &
              t_entry=t_entry, t_exit=t_exit, t_cur=t_cur, t_max=t_max, t_delta=t_delta &
              )
            status = collision_query_grid_stalled
            return
          end if
          t_max(axis) = t_max(axis) + t_delta(axis)
          if (.not. ieee_is_finite(t_max(axis))) then
            call report_collision_grid_stall( &
              'nonfinite_t_max_after_align', mesh, p0, p1, cell=cell, step=step, axis=axis, &
              iterations=align_iterations, t_entry=t_entry, t_exit=t_exit, t_cur=t_cur, &
              t_max=t_max, t_delta=t_delta &
              )
            status = collision_query_grid_stalled
            return
          end if
        end do
      end if
    end do

    nx = mesh%grid_ncell(1)
    ny = mesh%grid_ncell(2)
    traversal_iterations = 0_i64
    max_traversal_iterations = sum(int(mesh%grid_ncell, i64)) + 3_i64

    do
      traversal_iterations = traversal_iterations + 1_i64
      if (traversal_iterations > max_traversal_iterations) then
        call report_collision_grid_stall( &
          'traversal_iteration_limit', mesh, p0, p1, cell=cell, step=step, &
          iterations=traversal_iterations, max_iterations=max_traversal_iterations, &
          t_entry=t_entry, t_exit=t_exit, t_cur=t_cur, t_max=t_max, t_delta=t_delta &
          )
        status = collision_query_grid_stalled
        return
      end if
      if (.not. ieee_is_finite(t_cur)) then
        call report_collision_grid_stall( &
          'nonfinite_t_cur', mesh, p0, p1, cell=cell, step=step, &
          iterations=traversal_iterations, t_entry=t_entry, t_exit=t_exit, t_cur=t_cur, &
          t_max=t_max, t_delta=t_delta &
          )
        status = collision_query_grid_stalled
        return
      end if
      if (t_cur > t_exit + t_eps) exit
      if (t_cur > best_t + t_eps) exit

      if (any(cell < 1_i32) .or. any(cell > mesh%grid_ncell)) then
        call report_collision_grid_stall( &
          'cell_out_of_range', mesh, p0, p1, cell=cell, step=step, &
          iterations=traversal_iterations, t_entry=t_entry, t_exit=t_exit, t_cur=t_cur, &
          t_max=t_max, t_delta=t_delta &
          )
        status = collision_query_grid_stalled
        return
      end if

      cid = cell_id(cell(1), cell(2), cell(3), nx, ny)
      start_idx = mesh%grid_cell_start(cid)
      end_idx = mesh%grid_cell_start(cid + 1_i32) - 1_i32
      do k = start_idx, end_idx
        elem_idx = mesh%grid_cell_elem(k)
        if (use_box_filter) then
          if (.not. segment_bbox_overlap_precomputed( &
              mesh%bb_min(:, elem_idx), mesh%bb_max(:, elem_idx), box_min, box_max)) cycle
          if (require_elem_inside) then
            if (.not. bbox_inside_box( &
                mesh%bb_min(:, elem_idx), mesh%bb_max(:, elem_idx), box_min, box_max, box_tol)) cycle
          end if
        end if
        if (.not. segment_bbox_overlap_precomputed( &
            seg_min, seg_max, mesh%bb_min(:, elem_idx), mesh%bb_max(:, elem_idx))) cycle
        call segment_triangle_intersect( &
          p0, p1, mesh%v0(:, elem_idx), mesh%v1(:, elem_idx), mesh%v2(:, elem_idx), ok, t, h &
          )
        if (.not. ok) cycle
        if (use_box_filter) then
          if (.not. point_inside_box(h, box_min, box_max, box_tol)) cycle
        end if
        if (t < best_t) then
          best_t = t
          hit%has_hit = .true.
          hit%elem_idx = elem_idx
          hit%t = t
          hit%pos = h
          hit%pos_wrapped = h
          hit%image_shift = 0_i32
        end if
      end do

      t_next = min(t_max(1), min(t_max(2), t_max(3)))
      if (.not. ieee_is_finite(t_next) .or. t_next < t_cur - t_eps) then
        call report_collision_grid_stall( &
          'invalid_t_next', mesh, p0, p1, cell=cell, step=step, &
          iterations=traversal_iterations, t_entry=t_entry, t_exit=t_exit, t_cur=t_cur, t_next=t_next, &
          t_max=t_max, t_delta=t_delta &
          )
        status = collision_query_grid_stalled
        return
      end if
      if (t_next > t_exit + t_eps) exit
      if (t_next > best_t + t_eps) exit

      advanced_cell = .false.
      do axis = 1, 3
        if (t_max(axis) <= t_next + t_eps) then
          if (step(axis) /= 0_i32) then
            advanced_cell = .true.
            cell(axis) = cell(axis) + step(axis)
            if (cell(axis) < 1_i32 .or. cell(axis) > mesh%grid_ncell(axis)) return
            t_max(axis) = t_max(axis) + t_delta(axis)
          end if
        end if
      end do
      if (.not. advanced_cell) then
        call report_collision_grid_stall( &
          'no_cell_advanced', mesh, p0, p1, cell=cell, step=step, &
          iterations=traversal_iterations, t_entry=t_entry, t_exit=t_exit, t_cur=t_cur, t_next=t_next, &
          t_max=t_max, t_delta=t_delta &
          )
        status = collision_query_grid_stalled
        return
      end if
      t_cur = t_next
    end do
  end subroutine find_first_hit_base_grid

  !> `BEACH_COLLISION_DIAGNOSTICS=1` のときだけ、grid DDA停止位置の内部状態を出力する。
  subroutine report_collision_grid_stall( &
    reason, mesh, p0, p1, cell, step, axis, iterations, max_iterations, &
    t_entry, t_exit, t_cur, t_next, t_max, t_delta &
    )
    character(len=*), intent(in) :: reason
    type(mesh_type), intent(in) :: mesh
    real(dp), intent(in) :: p0(3), p1(3)
    integer(i32), intent(in), optional :: cell(3), step(3), axis
    integer(i64), intent(in), optional :: iterations, max_iterations
    real(dp), intent(in), optional :: t_entry, t_exit, t_cur, t_next
    real(dp), intent(in), optional :: t_max(3), t_delta(3)

    character(len=32) :: env_value
    integer :: env_length, env_status

    env_value = ''
    call get_environment_variable( &
      'BEACH_COLLISION_DIAGNOSTICS', env_value, length=env_length, status=env_status &
      )
    if (env_status /= 0 .or. env_length <= 0) return
    select case (trim(lower_ascii(env_value)))
    case ('1', 'true', 'yes', 'on')
      continue
    case default
      return
    end select

    !$omp critical (beach_collision_grid_diagnostic)
    write (error_unit, '(a,a)') 'collision grid diagnostic: reason=', trim(reason)
    write (error_unit, '(a,3(1x,es24.16))') '  p0=', p0
    write (error_unit, '(a,3(1x,es24.16))') '  p1=', p1
    write (error_unit, '(a,3(1x,i0))') '  grid_ncell=', mesh%grid_ncell
    write (error_unit, '(a,3(1x,es24.16))') '  grid_bb_min=', mesh%grid_bb_min
    write (error_unit, '(a,3(1x,es24.16))') '  grid_bb_max=', mesh%grid_bb_max
    write (error_unit, '(a,3(1x,es24.16))') '  grid_inv_cell=', mesh%grid_inv_cell
    if (present(cell)) write (error_unit, '(a,3(1x,i0))') '  cell=', cell
    if (present(step)) write (error_unit, '(a,3(1x,i0))') '  step=', step
    if (present(axis)) write (error_unit, '(a,i0)') '  axis=', axis
    if (present(iterations)) write (error_unit, '(a,i0)') '  iterations=', iterations
    if (present(max_iterations)) write (error_unit, '(a,i0)') '  max_iterations=', max_iterations
    if (present(t_entry)) write (error_unit, '(a,es24.16)') '  t_entry=', t_entry
    if (present(t_exit)) write (error_unit, '(a,es24.16)') '  t_exit=', t_exit
    if (present(t_cur)) write (error_unit, '(a,es24.16)') '  t_cur=', t_cur
    if (present(t_next)) write (error_unit, '(a,es24.16)') '  t_next=', t_next
    if (present(t_max)) write (error_unit, '(a,3(1x,es24.16))') '  t_max=', t_max
    if (present(t_delta)) write (error_unit, '(a,3(1x,es24.16))') '  t_delta=', t_delta
    flush (error_unit)
    !$omp end critical (beach_collision_grid_diagnostic)
  end subroutine report_collision_grid_stall

  !> 線分 `p(t)=p0+t*d` (`0<=t<=1`) とAABBの交差区間 `[t_entry,t_exit]` を返す。
  pure subroutine segment_aabb_intersection_t(p0, d, bb_min, bb_max, ok, t_entry, t_exit)
    real(dp), intent(in) :: p0(3), d(3), bb_min(3), bb_max(3)
    logical, intent(out) :: ok
    real(dp), intent(out) :: t_entry, t_exit

    real(dp), parameter :: axis_rel_eps = 64.0d0*epsilon(1.0d0)
    real(dp) :: t0, t1, t_near, t_far, inv_d, tmp
    real(dp) :: axis_eps
    integer(i32) :: axis

    t0 = 0.0d0
    t1 = 1.0d0
    axis_eps = axis_rel_eps*max( &
               maxval(abs(d)), &
               maxval(abs(bb_max - bb_min)), &
               tiny(1.0d0) &
               )
    do axis = 1, 3
      if (abs(d(axis)) <= axis_eps) then
        if (p0(axis) < bb_min(axis) .or. p0(axis) > bb_max(axis)) then
          ok = .false.
          t_entry = 0.0d0
          t_exit = -1.0d0
          return
        end if
      else
        inv_d = 1.0d0/d(axis)
        t_near = (bb_min(axis) - p0(axis))*inv_d
        t_far = (bb_max(axis) - p0(axis))*inv_d
        if (t_near > t_far) then
          tmp = t_near
          t_near = t_far
          t_far = tmp
        end if
        if (t_near > t0) t0 = t_near
        if (t_far < t1) t1 = t_far
        if (t0 > t1) then
          ok = .false.
          t_entry = 0.0d0
          t_exit = -1.0d0
          return
        end if
      end if
    end do

    ok = .true.
    t_entry = t0
    t_exit = t1
  end subroutine segment_aabb_intersection_t

  !> 座標をグリッドセル添字へ変換し、範囲外は端セルへ丸める。
  pure integer(i32) function coord_to_cell(mesh, x, axis) result(idx)
    type(mesh_type), intent(in) :: mesh
    real(dp), intent(in) :: x
    integer(i32), intent(in) :: axis
    real(dp) :: u

    if (x <= mesh%grid_bb_min(axis)) then
      idx = 1_i32
      return
    end if
    if (x >= mesh%grid_bb_max(axis)) then
      idx = mesh%grid_ncell(axis)
      return
    end if

    u = (x - mesh%grid_bb_min(axis))*mesh%grid_inv_cell(axis)
    idx = int(u, kind=i32) + 1_i32
    if (idx < 1_i32) idx = 1_i32
    if (idx > mesh%grid_ncell(axis)) idx = mesh%grid_ncell(axis)
  end function coord_to_cell

  !> 3次元セル添字 `(ix,iy,iz)` をCSR一次元インデックスへ変換する。
  pure integer(i32) function cell_id(ix, iy, iz, nx, ny) result(cid)
    integer(i32), intent(in) :: ix, iy, iz, nx, ny
    cid = (iz - 1_i32)*(nx*ny) + (iy - 1_i32)*nx + ix
  end function cell_id

  pure logical function point_inside_box(p, box_min, box_max, tol)
    real(dp), intent(in) :: p(3), box_min(3), box_max(3), tol
    point_inside_box = all(p >= (box_min - tol)) .and. all(p <= (box_max + tol))
  end function point_inside_box

  pure logical function point_inside_box_periodic2(p, box_min, box_max, tol, periodic_axes, require_half_open)
    real(dp), intent(in) :: p(3), box_min(3), box_max(3), tol
    integer(i32), intent(in) :: periodic_axes(2)
    logical, intent(in) :: require_half_open
    integer(i32) :: axis
    logical :: is_periodic

    point_inside_box_periodic2 = .true.
    do axis = 1_i32, 3_i32
      is_periodic = any(periodic_axes == axis)
      if (require_half_open .and. is_periodic) then
        if (p(axis) < box_min(axis) - tol .or. p(axis) >= box_max(axis) + tol) then
          point_inside_box_periodic2 = .false.
          return
        end if
      else
        if (p(axis) < box_min(axis) - tol .or. p(axis) > box_max(axis) + tol) then
          point_inside_box_periodic2 = .false.
          return
        end if
      end if
    end do
  end function point_inside_box_periodic2

  pure logical function bbox_inside_box(bb_min, bb_max, box_min, box_max, tol)
    real(dp), intent(in) :: bb_min(3), bb_max(3), box_min(3), box_max(3), tol
    bbox_inside_box = all(bb_min >= (box_min - tol)) .and. all(bb_max <= (box_max + tol))
  end function bbox_inside_box

  !> Möller–Trumbore法で線分と三角形の交差有無・線分パラメータ `t`・交点座標を計算する。
  !! @param[in] p0 線分始点（粒子の移動前位置） [m]。
  !! @param[in] p1 線分終点（粒子の移動後候補位置） [m]。
  !! @param[in] v0 三角形頂点0の座標。
  !! @param[in] v1 三角形頂点1の座標。
  !! @param[in] v2 三角形頂点2の座標。
  !! @param[out] ok 線分と三角形が交差した場合に `.true.`。
  !! @param[out] t 交点の線分内パラメータ（`p0 + t*(p1-p0)`）。
  !! @param[out] h 交点座標。
  subroutine segment_triangle_intersect(p0, p1, v0, v1, v2, ok, t, h)
    real(dp), intent(in) :: p0(3), p1(3), v0(3), v1(3), v2(3)
    logical, intent(out) :: ok
    real(dp), intent(out) :: t, h(3)

    real(dp), parameter :: det_rel_eps = 64.0d0*epsilon(1.0d0)
    real(dp) :: d(3), e1(3), e2(3), q(3), s(3), hh(3), a, f, u, v
    real(dp) :: det_scale

    d = p1 - p0
    e1 = v1 - v0
    e2 = v2 - v0
    hh = cross(d, e2)
    a = dot_product(e1, hh)
    det_scale = sqrt(sum(d*d))*sqrt(sum(e1*e1))*sqrt(sum(e2*e2))
    if (det_scale <= tiny(1.0d0)) then
      ok = .false.; return
    end if
    if (abs(a) <= det_rel_eps*det_scale) then
      ok = .false.; return
    end if

    f = 1.0d0/a
    s = p0 - v0
    u = f*dot_product(s, hh)
    if (u < 0.0d0 .or. u > 1.0d0) then
      ok = .false.; return
    end if

    q = cross(s, e1)
    v = f*dot_product(d, q)
    if (v < 0.0d0 .or. (u + v) > 1.0d0) then
      ok = .false.; return
    end if

    t = f*dot_product(e2, q)
    if (t < 0.0d0 .or. t > 1.0d0) then
      ok = .false.; return
    end if

    h = p0 + t*d
    ok = .true.
  end subroutine segment_triangle_intersect

  pure function cross(a, b) result(c)
    real(dp), intent(in) :: a(3), b(3)
    real(dp) :: c(3)
    c(1) = a(2)*b(3) - a(3)*b(2)
    c(2) = a(3)*b(1) - a(1)*b(3)
    c(3) = a(1)*b(2) - a(2)*b(1)
  end function cross

  !> hit 構造体を未命中状態へ初期化する。
  subroutine initialize_hit(hit)
    type(hit_info), intent(out) :: hit

    hit%has_hit = .false.
    hit%elem_idx = -1_i32
    hit%t = 0.0d0
    hit%pos = 0.0d0
    hit%image_shift = 0_i32
    hit%pos_wrapped = 0.0d0
  end subroutine initialize_hit

  !> box filter 関連の optional 引数を検証付きで展開する。
  subroutine resolve_box_filter_args( &
    box_min, box_max, require_elem_inside, use_box_filter, box_min_local, box_max_local, box_tol, require_inside_elem &
    )
    real(dp), intent(in), optional :: box_min(3), box_max(3)
    logical, intent(in), optional :: require_elem_inside
    logical, intent(out) :: use_box_filter, require_inside_elem
    real(dp), intent(out) :: box_min_local(3), box_max_local(3), box_tol

    use_box_filter = present(box_min) .or. present(box_max)
    if (use_box_filter .and. .not. (present(box_min) .and. present(box_max))) then
      error stop 'find_first_hit requires both box_min and box_max when using box filter.'
    end if
    if (use_box_filter) then
      box_min_local = box_min
      box_max_local = box_max
      box_tol = 1.0d-12*max(1.0d0, maxval(abs(box_max_local - box_min_local)))
    else
      box_min_local = 0.0d0
      box_max_local = 0.0d0
      box_tol = 0.0d0
    end if

    require_inside_elem = .false.
    if (present(require_elem_inside)) require_inside_elem = require_elem_inside
    if (require_inside_elem .and. .not. use_box_filter) then
      error stop 'find_first_hit require_elem_inside=true needs box_min/box_max.'
    end if
  end subroutine resolve_box_filter_args

  !> periodic2 collision で必要な 2 軸周期設定を解決する。
  subroutine resolve_periodic2_collision_config(sim, use_periodic2, periodic_axes, periodic_len)
    type(sim_config), intent(in) :: sim
    logical, intent(out) :: use_periodic2
    integer(i32), intent(out) :: periodic_axes(2)
    real(dp), intent(out) :: periodic_len(2)

    character(len=16) :: field_bc_mode
    integer(i32) :: axis, n_periodic
    real(dp) :: span

    use_periodic2 = .false.
    periodic_axes = 0_i32
    periodic_len = 0.0d0
    field_bc_mode = lower_ascii(trim(sim%field_bc_mode))
    if (trim(field_bc_mode) /= 'periodic2') return

    if (.not. sim%use_box) then
      error stop 'sim.field_bc_mode="periodic2" requires sim.use_box=true.'
    end if

    n_periodic = 0_i32
    do axis = 1_i32, 3_i32
      if ((sim%bc_low(axis) == bc_periodic) .neqv. (sim%bc_high(axis) == bc_periodic)) then
        error stop 'periodic2 requires bc_low(axis)=bc_high(axis)=periodic for periodic axes.'
      end if
      if (sim%bc_low(axis) == bc_periodic) then
        n_periodic = n_periodic + 1_i32
        if (n_periodic <= 2_i32) periodic_axes(n_periodic) = axis
      end if
    end do
    if (n_periodic /= 2_i32) then
      error stop 'sim.field_bc_mode="periodic2" requires exactly two periodic axes.'
    end if

    do axis = 1_i32, 2_i32
      span = sim%box_max(periodic_axes(axis)) - sim%box_min(periodic_axes(axis))
      if (span <= 0.0d0) error stop 'periodic2 requires positive box length on periodic axes.'
      periodic_len(axis) = span
    end do

    use_periodic2 = .true.
  end subroutine resolve_periodic2_collision_config

  !> 線分 AABB と canonical mesh AABB の重なりから必要な image shift 範囲を決める。
  subroutine compute_periodic_shift_bounds(mesh, p0, p1, axis, period_len, nmin, nmax, status)
    type(mesh_type), intent(in) :: mesh
    real(dp), intent(in) :: p0(3), p1(3)
    integer(i32), intent(in) :: axis
    real(dp), intent(in) :: period_len
    integer(i32), intent(out) :: nmin, nmax
    integer(i32), intent(out), optional :: status

    integer(i32) :: query_status
    integer(i64) :: nmin_i64, nmax_i64, i32_min_i64, i32_max_i64
    real(dp) :: seg_min, seg_max, mesh_min, mesh_max, tol, lower_bound, upper_bound, i64_limit

    nmin = 0_i32
    nmax = -1_i32
    query_status = collision_query_ok
    seg_min = min(p0(axis), p1(axis))
    seg_max = max(p0(axis), p1(axis))
    mesh_min = mesh%grid_bb_min(axis)
    mesh_max = mesh%grid_bb_max(axis)
    tol = 1.0d-12*max(1.0d0, abs(seg_min), abs(seg_max), abs(mesh_min), abs(mesh_max), period_len)

    lower_bound = (seg_min - mesh_max - tol)/period_len
    upper_bound = (seg_max - mesh_min + tol)/period_len
    if (.not. ieee_is_finite(lower_bound) .or. .not. ieee_is_finite(upper_bound)) then
      call finalize_collision_query(collision_query_index_range, status)
      return
    end if

    i64_limit = real(huge(0_i64), dp)
    if (lower_bound <= -i64_limit .or. lower_bound >= i64_limit .or. &
        upper_bound <= -i64_limit .or. upper_bound >= i64_limit) then
      call finalize_collision_query(collision_query_index_range, status)
      return
    end if

    nmin_i64 = ceiling(lower_bound, kind=i64)
    nmax_i64 = floor(upper_bound, kind=i64)
    i32_max_i64 = int(huge(0_i32), i64)
    i32_min_i64 = -i32_max_i64 - 1_i64
    if (nmin_i64 < i32_min_i64 .or. nmin_i64 > i32_max_i64 .or. &
        nmax_i64 < i32_min_i64 .or. nmax_i64 > i32_max_i64) then
      call finalize_collision_query(collision_query_index_range, status)
      return
    end if

    nmin = int(nmin_i64, i32)
    nmax = int(nmax_i64, i32)
    call finalize_collision_query(query_status, status)
  end subroutine compute_periodic_shift_bounds

  !> 不完全な照会を status 要求元へ返し、未要求の既存 caller は fail closed で停止する。
  subroutine finalize_collision_query(query_status, status)
    integer(i32), intent(in) :: query_status
    integer(i32), intent(out), optional :: status

    if (present(status)) then
      status = query_status
      return
    end if

    select case (query_status)
    case (collision_query_ok)
      continue
    case (collision_query_image_limit)
      error stop 'periodic2 collision query incomplete: image enumeration limit exceeded.'
    case (collision_query_index_range)
      error stop 'periodic2 collision query incomplete: image index is outside the supported i32 range.'
    case (collision_query_invalid_segment)
      error stop 'collision query incomplete: segment coordinates must be finite.'
    case (collision_query_grid_stalled)
      error stop 'collision query incomplete: collision-grid traversal failed to make bounded progress.'
    case default
      error stop 'periodic2 collision query incomplete: unknown collision query status.'
    end select
  end subroutine finalize_collision_query

  !> point を primary cell へ折り返す。
  subroutine wrap_periodic2_point(point, box_min, periodic_axes, periodic_len)
    real(dp), intent(inout) :: point(3)
    real(dp), intent(in) :: box_min(3), periodic_len(2)
    integer(i32), intent(in) :: periodic_axes(2)
    integer(i32) :: iaxis

    do iaxis = 1, 2
      point(periodic_axes(iaxis)) = box_min(periodic_axes(iaxis)) + &
                                    modulo(point(periodic_axes(iaxis)) - box_min(periodic_axes(iaxis)), periodic_len(iaxis))
    end do
  end subroutine wrap_periodic2_point

  !> 候補 hit が現在の best より優先されるかを deterministic に判定する。
  pure logical function prefer_periodic_candidate(t, elem_idx, image_shift, best_hit)
    real(dp), intent(in) :: t
    integer(i32), intent(in) :: elem_idx, image_shift(2)
    type(hit_info), intent(in) :: best_hit
    real(dp) :: tol

    if (.not. best_hit%has_hit) then
      prefer_periodic_candidate = .true.
      return
    end if

    tol = 1.0d-12*max(1.0d0, abs(t), abs(best_hit%t))
    if (t < best_hit%t - tol) then
      prefer_periodic_candidate = .true.
      return
    end if
    if (t > best_hit%t + tol) then
      prefer_periodic_candidate = .false.
      return
    end if

    if (elem_idx < best_hit%elem_idx) then
      prefer_periodic_candidate = .true.
      return
    end if
    if (elem_idx > best_hit%elem_idx) then
      prefer_periodic_candidate = .false.
      return
    end if

    if (image_shift(1) < best_hit%image_shift(1)) then
      prefer_periodic_candidate = .true.
    else if (image_shift(1) > best_hit%image_shift(1)) then
      prefer_periodic_candidate = .false.
    else
      prefer_periodic_candidate = image_shift(2) < best_hit%image_shift(2)
    end if
  end function prefer_periodic_candidate

end module bem_collision