module collision use typedef, only: & real64, int32 implicit none integer(kind=int32), parameter :: & ITMAX = 99, & ITCONV = 3 real(kind=real64), parameter :: & EPS = 1.d-12, & CONV = 1.d-2 contains pure function inertia2axes(inertia, mass, radius) result(axes) use typedef, only: & real64 implicit None real(kind=real64), parameter :: & eps = 1.d-15, & del = 1.d0 + eps real(kind=real64), dimension(3), intent(in) :: & inertia real(kind=real64), intent(in) :: & mass, radius real(kind=real64), dimension(3) :: & axes character(len=80) :: & s if (inertia(1) == 0.d0) then axes(:) = radius return endif axes(:) = sqrt((inertia((/3, 3, 2/)) & + inertia((/2, 1, 1/)) & - inertia((/1, 2, 3/))) * (2.5d0 / mass)) axes(:) = axes(:) * product(axes(:))**(-1.d0/3.d0) * radius if ((axes(2) > axes(1) * del) .or. & (axes(3) > axes(2) * del)) then write(s, '(1p,3(" ",E25.17))') axes(1:3) error stop ' [inertia2axes] inverted axes: ' // s endif end function inertia2axes pure function axes2bodies(axes, rotation) result(body) use typedef, only: & real64, int32 implicit none integer(kind=int32), parameter :: & ndim = 3 real(kind=real64), dimension(ndim), intent(in) :: & axes real(kind=real64), dimension(ndim,ndim), intent(in) :: & rotation real(kind=real64), dimension(ndim,ndim) :: & body integer(kind=int32) :: & i do i=1,3 body(i,:) = 1.d0 / axes(i)**2 * rotation(:,i) enddo body(:,:) = matmul(rotation(:,:), body(:,:)) end function axes2bodies pure function collided(d1, d2, r0) result(col) use typedef, only: & real64, int32, int64 use linalg, only: & inverse implicit none integer(kind=int32), parameter :: & ndim = 3 real(kind=real64), dimension(ndim), intent(in) :: & r0 real(kind=real64), dimension(ndim,ndim), intent(in) :: & d1, d2 logical :: & col ! local variables real(kind=real64) :: & lambda, f, g, p, dl, fmin, f0 real(kind=real64), dimension(ndim,ndim) :: & ii real(kind=real64), dimension(ndim) :: & x, r, v integer(kind=int32) :: & it, ntry ! find minimum distance lambda = 1.d0 fmin = 1.d0 x(:) = matmul(d2(:,:), r0(:)) ntry = 1 1000 continue f0 = 1.d99 do it = 1, ITMAX ii(:,:) = inverse(lambda * d1(:,:) + d2(:,:), int(ndim, kind=int64)) r(:) = -matmul(ii(:,:), x(:)) g = dot_product(r(:), matmul(d1(:,:), r(:))) - 1.d0 p = 2.d0 * dot_product(r(:), matmul(d1(:,:), matmul(ii(:,:), & matmul(d1(:,:), matmul(ii(:,:), x(:)))))) dl = - g / p * fmin ! check adjustments to dl and fmin if (abs(dl) > 0.5d0) then fmin = 0.5d0 * fmin dl = sign(0.5d0, dl) else if (abs(dl) < 0.1d0 * fmin) then fmin = min(1.d0, 2.d0 * fmin) endif ! apply updates and check termination lambda = lambda + dl v(:) = r0(:) + r(:) f = dot_product(v(:), matmul(d2(:,:), v(:))) if (abs(g) < EPS) exit ! stop iteration prematurely when step size << diff to 1 if ((abs(f - f0) < abs(f - 1.d0) * CONV * fmin).and.(it > ITCONV)) exit f0 = f enddo if (it >= ITMAX) then if (ntry == 1) then ntry = ntry + 1 fmin = 0.1d0 lambda = 0.d0 goto 1000 endif ! if this fails, try introduce fminmax ! error stop ' [collided] maximum iterations reached.' ! we now take the approach if the body is within reach and running carzy, ! it may well have collided. f = 0.d0 endif col = f <= 1.d0 end function collided pure function collision_inspector(inertias, masses, radii, rotations, distances, n, m) result(collisions) use, intrinsic :: & ieee_arithmetic, only: & ieee_is_nan use typedef, only: & int32, real64 implicit none integer(kind=int32), parameter :: & ndim = 3 real(kind=real64), dimension(ndim, n), intent(in) :: & inertias real(kind=real64), dimension(n), intent(in) :: & masses, radii real(kind=real64), dimension(ndim, m), intent(in) :: & distances real(kind=real64), dimension(ndim, ndim, n), intent(in) :: & rotations integer(kind=int32), intent(in) :: & n, m logical, dimension(m) :: & collisions ! local variables integer(kind=int32) :: & i, j, k, kk, i1, i2 logical, dimension(size(inertias, 2)) :: & threedim, calculated real(kind=real64) :: & rn real(kind=real64), dimension(ndim, size(inertias, 2)) :: & axes real(kind=real64), dimension(ndim, ndim, size(inertias, 2)) :: & bodies if ((n < 2).or.(m * 2 /= n * (n - 1))) & error stop ' [collsions] dimension error.' if (ieee_is_nan(distances(1,1))) then collisions(:) = .true. return endif calculated(:) = .false. do i=1,n if (inertias(1,i) == 0.d0) then axes(:,i) = radii(i) threedim(i) = .false. else axes(:,i) = inertia2axes(inertias(:,i), masses(i), radii(i)) threedim(i) = .true. endif end do i1 = 0 i2 = 1 do j=1,m i1 = i1 + 1 if (i1 == i2) then i1 = 1 i2 = i2 + 1 endif rn = norm2(distances(:, j)) if (rn < axes(3,i1) + axes(3,i2)) then collisions(j) = .true. cycle endif if (rn > axes(1,i1) + axes(1,i2)) then collisions(j) = .false. cycle endif do kk=1,2 if (kk == 1) then k = i1 else k = i2 end if if (calculated(k)) & cycle if (threedim(k)) then bodies(:, :, k) = axes2bodies(axes(:, k), rotations(:, :, k)) else bodies(:, :, k) = 0.d0 do i = 1, ndim bodies(i, i, k) = axes(1, k) enddo endif calculated(k) = .true. end do collisions(j) = collided(bodies(:,:,i1), bodies(:,:,i2), distances(:,j)) end do end function collision_inspector end module collision