NewtonInverse_3D Subroutine

  subroutine NewtonInverse_3D(geometry,iEl,N,xTarget,xi,converged)
    !! Newton inverse-map for the 3D SEM element iEl. See NewtonInverse_2D.
    implicit none
    type(SEMHex),intent(in) :: geometry
    integer,intent(in) :: iEl,N
    real(prec),intent(in) :: xTarget(3)
    real(prec),intent(out) :: xi(3)
    logical,intent(out) :: converged
    ! Local
    integer :: iter,i,j,k,r,c
    real(prec) :: lS(0:N),lT(0:N),lU(0:N)
    real(prec) :: xCur(3),jac(3,3),res(3),delta(3),inv(3,3)
    real(prec) :: w,det

    xi(1) = 0.0_prec
    xi(2) = 0.0_prec
    xi(3) = 0.0_prec
    converged = .false.

    do iter = 1,newtonMax
      lS = geometry%x%interp%CalculateLagrangePolynomials(xi(1))
      lT = geometry%x%interp%CalculateLagrangePolynomials(xi(2))
      lU = geometry%x%interp%CalculateLagrangePolynomials(xi(3))

      xCur(1) = 0.0_prec
      xCur(2) = 0.0_prec
      xCur(3) = 0.0_prec
      do c = 1,3
        do r = 1,3
          jac(r,c) = 0.0_prec
        enddo
      enddo

      do k = 1,N+1
        do j = 1,N+1
          do i = 1,N+1
            w = lS(i-1)*lT(j-1)*lU(k-1)
            xCur(1) = xCur(1)+w*geometry%x%interior(i,j,k,iEl,1,1)
            xCur(2) = xCur(2)+w*geometry%x%interior(i,j,k,iEl,1,2)
            xCur(3) = xCur(3)+w*geometry%x%interior(i,j,k,iEl,1,3)
            do c = 1,3
              do r = 1,3
                jac(r,c) = jac(r,c)+w*geometry%dxds%interior(i,j,k,iEl,1,r,c)
              enddo
            enddo
          enddo
        enddo
      enddo

      res(1) = xTarget(1)-xCur(1)
      res(2) = xTarget(2)-xCur(2)
      res(3) = xTarget(3)-xCur(3)

      ! Cofactor expansion for the 3x3 inverse.
      inv(1,1) = jac(2,2)*jac(3,3)-jac(2,3)*jac(3,2)
      inv(1,2) = jac(1,3)*jac(3,2)-jac(1,2)*jac(3,3)
      inv(1,3) = jac(1,2)*jac(2,3)-jac(1,3)*jac(2,2)
      inv(2,1) = jac(2,3)*jac(3,1)-jac(2,1)*jac(3,3)
      inv(2,2) = jac(1,1)*jac(3,3)-jac(1,3)*jac(3,1)
      inv(2,3) = jac(1,3)*jac(2,1)-jac(1,1)*jac(2,3)
      inv(3,1) = jac(2,1)*jac(3,2)-jac(2,2)*jac(3,1)
      inv(3,2) = jac(1,2)*jac(3,1)-jac(1,1)*jac(3,2)
      inv(3,3) = jac(1,1)*jac(2,2)-jac(1,2)*jac(2,1)
      det = jac(1,1)*inv(1,1)+jac(1,2)*inv(2,1)+jac(1,3)*inv(3,1)
      if(abs(det) < tiny(1.0_prec)*1.0e3_prec) return

      delta(1) = (inv(1,1)*res(1)+inv(1,2)*res(2)+inv(1,3)*res(3))/det
      delta(2) = (inv(2,1)*res(1)+inv(2,2)*res(2)+inv(2,3)*res(3))/det
      delta(3) = (inv(3,1)*res(1)+inv(3,2)*res(2)+inv(3,3)*res(3))/det
      xi(1) = xi(1)+delta(1)
      xi(2) = xi(2)+delta(2)
      xi(3) = xi(3)+delta(3)

      if(max(abs(delta(1)),abs(delta(2)),abs(delta(3))) < newtonTolerance) then
        converged = .true.
        return
      endif

      if(abs(xi(1)) > 5.0_prec .or. abs(xi(2)) > 5.0_prec .or. abs(xi(3)) > 5.0_prec) return
    enddo

  endsubroutine NewtonInverse_3D